src/mc/include/mc/can_driver.h

@@ -42,13 +42,25 @@ public:
     // 设置接收回调
     void setReceiveCallback(ReceiveCallback callback, void *userData = nullptr);
 
+    // 设置硬件过滤规则
+    bool setFilter(const std::vector<can_filter> &filters);
+
+    // 追加一个过滤器
+    bool addFilter(const can_filter &filter);
+
+    // 追加一组过滤器
+    bool addFilters(const std::vector<can_filter> &filters);
+
 private:
     void receiveThreadFunc();
+    bool applyFilters(); // 应用当前filters_
+
     int sockfd = -1;
     std::atomic<bool> running{false};
     std::thread receiveThread;
     ReceiveCallback callback;
     void *userData = nullptr;
+    std::vector<can_filter> filters_; // 当前所有过滤器
 };
 
 #endif // CAN_DRIVER_H

src/mc/src/can_driver.cpp

@@ -1,18 +1,15 @@
 #include "mc/can_driver.h"
-#include "mc/controlcan.h"
 #include <cstring>
-#include <fcntl.h>
+#include <stdexcept>
 #include <iostream>
+#include <thread>
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <net/if.h>
 #include <linux/can.h>
 #include <linux/can/raw.h>
-#include <net/if.h>
+#include "mc/controlcan.h"
-#include <stdexcept>
-#include <sys/ioctl.h>
-#include <thread>
-#include <unistd.h>
-#include <mutex>
-
-std::mutex canMutex;
 
 DWORD deviceType = VCI_USBCAN2; // 或 VCI_USBCAN_2E_U 等
 DWORD deviceIndex = 0;          // 通常为 0（第一个设备）
@@ -20,15 +17,17 @@ DWORD channelIndex = 0;         // 通道号：0 表示 CAN1，1 表示 CAN2
 
 CANDriver::CANDriver() = default;
 
-CANDriver::~CANDriver() { close(); }
+CANDriver::~CANDriver()
+{
+    close();
+}
 
 bool CANDriver::open(const std::string &interface)
 {
     if (running)
         return false;
 
-  // 参数可解析 interface 设定 channelIndex，例如 interface="can0" =>
+    // 参数可解析 interface 设定 channelIndex，例如 interface="can0" => channelIndex=0
-  // channelIndex=0
     deviceType = VCI_USBCAN2;
     deviceIndex = 0;
     channelIndex = (interface == "can1") ? 1 : 0;
@@ -47,8 +46,7 @@ bool CANDriver::open(const std::string &interface)
     config.Timing1 = 0x1C;
     config.Mode = 0; // 正常模式
 
-  if (VCI_InitCAN(deviceType, deviceIndex, channelIndex, &config) !=
+    if (VCI_InitCAN(deviceType, deviceIndex, channelIndex, &config) != STATUS_OK)
-      STATUS_OK)
     {
         std::cerr << "VCI_InitCAN failed" << std::endl;
         VCI_CloseDevice(deviceType, deviceIndex);
@@ -62,13 +60,6 @@ bool CANDriver::open(const std::string &interface)
         return false;
     }
 
-  if (VCI_ClearBuffer(deviceType, deviceIndex, channelIndex) != 1)
-  {
-    std::cerr << "VCI_ClearBuffer failed" << std::endl;
-    VCI_CloseDevice(deviceType, deviceIndex);
-    return false;
-  }
-
     running = true;
     receiveThread = std::thread(&CANDriver::receiveThreadFunc, this);
     return true;
@@ -83,9 +74,6 @@ void CANDriver::close()
     if (receiveThread.joinable())
         receiveThread.join();
 
-  // 先清空缓冲区，防止缓冲区残留数据导致重复接收
-  VCI_ClearBuffer(deviceType, deviceIndex, channelIndex);
-
     VCI_ResetCAN(deviceType, deviceIndex, channelIndex);
     VCI_CloseDevice(deviceType, deviceIndex);
 }
@@ -103,7 +91,6 @@ bool CANDriver::sendFrame(const CANFrame &frame)
     obj.DataLen = frame.dlc;
     std::memcpy(obj.Data, frame.data, frame.dlc);
 
-  std::lock_guard<std::mutex> lock(canMutex); // 加锁
     if (VCI_Transmit(deviceType, deviceIndex, channelIndex, &obj, 1) != 1)
     {
         std::cerr << "VCI_Transmit failed" << std::endl;
@@ -119,19 +106,54 @@ void CANDriver::setReceiveCallback(ReceiveCallback callback, void *userData)
     this->userData = userData;
 }
 
-void CANDriver::receiveThreadFunc()
+bool CANDriver::setFilter(const std::vector<can_filter> &filters)
 {
-  constexpr int maxFrames = 2500;
+    if (!running)
-  VCI_CAN_OBJ rec[maxFrames];
+        return false;
 
-  while (running)
+    filters_ = filters;
+    return applyFilters();
+}
+
+bool CANDriver::addFilter(const can_filter &filter)
+{
+    filters_.push_back(filter);
+    return applyFilters();
+}
+
+bool CANDriver::addFilters(const std::vector<can_filter> &filters)
+{
+    filters_.insert(filters_.end(), filters.begin(), filters.end());
+    return applyFilters();
+}
+
+bool CANDriver::applyFilters()
+{
+    if (!running)
+        return false;
+
+    if (filters_.empty())
     {
-    ULONG len = 0;
+        setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, nullptr, 0);
-    {
+        return true;
-      std::lock_guard<std::mutex> lock(canMutex); // 加锁
-      len = VCI_Receive(deviceType, deviceIndex, channelIndex, rec, 100, 10);
     }
 
+    if (setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER,
+                   filters_.data(), filters_.size() * sizeof(can_filter)) < 0)
+    {
+        perror("setsockopt");
+        return false;
+    }
+
+    return true;
+}
+
+void CANDriver::receiveThreadFunc()
+{
+    while (running)
+    {
+        VCI_CAN_OBJ rec[100];
+        ULONG len = VCI_Receive(deviceType, deviceIndex, channelIndex, rec, 100, 10);
         if (len == 0)
         {
             std::this_thread::sleep_for(std::chrono::milliseconds(10));
@@ -145,13 +167,6 @@ void CANDriver::receiveThreadFunc()
             frame.ext = rec[i].ExternFlag != 0;
             frame.rtr = rec[i].RemoteFlag != 0;
             frame.dlc = rec[i].DataLen;
-
-      if (frame.dlc > 8)
-      {
-        std::cerr << "Invalid CAN data length: " << (int)frame.dlc << std::endl;
-        frame.dlc = 8; // 或跳过此帧
-      }
-
             std::memcpy(frame.data, rec[i].Data, frame.dlc);
             callback(frame, userData);
         }

src/mc/src/mc.cpp

@@ -1,12 +1,12 @@
+#include <cstdio>
+#include <iostream>
+#include <iomanip>
 #include "mc/can_driver.h"
 #include "mc/can_struct.h"
 #include "mc/get_config.h"
 #include "rclcpp/rclcpp.hpp"
 #include "sweeper_interfaces/msg/can_frame.hpp"
 #include "sweeper_interfaces/msg/mc_ctrl.hpp"
-#include <cstdio>
-#include <iomanip>
-#include <iostream>
 
 namespace sweeperMsg = sweeper_interfaces::msg;
 
@@ -23,7 +23,7 @@ std::atomic<bool> vcu_awake = false;        // 是否唤醒成功
 rclcpp::Time last_vcu_msg_time;             // 上次收到vcu数据帧的时间
 
 // 全局开关，控制是否打印 CAN 消息到终端
-bool g_can_print_enable = true;
+bool g_can_print_enable = false;
 
 struct ControlCache
 {
@@ -47,8 +47,7 @@ struct ControlCache
       return false;
 
     auto now = std::chrono::steady_clock::now();
-    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(
+    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time);
-        now - last_update_time);
 
     if (duration.count() > 100) // ms超时
     {
@@ -96,27 +95,23 @@ void receiveHandler(const CANFrame &frame, void *userData)
   auto msg = sweeperMsg::CanFrame();
   msg.id = frame.id;
   msg.dlc = frame.dlc;
-  size_t len = frame.dlc <= 8 ? frame.dlc : 8;
+  msg.data.assign(frame.data, frame.data + frame.dlc);
-  msg.data.assign(frame.data, frame.data + len);
-
   publisher->publish(msg);
 
   // 根据开关决定是否打印 CAN 消息
-  if (g_can_print_enable && len > 0)
+  if (g_can_print_enable)
   {
     std::stringstream ss;
     ss << "CAN ID: ";
     if (frame.id > 0x7FF)
       ss << std::hex << std::uppercase << frame.id;
     else
-      ss << std::hex << std::uppercase << std::setw(5) << std::setfill(' ')
+      ss << std::hex << std::uppercase << std::setw(5) << std::setfill(' ') << frame.id;
-         << frame.id;
 
     ss << " Data: ";
     for (int i = 0; i < frame.dlc; ++i)
     {
-      ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i]
+      ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " ";
-         << " ";
     }
 
     RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str());
@@ -167,33 +162,32 @@ void VCUWakeUp()
 void setupTimers(rclcpp::Node::SharedPtr node)
 {
   // VCU 唤醒帧发送定时器，1Hz
-  static rclcpp::TimerBase::SharedPtr timer_wakeup =
+  static rclcpp::TimerBase::SharedPtr timer_wakeup = node->create_wall_timer(
-      node->create_wall_timer(std::chrono::seconds(1), [node]()
+      std::chrono::seconds(1), [node]()
       {
-        if (!vcu_awake.load()) {
+        if (!vcu_awake.load())
-          RCLCPP_INFO(node->get_logger(),
+        {
-                      "[TIMER][VCU] Not awake, sending wake-up frame...");
+          RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
           VCUWakeUp();
         } });
 
   // vcu报文 watchdog 检查，200ms
-  static rclcpp::TimerBase::SharedPtr timer_estop_watchdog =
+  static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer(
-      node->create_wall_timer(std::chrono::milliseconds(200), [node]()
+      std::chrono::milliseconds(200), [node]()
       {
         auto now = node->now();
         auto elapsed = now - last_vcu_msg_time;
 
-        if (vcu_msg_received.load() &&
+        if (vcu_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5))
-            elapsed > rclcpp::Duration::from_seconds(0.5)) {
+        {
-          RCLCPP_WARN(node->get_logger(),
+          RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] message timeout, resetting wake-up state.");
-                      "[TIMER][VCU] message timeout, resetting wake-up state.");
           vcu_msg_received.store(false);
           vcu_awake.store(false);
         } });
 
   // MCU控制，50Hz
-  static rclcpp::TimerBase::SharedPtr timer_mcu =
+  static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer(
-      node->create_wall_timer(std::chrono::milliseconds(20), []()
+      std::chrono::milliseconds(20), []()
       {
         sweeperMsg::McCtrl msg = get_safe_control();
 
@@ -205,8 +199,8 @@ void setupTimers(rclcpp::Node::SharedPtr node)
         canctl.sendFrame(mcu_cmd.toFrame()); });
 
   // EPS 控制，20Hz
-  static rclcpp::TimerBase::SharedPtr timer_eps =
+  static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer(
-      node->create_wall_timer(std::chrono::milliseconds(50), []()
+      std::chrono::milliseconds(50), []()
       {
         sweeperMsg::McCtrl msg = get_safe_control();
 
@@ -218,8 +212,8 @@ void setupTimers(rclcpp::Node::SharedPtr node)
         canctl.sendFrame(eps_cmd.toFrame()); });
 
   // VCU 控制，10Hz
-  static rclcpp::TimerBase::SharedPtr timer_vcu =
+  static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer(
-      node->create_wall_timer(std::chrono::milliseconds(100), []()
+      std::chrono::milliseconds(100), []()
       {
         sweeperMsg::McCtrl msg = get_safe_control();
 
@@ -235,8 +229,9 @@ void setupTimers(rclcpp::Node::SharedPtr node)
         vcu2_cmd.setSpray(msg.spray);
         vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl);
 
-        canctl.sendFrame(vcu1_cmd.toFrame());
+        // canctl.sendFrame(vcu1_cmd.toFrame());
-        canctl.sendFrame(vcu2_cmd.toFrame()); });
+        // canctl.sendFrame(vcu2_cmd.toFrame());
+      });
 }
 
 int main(int argc, char **argv)
@@ -249,8 +244,7 @@ int main(int argc, char **argv)
   RCLCPP_INFO(node->get_logger(), "Starting mc package...");
 
   // 创建一个 Publisher
-  auto can_publisher =
+  auto can_publisher = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
-      node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
 
   // 创建 Subscriber（收控制指令）
   auto subscriber = node->create_subscription<sweeperMsg::McCtrl>(
@@ -266,8 +260,7 @@ int main(int argc, char **argv)
 
   if (!canctl.open(mc_config.can_dev))
   {
-    RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s",
+    RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s", mc_config.can_dev.c_str());
-                 mc_config.can_dev.c_str());
     return -1;
   }
 

src/radio_ctrl/src/radio_ctrl.cpp

@@ -91,15 +91,7 @@ private:
                 {
                     msg.gear = 1; // R挡
                 }
-                else
-                {
-                    msg.gear = 0;
-                    msg.brake = 1;
-                    msg.rpm = 0;
-                }
 
-                if (ch_data[7] != 992)
-                {
                 // 油门 / 刹车逻辑
                 if (ch_data[1] <= speed[1])
                 {
@@ -111,7 +103,6 @@ private:
                     msg.brake = 0;
                     msg.rpm = static_cast<uint8_t>(MCU_RPM_MAX * (ch_data[1] - speed[1]) / (speed[2] - speed[1]));
                 }
-                }
 
                 // 一键清扫
                 if (ch_data[5] == 1792)
@@ -132,7 +123,7 @@ private:
                 }
 
                 // 转向逻辑
-                float target_angle = (static_cast<float>(ch_data[3] - 992)) / 800 * EPS_ANGLE_MAX;
+                float target_angle = (992 - static_cast<float>(ch_data[3])) / 800 * EPS_ANGLE_MAX;
 
                 // 角速度（度/秒）
                 float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T;