You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
146 lines
3.5 KiB
C++
146 lines
3.5 KiB
C++
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <sstream>
|
|
#include <unistd.h>
|
|
#include <algorithm>
|
|
#include <fcntl.h>
|
|
#include <errno.h>
|
|
#include <iostream>
|
|
#include <iomanip>
|
|
#include <net/if.h>
|
|
#include <linux/can.h>
|
|
#include <linux/can/raw.h>
|
|
#include <sys/socket.h>
|
|
#include <sys/ioctl.h>
|
|
|
|
#include "CanMsgHandler.hpp"
|
|
|
|
CanMsgHandler::CanMsgHandler() {}
|
|
|
|
CanMsgHandler::~CanMsgHandler()
|
|
{
|
|
ClosePort();
|
|
}
|
|
|
|
// 打开CAN
|
|
bool CanMsgHandler::OpenPort(const std::string& port, const std::string& baudrate)
|
|
{
|
|
ClosePort();
|
|
|
|
// 创建SocketCAN套接字
|
|
fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
|
|
if (fd < 0) {
|
|
perror("Failed to create CAN socket");
|
|
return false;
|
|
}
|
|
|
|
// 获取接口索引
|
|
struct ifreq ifr;
|
|
strncpy(ifr.ifr_name, port.c_str(), IFNAMSIZ - 1);
|
|
ifr.ifr_name[IFNAMSIZ - 1] = '\0';
|
|
|
|
if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
|
|
perror("Failed to get interface index");
|
|
close(fd);
|
|
fd = -1;
|
|
return false;
|
|
}
|
|
|
|
// 绑定套接字到CAN接口
|
|
struct sockaddr_can addr;
|
|
memset(&addr, 0, sizeof(addr));
|
|
addr.can_family = AF_CAN;
|
|
addr.can_ifindex = ifr.ifr_ifindex;
|
|
|
|
if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
|
perror("Failed to bind CAN socket");
|
|
close(fd);
|
|
fd = -1;
|
|
return false;
|
|
}
|
|
|
|
// // 设置非阻塞模式
|
|
// int flags = fcntl(fd, F_GETFL, 0);
|
|
// fcntl(fd, F_SETFL, flags | O_NONBLOCK);
|
|
|
|
// 设置CAN波特率
|
|
// std::string cmd = "ip link set " + port + " down";
|
|
// std::cout << cmd << std::endl;
|
|
// if (system(cmd.c_str()) != 0) {
|
|
// return false;
|
|
// }
|
|
|
|
// cmd = "ip link set " + port + " type can bitrate " + baudrate + " dbitrate 1000000 fd on";
|
|
// std::cout << cmd << std::endl;
|
|
// if (system(cmd.c_str()) != 0) {
|
|
// return false;
|
|
// }
|
|
|
|
// cmd = "ip link set " + port + " up";
|
|
// std::cout << cmd << std::endl;
|
|
// if (system(cmd.c_str()) != 0) {
|
|
// return false;
|
|
// }
|
|
|
|
return true;
|
|
}
|
|
|
|
// 发送数据
|
|
int CanMsgHandler::SendDeviceMsg(std::vector<uint8_t> &data)
|
|
{
|
|
struct can_frame frame;
|
|
|
|
frame.can_id = data[0];
|
|
frame.can_id = frame.can_id << 12;
|
|
frame.can_id += data[1];
|
|
frame.can_id = frame.can_id << 8;
|
|
frame.can_id += data[2];
|
|
frame.can_id |= CAN_EFF_FLAG;
|
|
|
|
frame.can_dlc = data.size() - 3;
|
|
|
|
for (int i = 0; i < frame.can_dlc; i++)
|
|
{
|
|
frame.data[i] = data[i + 3];
|
|
}
|
|
for (int num : data) {
|
|
std::cout << std::hex << std::setw(2) << std::setfill('0')
|
|
<< num << " ";
|
|
}
|
|
std::cout << std::endl;
|
|
|
|
int bytesWritten = SendMsg(&frame, sizeof(struct can_frame));
|
|
if (bytesWritten < 0) {
|
|
perror("发送数据失败");
|
|
}
|
|
|
|
return bytesWritten;
|
|
}
|
|
|
|
int CanMsgHandler::RecvDeviceMsg(std::vector<uint8_t>& data, int timeoutMs)
|
|
{
|
|
int bytesRead;
|
|
struct can_frame frame;
|
|
|
|
bytesRead = RecvMsg(&frame, sizeof(struct can_frame), timeoutMs);
|
|
|
|
if (bytesRead > 0)
|
|
{
|
|
data.insert(data.end(), (frame.can_id >> 20) & 0xFF);
|
|
data.insert(data.end(), (frame.can_id >> 8) & 0xFF);
|
|
data.insert(data.end(), frame.can_id & 0xFF);
|
|
data.insert(data.end(), frame.data, frame.data + frame.can_dlc);
|
|
}
|
|
|
|
return bytesRead;
|
|
}
|
|
|
|
uint32_t CanMsgHandler::GetCanId(uint32_t DeviceID, uint32_t CommandID, uint32_t BlockID)
|
|
{
|
|
uint32_t Canid = DeviceID << 20 + CommandID << 8 + BlockID;
|
|
|
|
return Canid;
|
|
}
|
|
|