Auto commit at 2025-06-18 16:59:19

src/mc/include/mc/timer_tasks.hpp

@@ -3,4 +3,5 @@
 #include "mc/can_driver.h"
 #include "mc/dumper.h"
 
+// 注册所有定时器任务
 void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriver &canctl);

src/mc/src/timer_tasks.cpp

@@ -7,20 +7,19 @@ extern std::atomic<bool> vcu_awake;
 extern std::atomic<bool> vcu_msg_received;
 extern rclcpp::Time last_vcu_msg_time;
 
-void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriver &canctl)
+// 定时器回调：VCU 唤醒
+void wakeupTimerTask(const rclcpp::Node::SharedPtr &node, CANDriver &canctl)
 {
-    static auto timer_wakeup = node->create_wall_timer(
-        std::chrono::seconds(1), [node, &canctl]()
-        {
   if (!vcu_awake.load())
   {
     RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
     VCUWakeUp(canctl);
-        } });
+  }
+}
 
-    static auto timer_watchdog = node->create_wall_timer(
+// 定时器回调：VCU watchdog + Dumper Dial处理
-        std::chrono::milliseconds(200), [node, &dumper]()
+void vcuWatchdogTask(const rclcpp::Node::SharedPtr &node, DumperController &dumper)
-        {
+{
   auto now = node->now();
   if (vcu_msg_received.load() && (now - last_vcu_msg_time > rclcpp::Duration::from_seconds(0.5)))
   {
@@ -35,40 +34,45 @@ void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriv
     dumper.tryStartup();
     auto msg = get_safe_control();
     dumper.updateDial(static_cast<int>(msg.dump), dumper.getSimpleState());
-        } });
+  }
+}
 
-    static auto timer_dbs = node->create_wall_timer(
+// 定时器回调：DBS 控制
-        std::chrono::milliseconds(10), [&canctl]()
+void dbsTimerTask(CANDriver &canctl)
-        {
+{
   auto msg = get_safe_control();
   dbs_cmd.setEnabled(msg.ehb_anable);
   dbs_cmd.setPressure(msg.ehb_brake_pressure);
-        canctl.sendFrame(dbs_cmd.toFrame()); });
+  canctl.sendFrame(dbs_cmd.toFrame());
+}
 
-    static auto timer_mcu = node->create_wall_timer(
+// 定时器回调：MCU 控制
-        std::chrono::milliseconds(20), [&canctl]()
+void mcuTimerTask(CANDriver &canctl)
-        {
+{
   auto msg = get_safe_control();
   mcu_cmd.setEnabled(true);
   mcu_cmd.setGear(msg.gear);
   mcu_cmd.setRPM(msg.rpm);
   mcu_cmd.setBrake(msg.brake);
-        canctl.sendFrame(mcu_cmd.toFrame()); });
+  canctl.sendFrame(mcu_cmd.toFrame());
+}
 
-    static auto timer_eps = node->create_wall_timer(
+// 定时器回调：EPS 控制
-        std::chrono::milliseconds(50), [&canctl]()
+void epsTimerTask(CANDriver &canctl)
-        {
+{
   auto msg = get_safe_control();
   eps_cmd.setCenterCmd(0);
   eps_cmd.setAngle(msg.angle);
   eps_cmd.setAngularSpeed(msg.angle_speed);
   eps_cmd.pack();
-        canctl.sendFrame(eps_cmd.toFrame()); });
+  canctl.sendFrame(eps_cmd.toFrame());
+}
 
-    static auto timer_vcu = node->create_wall_timer(
+// 定时器回调：VCU 灯控、扫地、除尘等功能
-        std::chrono::milliseconds(100), [&canctl, &dumper]()
+void vcuTimerTask(CANDriver &canctl, DumperController &dumper)
-        {
+{
   auto msg = get_safe_control();
+
   vcu1_cmd.setLeftLight(msg.left_light);
   vcu1_cmd.setDustShake(msg.dust_shake);
   vcu1_cmd.setHeadLight(msg.headlight);
@@ -81,9 +85,44 @@ void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriv
   vcu2_cmd.setSpray(msg.spray);
   vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl);
 
+  // 只有在扫地中并且斗处于收斗状态时才发VCU指令
   if (msg.sweep_ctrl && dumper.getSimpleState() == 0)
   {
     canctl.sendFrame(vcu1_cmd.toFrame());
     canctl.sendFrame(vcu2_cmd.toFrame());
-        } });
+  }
+}
+
+// 注册所有定时器任务
+void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriver &canctl)
+{
+  static auto timer_wakeup = node->create_wall_timer(
+      std::chrono::seconds(1),
+      [node, &canctl]()
+      { wakeupTimerTask(node, canctl); });
+
+  static auto timer_watchdog = node->create_wall_timer(
+      std::chrono::milliseconds(200),
+      [node, &dumper]()
+      { vcuWatchdogTask(node, dumper); });
+
+  static auto timer_dbs = node->create_wall_timer(
+      std::chrono::milliseconds(10),
+      [&canctl]()
+      { dbsTimerTask(canctl); });
+
+  static auto timer_mcu = node->create_wall_timer(
+      std::chrono::milliseconds(20),
+      [&canctl]()
+      { mcuTimerTask(canctl); });
+
+  static auto timer_eps = node->create_wall_timer(
+      std::chrono::milliseconds(50),
+      [&canctl]()
+      { epsTimerTask(canctl); });
+
+  static auto timer_vcu = node->create_wall_timer(
+      std::chrono::milliseconds(100),
+      [&canctl, &dumper]()
+      { vcuTimerTask(canctl, dumper); });
 }