Auto commit at 2025-05-29 13:59:40

src/mc/src/mc.cpp

@@ -10,6 +10,9 @@
 
 CANDriver canctl;
 
+// 急停状态量
+std::atomic<bool> estop_active = false;
+
 struct ControlCache
 {
   std::mutex mutex; // 防止多线程冲突
@@ -72,6 +75,7 @@ mc::msg::McCtrl get_safe_control()
   return msg;
 }
 
+// CAN消息接收回调
 void receiveHandler(const CANFrame &frame, void *userData)
 {
   // 获取 ROS 2 Publisher
@@ -103,16 +107,64 @@ void receiveHandler(const CANFrame &frame, void *userData)
   }
   std::cout << " Data: ";
 
+  // 终端打印
   for (int i = 0; i < frame.dlc; ++i)
   {
     std::cout << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " ";
   }
-
   std::cout << std::dec << "\n";
+
+  // 检查是否为急停报文
+  if (frame.id == 0x18FA0121 && frame.dlc >= 3)
+  {
+    uint8_t estop_status = (frame.data[2] & 0x0c) >> 2; // 急停状态位
+
+    // 急停被按下
+    if (estop_status != 0x03)
+    {
+      if (!estop_active.load())
+      {
+        std::cout << "!!! E-STOP TRIGGERED, entering safe mode" << std::endl;
+      }
+
+      estop_active.store(true);
+
+      // 立即更新控制缓存，强制进入安全状态
+      mc::msg::McCtrl safe_msg;
+      safe_msg.mcu_enabled = false;
+      safe_msg.brake = 1;
+      safe_msg.gear = 0;
+      safe_msg.rpm = 0;
+      safe_msg.brake_time_ms = 500;
+      safe_msg.angle = 0;
+      safe_msg.angle_speed = 120;
+      safe_msg.main_brush_lift = false;
+      safe_msg.edge_brush_lift = false;
+      safe_msg.vacuum = false;
+      safe_msg.spray = false;
+      safe_msg.mud_flap = false;
+      safe_msg.dust_shake = false;
+      safe_msg.main_brush_spin = false;
+      safe_msg.edge_brush_spin = false;
+
+      control_cache.update(safe_msg); // 用这个覆盖已有控制指令
+    }
+    else
+    {
+      if (estop_active.load())
+      {
+        std::cout << "E-STOP released, resuming control" << std::endl;
+      }
+      estop_active.store(false);
+    }
+  }
 }
 
 void mcCtrlCallback(const mc::msg::McCtrl::SharedPtr msg)
 {
+  if (estop_active)
+    return;
+
   control_cache.update(*msg);
 }