Auto commit at 2025-05-27 10:40:10

src/mc/src/mc.cpp

@@ -14,12 +14,14 @@ struct ControlCache
 {
   std::mutex mutex; // 防止多线程冲突
   mc::msg::McCtrl latest_msg;
+  std::chrono::steady_clock::time_point last_update_time;
   bool has_data = false;
 
   void update(const mc::msg::McCtrl &msg)
   {
     std::lock_guard<std::mutex> lock(mutex);
     latest_msg = msg;
+    last_update_time = std::chrono::steady_clock::now();
     has_data = true;
   }
 
@@ -28,6 +30,15 @@ struct ControlCache
     std::lock_guard<std::mutex> lock(mutex);
     if (!has_data)
       return false;
+
+    auto now = std::chrono::steady_clock::now();
+    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time);
+
+    if (duration.count() > 100) // ms超时
+    {
+      return false; // 数据过期
+    }
+
     msg = latest_msg;
     return true;
   }
@@ -35,6 +46,32 @@ struct ControlCache
 
 ControlCache control_cache;
 
+// 发布检测
+mc::msg::McCtrl get_safe_control()
+{
+  mc::msg::McCtrl msg;
+  if (!control_cache.get(msg))
+  {
+    // 超时或未接收到控制数据，进入安全状态
+    msg.mcu_enabled = false;
+    msg.brake = 1;
+    msg.gear = 0;
+    msg.rpm = 0;
+    msg.brake_time_ms = 500;
+    msg.angle = 0;
+    msg.angle_speed = 120;
+    msg.main_brush_lift = false;
+    msg.edge_brush_lift = false;
+    msg.vacuum = false;
+    msg.spray = false;
+    msg.mud_flap = false;
+    msg.dust_shake = false;
+    msg.main_brush_spin = false;
+    msg.edge_brush_spin = false;
+  }
+  return msg;
+}
+
 void receiveHandler(const CANFrame &frame, void *userData)
 {
   // 获取 ROS 2 Publisher
@@ -85,8 +122,7 @@ void setupTimers(rclcpp::Node::SharedPtr node)
   static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer(
       std::chrono::milliseconds(20), []()
       {
-        mc::msg::McCtrl msg;
-        if (!control_cache.get(msg)) return;
+        mc::msg::McCtrl msg = get_safe_control();
 
         mcu_cmd.setEnabled(msg.mcu_enabled);
         mcu_cmd.setGear(msg.gear);
@@ -95,18 +131,13 @@ void setupTimers(rclcpp::Node::SharedPtr node)
         mcu_cmd.setBrake(msg.brake);
         mcu_cmd.pack();
 
-        
-
-        canctl.sendFrame(mcu_cmd.toFrame());
-         
-        });
+        canctl.sendFrame(mcu_cmd.toFrame()); });
 
   // EPS 控制，20Hz
   static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer(
       std::chrono::milliseconds(50), []()
       {
-        mc::msg::McCtrl msg;
-        if (!control_cache.get(msg)) return;
+        mc::msg::McCtrl msg = get_safe_control();
 
         eps_cmd.setCenterCmd(0);
         eps_cmd.setAngle(static_cast<int16_t>(msg.angle * 5));
@@ -114,15 +145,13 @@ void setupTimers(rclcpp::Node::SharedPtr node)
         eps_cmd.setControlMode(0x20);
         eps_cmd.pack();
 
-         canctl.sendFrame(eps_cmd.toFrame()); 
-        });
+        canctl.sendFrame(eps_cmd.toFrame()); });
 
   // VCU2 控制，10Hz
   static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer(
       std::chrono::milliseconds(100), []()
       {
-        mc::msg::McCtrl msg;
-        if (!control_cache.get(msg)) return;
+        mc::msg::McCtrl msg = get_safe_control();
 
         vcu1_cmd.setLeftLight(msg.left_light);
         vcu1_cmd.setNightLightPercent(msg.night_light);
@@ -137,17 +166,14 @@ void setupTimers(rclcpp::Node::SharedPtr node)
         vcu2_cmd.setSpray(msg.spray);
         vcu2_cmd.setVacuum(msg.vacuum);
 
-        //canctl.sendFrame(vcu1_cmd.toFrame());
-
-       canctl.sendFrame(vcu2_cmd.toFrame()); 
-       });
+        canctl.sendFrame(vcu1_cmd.toFrame());
+        canctl.sendFrame(vcu2_cmd.toFrame()); });
 
   // 刷子控制，3.3Hz
   static rclcpp::TimerBase::SharedPtr timer_brush = node->create_wall_timer(
       std::chrono::milliseconds(300), []()
       {
-        mc::msg::McCtrl msg;
-        if (!control_cache.get(msg)) return;
+        mc::msg::McCtrl msg = get_safe_control();
 
         main_brush_cmd.setBrushEnable(msg.main_brush_spin);
         main_brush_cmd.setBrushPwm(msg.main_brush_pwm);
@@ -156,8 +182,7 @@ void setupTimers(rclcpp::Node::SharedPtr node)
         edge_brush_cmd.setBrushPwm(msg.edge_brush_pwm);
 
         canctl.sendFrame(main_brush_cmd.toFrame());
-        canctl.sendFrame(edge_brush_cmd.toFrame()); 
-        });
+        canctl.sendFrame(edge_brush_cmd.toFrame()); });
 }
 
 void VCUWakeUp()

src/radio_ctrl/src/radio_ctrl.cpp

@@ -71,9 +71,9 @@ private:
             for (int i = 0; i < 8; ++i)
             {
                 ch_data[i] = uart_handler_->get_channel_value(i);
-                printf("ch[%d]:%d   ", i, ch_data[i]);
+                // printf("ch[%d]:%d   ", i, ch_data[i]);
             }
-            printf("\n");
+            // printf("\n");
 
             if (ch_data[6] == 192) // 是否使能车辆控制
             {

src/radio_ctrl/src/uart_handler.cpp

@@ -30,7 +30,7 @@ struct termios2
 
 // 构造函数
 UartHandler::UartHandler(const std::string &device, int baudrate)
-    : serial_device(device), baudrate(baudrate), fd(-1), failsafe_status(SBUS_SIGNAL_OK)
+    : serial_device(device), baudrate(baudrate), fd(-1), failsafe_status(SBUS_SIGNAL_LOST)
 {
     std::fill(std::begin(sbus_channels), std::end(sbus_channels), 0); // 初始化通道数据
 }
@@ -162,11 +162,11 @@ void UartHandler::parse_data(uint8_t *buf, int len)
 
         if (status == SBUS_SIGNAL_LOST)
         {
-            std::cout << "[Warning] SBUS Signal Lost!\n";
+            // std::cout << "[Warning] SBUS Signal Lost!\n";
         }
         else if (status == SBUS_SIGNAL_FAILSAFE)
         {
-            std::cout << "[Warning] SBUS Failsafe!\n";
+            // std::cout << "[Warning] SBUS Failsafe!\n";
         }
     }
     else