#include <cstdint>
#include <thread>
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <pthread.h>
#include <iostream>
#include <cstring>
#include <netinet/in.h>
#include <limits>
#include <linux/can.h>
#include <net/if.h>
#include <linux/sockios.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fstream>
#include <sstream>
#include <linux/can/raw.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <fcntl.h>
// CRC校验函数(这里使用简单的CRC16校验)
uint16_t crc16(const uint8_t* data, size_t length) {
static const uint16_t polynomial = 0xA001;
uint16_t crc = 0xFFFF;
for (size_t pos = 0; pos < length; ++pos) {
crc ^= (uint16_t)data[pos];
for (uint8_t i = 8; i != 0; --i) {
if (crc & 0x0001) {
crc = (crc >> 1) ^ polynomial;
}
else {
crc >>= 1;
}
}
}
std::printf("%02X ", crc);
return crc;
}
void sendCanFrame(int canSock, int ifaceNum, uint32_t canID, uint8_t* data, size_t size)
{
struct can_frame frame;
frame.can_id = canID;
frame.can_dlc = size;
memcpy(frame.data, data, size);
struct sockaddr_can addr;
memset(&addr, 0, sizeof(addr));
addr.can_family = AF_CAN;
addr.can_ifindex = ifaceNum;
ssize_t nbytes = write(canSock, &frame, sizeof(frame));
if (nbytes < 0) {
perror("Failed to write CAN frame");
}
}
// 假设的setmotor函数定义
bool setmotor(uint32_t id, uint32_t speed) {
int canSock0 = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (canSock0 < 0) {
perror("Failed to open CAN socket");
return 1;
}
struct ifreq ifr0;
strcpy(ifr0.ifr_name, "can0");
if (ioctl(canSock0, SIOCGIFINDEX, &ifr0) < 0) {
perror("Failed to get7 CAN interface index");
return 1;
}
// 将CAN0接口绑定到套接字
struct sockaddr_can addr0;
memset(&addr0, 0, sizeof(addr0));
addr0.can_family = AF_CAN;
addr0.can_ifindex = ifr0.ifr_ifindex;
if (bind(canSock0, (struct sockaddr*)&addr0, sizeof(addr0)) < 0) {
perror("Failed to bind CAN socket");
return 1;
}
uint8_t data1[2] = { 0x1,static_cast<uint8_t>(id) };
sendCanFrame(canSock0, ifr0.ifr_ifindex, 0x0, data1, sizeof(data1));
uint8_t data2[8] = { 0x2F, 0x60, 0x60, 0x00, 0x03, 0x00, 0x00, 0x00 };
sendCanFrame(canSock0, ifr0.ifr_ifindex, (0x600 + id), data2, sizeof(data2));
uint8_t data3[8] = { 0x23, 0xFF, 0x60, 0X00, static_cast<uint8_t>(speed & 0xff), static_cast<uint8_t>((speed >> 8) & 0xff), 0x00, 0x00 };
sendCanFrame(canSock0, ifr0.ifr_ifindex, (0x600 + id), data3, sizeof(data3));
uint8_t data4[8] = { 0x23, 0x83, 0x60, 0X00, 0x88, 0x13, 0x00, 0x00 };
sendCanFrame(canSock0, ifr0.ifr_ifindex, (0x600 + id), data4, sizeof(data4));
uint8_t data5[8] = { 0x23, 0x84, 0x60, 0X00, 0x88, 0x13, 0x00, 0x00 };
sendCanFrame(canSock0, ifr0.ifr_ifindex, (0x600 + id), data5, sizeof(data5));
uint8_t data6[8] = { 0x2B, 0x40, 0x60, 0X00, 0x06, 0x00, 0x00, 0x00 };
sendCanFrame(canSock0, ifr0.ifr_ifindex, (0x600 + id), data6, sizeof(data6));
uint8_t data7[8] = { 0x2B, 0x40, 0x60, 0X00, 0x07, 0x00, 0x00, 0x00 };
sendCanFrame(canSock0, ifr0.ifr_ifindex, (0x600 + id), data7, sizeof(data7));
uint8_t data8[8] = { 0x2B, 0x40, 0x60, 0X00, 0x0F, 0x00, 0x00, 0x00 };
sendCanFrame(canSock0, ifr0.ifr_ifindex, (0x600 + id), data8, sizeof(data8));
sleep(1);
if (speed <= 200 && speed > 24000) {
uint8_t data7[8] = { 0x2B, 0x40, 0x60, 0X00, 0x07, 0x00, 0x00, 0x00 };
sendCanFrame(canSock0, ifr0.ifr_ifindex, (0x600 + id), data7, sizeof(data7));
}
else if (id == 1)
{
if (speed <= 200 && speed > 1500)
{
uint8_t data7[8] = { 0x2B, 0x40, 0x60, 0X00, 0x07, 0x00, 0x00, 0x00 };
sendCanFrame(canSock0, ifr0.ifr_ifindex, (0x600 + id), data7, sizeof(data7));
}
}
return true;
}
const char* SERVER_IP = "192.168.1.104";
const int SERVER_PORT = 8009;
const int BUFFER_SIZE = 1024;
// 客户端连接结构
struct ClientConnection {
int socket_fd;
struct sockaddr_in client_addr;
socklen_t client_addr_size;
};
// 接收数据的线程函数
void* receive_thread(void* arg) {
ClientConnection* conn = static_cast<ClientConnection*>(arg);
uint8_t buffer[BUFFER_SIZE];
ssize_t bytes_read;
while (true) {
bytes_read = recv(conn->socket_fd, buffer, BUFFER_SIZE, 0);
if (bytes_read <= 0) {
std::cerr << "Client disconnected" << std::endl;
break;
}
// 确保接收到足够的数据
if (bytes_read < 8) {
std::cerr << "Invalid data length received from client" << std::endl;
continue;
}
uint16_t received_crc = (buffer[7] << 8) | buffer[6];
uint16_t calculated_crc = crc16(buffer, 6);
std::printf("%02X\n ", received_crc);
std::printf("%02X \n", calculated_crc);
if (calculated_crc == received_crc)
{
// 提取最后两个字节并转换为10进制
uint16_t speed = buffer[5] | (buffer[4] << 8);
// 检查第二个字节的值
if (buffer[1] == 0x06 && buffer[3] == 0xF3 && buffer[2] == 03) {
if (setmotor(4, speed)) { // 假设电机ID为0,根据实际需要修改
std::string response = "id :4 MOTOR_SET_SUCCESS Speed: " + std::to_string(speed);
send(conn->socket_fd, response.c_str(), response.length() + 1, 0);
}
else {
send(conn->socket_fd, "MOTOR_SET_FAILURE", 17, 0);
}
}
else {
// 只返回speed值
std::string response = "motor :4 Speed: " + std::to_string(speed);
send(conn->socket_fd, response.c_str(), response.length() + 1, 0);
}
}
}
close(conn->socket_fd);
delete conn;
return nullptr;
}
// 接受客户端连接的线程函数
void* accept_thread(void* arg) {
int server_fd = *(static_cast<int*>(arg));
struct sockaddr_in client_addr;
socklen_t client_addr_size = sizeof(client_addr);
while (true) {
int client_fd = accept(server_fd, (struct sockaddr*)&client_addr, &client_addr_size);
if (client_fd == -1) {
std::cerr << "Error accepting connection" << std::endl;
continue;
}
std::cout << "Client connected" << std::endl;
// 创建ClientConnection对象并传递给接收线程
ClientConnection* conn = new ClientConnection{ client_fd, client_addr, client_addr_size };
pthread_t receive_thread_id;
if (pthread_create(&receive_thread_id, nullptr, receive_thread, conn)) {
std::cerr << "Error creating receive thread" << std::endl;
close(client_fd);
delete conn;
}
else {
// 分离线程,以便在结束时自动回收资源
pthread_detach(receive_thread_id);
}
}
return nullptr;
}
int main() {
int server_fd;
struct sockaddr_in server_addr;
// 创建套接字
server_fd = socket(AF_INET, SOCK_STREAM, 0);
if (server_fd == -1) {
std::cerr << "Error creating socket" << std::endl;
return 1;
}
// 设置服务器地址
memset(&server_addr, 0, sizeof(server_addr));
server_addr.sin_family = AF_INET;
server_addr.sin_port = htons(SERVER_PORT);
server_addr.sin_addr.s_addr = inet_addr(SERVER_IP);
// 绑定套接字到地址
if (bind(server_fd, (struct sockaddr*)&server_addr, sizeof(server_addr)) == -1) {
std::cerr << "Error binding socket" << std::endl;
close(server_fd);
return 1;
}
// 监听连接
if (listen(server_fd, 5) == -1) {
std::cerr << "Error listening on socket" << std::endl;
close(server_fd);
return 1;
}
std::cout << "Server listening on " << SERVER_IP << ":" << SERVER_PORT << std::endl;
// 创建接受连接的线程
pthread_t accept_thread_id;
if (pthread_create(&accept_thread_id, nullptr, accept_thread, &server_fd)) {
std::cerr << "Error creating accept thread" << std::endl;
close(server_fd);
return 1;
}
// 分离线程,以便在结束时自动回收资源
pthread_detach(accept_thread_id);
// 主线程可以继续执行其他任务,或者等待直到服务器被强制关闭
// 这里我们简单地让主线程休眠,防止它立即退出
while (true) {
sleep(1);
}
return 0;
}
协议已经验证过正确。