Auto commit at 2025-06-12 14:41:07

src/mc/CMakeLists.txt

@@ -5,6 +5,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+link_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib)
+
 # Find dependencies
 find_package(sweeper_interfaces REQUIRED)
 find_package(ament_cmake REQUIRED)
@@ -19,6 +21,8 @@ add_executable(mc_node ${SRC_FILES})
 
 ament_target_dependencies(mc_node rclcpp std_msgs sweeper_interfaces)
 
+target_link_libraries(mc_node controlcan pthread)
+
 # Set include directories for the target
 target_include_directories(
   mc_node
@@ -31,6 +35,15 @@ target_compile_features(mc_node PUBLIC c_std_99 cxx_std_17)
 install(TARGETS mc_node DESTINATION lib/${PROJECT_NAME})
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 
+set(LIB_DIR ${CMAKE_CURRENT_SOURCE_DIR}/lib)
+
+# 安装libcontrolcan动态库（.so）
+install(
+  FILES
+    ${LIB_DIR}/libcontrolcan.so
+  DESTINATION lib
+)
+
 # Handle testing if needed
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

src/mc/include/mc/controlcan.h

@@ -0,0 +1,104 @@
+#ifndef CONTROLCAN_H
+#define CONTROLCAN_H
+
+////文件版本：v2.02 20190609
+//接口卡类型定义
+
+#define VCI_USBCAN1		3
+#define VCI_USBCAN2		4
+#define VCI_USBCAN2A		4
+
+#define VCI_USBCAN_E_U 		20
+#define VCI_USBCAN_2E_U 	21
+
+//函数调用返回状态值
+#define	STATUS_OK					1
+#define STATUS_ERR					0
+	
+#define USHORT unsigned short int
+#define BYTE unsigned char
+#define CHAR char
+#define UCHAR unsigned char
+#define UINT unsigned int
+#define DWORD unsigned int
+#define PVOID void*
+#define ULONG unsigned int
+#define INT int
+#define UINT32 UINT
+#define LPVOID void*
+#define BOOL BYTE
+#define TRUE 1
+#define FALSE 0
+
+//1.ZLGCAN系列接口卡信息的数据类型。
+typedef  struct  _VCI_BOARD_INFO{
+		USHORT	hw_Version;
+		USHORT	fw_Version;
+		USHORT	dr_Version;
+		USHORT	in_Version;
+		USHORT	irq_Num;
+		BYTE	can_Num;
+		CHAR	str_Serial_Num[20];
+		CHAR	str_hw_Type[40];
+		USHORT	Reserved[4];
+} VCI_BOARD_INFO,*PVCI_BOARD_INFO; 
+
+//2.定义CAN信息帧的数据类型。
+typedef  struct  _VCI_CAN_OBJ{
+	UINT	ID;
+	UINT	TimeStamp;
+	BYTE	TimeFlag;
+	BYTE	SendType;
+	BYTE	RemoteFlag;//是否是远程帧
+	BYTE	ExternFlag;//是否是扩展帧
+	BYTE	DataLen;
+	BYTE	Data[8];
+	BYTE	Reserved[3];
+}VCI_CAN_OBJ,*PVCI_CAN_OBJ;
+
+//3.定义初始化CAN的数据类型
+typedef struct _INIT_CONFIG{
+	DWORD	AccCode;
+	DWORD	AccMask;
+	DWORD	Reserved;
+	UCHAR	Filter;
+	UCHAR	Timing0;	
+	UCHAR	Timing1;	
+	UCHAR	Mode;
+}VCI_INIT_CONFIG,*PVCI_INIT_CONFIG;
+
+///////// new add struct for filter /////////
+typedef struct _VCI_FILTER_RECORD{
+	DWORD ExtFrame;	//是否为扩展帧
+	DWORD Start;
+	DWORD End;
+}VCI_FILTER_RECORD,*PVCI_FILTER_RECORD;
+
+#ifdef __cplusplus
+#define EXTERN_C  extern "C"
+#else
+#define EXTERN_C
+#endif
+
+EXTERN_C DWORD VCI_OpenDevice(DWORD DeviceType,DWORD DeviceInd,DWORD Reserved);
+EXTERN_C DWORD VCI_CloseDevice(DWORD DeviceType,DWORD DeviceInd);
+EXTERN_C DWORD VCI_InitCAN(DWORD DeviceType, DWORD DeviceInd, DWORD CANInd, PVCI_INIT_CONFIG pInitConfig);
+
+EXTERN_C DWORD VCI_ReadBoardInfo(DWORD DeviceType,DWORD DeviceInd,PVCI_BOARD_INFO pInfo);
+
+EXTERN_C DWORD VCI_SetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
+
+EXTERN_C ULONG VCI_GetReceiveNum(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+EXTERN_C DWORD VCI_ClearBuffer(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+
+EXTERN_C DWORD VCI_StartCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+EXTERN_C DWORD VCI_ResetCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+
+EXTERN_C ULONG VCI_Transmit(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pSend,UINT Len);
+EXTERN_C ULONG VCI_Receive(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pReceive,UINT Len,INT WaitTime);
+
+EXTERN_C DWORD  VCI_UsbDeviceReset(DWORD DevType,DWORD DevIndex,DWORD Reserved);
+EXTERN_C DWORD  VCI_FindUsbDevice2(PVCI_BOARD_INFO pInfo);
+
+#endif
+

src/mc/src/can_driver.cpp

@@ -9,6 +9,11 @@
 #include <net/if.h>
 #include <linux/can.h>
 #include <linux/can/raw.h>
+#include "mc/controlcan.h"
+
+DWORD deviceType = VCI_USBCAN2; // 或 VCI_USBCAN_2E_U 等
+DWORD deviceIndex = 0;          // 通常为 0（第一个设备）
+DWORD channelIndex = 0;         // 通道号：0 表示 CAN1，1 表示 CAN2
 
 CANDriver::CANDriver() = default;
 
@@ -22,39 +27,39 @@ bool CANDriver::open(const std::string &interface)
     if (running)
         return false;
 
-    sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+    // 参数可解析 interface 设定 channelIndex，例如 interface="can0" => channelIndex=0
-    if (sockfd < 0)
+    deviceType = VCI_USBCAN2;
+    deviceIndex = 0;
+    channelIndex = (interface == "can1") ? 1 : 0;
+
+    if (VCI_OpenDevice(deviceType, deviceIndex, 0) != STATUS_OK)
     {
-        perror("socket");
+        std::cerr << "VCI_OpenDevice failed" << std::endl;
         return false;
     }
 
-    struct ifreq ifr;
+    VCI_INIT_CONFIG config{};
-    std::strncpy(ifr.ifr_name, interface.c_str(), IFNAMSIZ - 1);
+    config.AccCode = 0;
-    ifr.ifr_name[IFNAMSIZ - 1] = '\0';
+    config.AccMask = 0xFFFFFFFF;
+    config.Filter = 0;     // 接收所有
+    config.Timing0 = 0x00; // 500Kbps 示例（需查文档确认 Timing）
+    config.Timing1 = 0x1C;
+    config.Mode = 0; // 正常模式
 
-    if (ioctl(sockfd, SIOCGIFINDEX, &ifr) < 0)
+    if (VCI_InitCAN(deviceType, deviceIndex, channelIndex, &config) != STATUS_OK)
     {
-        perror("ioctl");
+        std::cerr << "VCI_InitCAN failed" << std::endl;
-        ::close(sockfd);
+        VCI_CloseDevice(deviceType, deviceIndex);
         return false;
     }
 
-    struct sockaddr_can addr{};
+    if (VCI_StartCAN(deviceType, deviceIndex, channelIndex) != STATUS_OK)
-    addr.can_family = AF_CAN;
-    addr.can_ifindex = ifr.ifr_ifindex;
-
-    if (bind(sockfd, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr)) < 0)
     {
-        perror("bind");
+        std::cerr << "VCI_StartCAN failed" << std::endl;
-        ::close(sockfd);
+        VCI_CloseDevice(deviceType, deviceIndex);
         return false;
     }
 
-    // 设置为非阻塞
-    int flags = fcntl(sockfd, F_GETFL, 0);
-    fcntl(sockfd, F_SETFL, flags | O_NONBLOCK);
-
     running = true;
     receiveThread = std::thread(&CANDriver::receiveThreadFunc, this);
     return true;
@@ -69,11 +74,8 @@ void CANDriver::close()
     if (receiveThread.joinable())
         receiveThread.join();
 
-    if (sockfd >= 0)
+    VCI_ResetCAN(deviceType, deviceIndex, channelIndex);
-    {
+    VCI_CloseDevice(deviceType, deviceIndex);
-        ::close(sockfd);
-        sockfd = -1;
-    }
 }
 
 bool CANDriver::sendFrame(const CANFrame &frame)
@@ -81,18 +83,17 @@ bool CANDriver::sendFrame(const CANFrame &frame)
     if (!running)
         return false;
 
-    struct can_frame raw_frame{};
+    VCI_CAN_OBJ obj{};
-    raw_frame.can_id = frame.id;
+    obj.ID = frame.id;
-    if (frame.ext)
+    obj.RemoteFlag = frame.rtr ? 1 : 0;
-        raw_frame.can_id |= CAN_EFF_FLAG;
+    obj.ExternFlag = frame.ext ? 1 : 0;
-    if (frame.rtr)
+    obj.SendType = 0; // 普通发送
-        raw_frame.can_id |= CAN_RTR_FLAG;
+    obj.DataLen = frame.dlc;
-    raw_frame.can_dlc = frame.dlc;
+    std::memcpy(obj.Data, frame.data, frame.dlc);
-    std::memcpy(raw_frame.data, frame.data, frame.dlc);
 
-    if (write(sockfd, &raw_frame, sizeof(raw_frame)) != sizeof(raw_frame))
+    if (VCI_Transmit(deviceType, deviceIndex, channelIndex, &obj, 1) != 1)
     {
-        perror("write");
+        std::cerr << "VCI_Transmit failed" << std::endl;
         return false;
     }
 
@@ -149,25 +150,24 @@ bool CANDriver::applyFilters()
 
 void CANDriver::receiveThreadFunc()
 {
-    struct can_frame raw_frame;
     while (running)
     {
-        ssize_t nbytes = read(sockfd, &raw_frame, sizeof(raw_frame));
+        VCI_CAN_OBJ rec[100];
-        if (nbytes < 0)
+        ULONG len = VCI_Receive(deviceType, deviceIndex, channelIndex, rec, 100, 10);
+        if (len == 0)
         {
-            if (errno != EAGAIN)
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
-                perror("read");
             continue;
         }
 
-        if (nbytes == sizeof(raw_frame) && callback)
+        for (ULONG i = 0; i < len && callback; ++i)
         {
             CANFrame frame;
-            frame.id = raw_frame.can_id & CAN_EFF_MASK;
+            frame.id = rec[i].ID;
-            frame.ext = static_cast<bool>(raw_frame.can_id & CAN_EFF_FLAG);
+            frame.ext = rec[i].ExternFlag != 0;
-            frame.rtr = static_cast<bool>(raw_frame.can_id & CAN_RTR_FLAG);
+            frame.rtr = rec[i].RemoteFlag != 0;
-            frame.dlc = raw_frame.can_dlc;
+            frame.dlc = rec[i].DataLen;
-            std::memcpy(frame.data, raw_frame.data, raw_frame.can_dlc);
+            std::memcpy(frame.data, rec[i].Data, frame.dlc);
             callback(frame, userData);
         }
     }

src/radio_ctrl/src/radio_ctrl.cpp

@@ -8,7 +8,8 @@
 
 namespace sweeperMsg = sweeper_interfaces::msg;
 
-constexpr float EPS_GEAR_RATIO = 16.5f;
+constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f;
+constexpr float GEAR_RATIO = 7.0f;
 constexpr float DELTA_T = 0.02f; // 20ms
 const uint16_t speed[3] = {192, 992, 1792};
 
@@ -128,7 +129,7 @@ private:
                 float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T;
 
                 // 电机转速（单位：rpm）
-                float motor_rpm = angle_speed * EPS_GEAR_RATIO;
+                float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO;
 
                 // 限制电机转速到 [120, 1500] 范围，防止过小/过大
                 uint16_t can_speed = std::clamp(