From bffaa1f39676427c2520baee53c92b6711f87833 Mon Sep 17 00:00:00 2001 From: cxh Date: Wed, 18 Jun 2025 16:59:19 +0800 Subject: [PATCH] Auto commit at 2025-06-18 16:59:19 --- src/mc/include/mc/timer_tasks.hpp | 1 + src/mc/src/timer_tasks.cpp | 185 ++++++++++++++++++------------ 2 files changed, 113 insertions(+), 73 deletions(-) diff --git a/src/mc/include/mc/timer_tasks.hpp b/src/mc/include/mc/timer_tasks.hpp index 2ffee92..c37327f 100644 --- a/src/mc/include/mc/timer_tasks.hpp +++ b/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); diff --git a/src/mc/src/timer_tasks.cpp b/src/mc/src/timer_tasks.cpp index 690e350..b00aa85 100644 --- a/src/mc/src/timer_tasks.cpp +++ b/src/mc/src/timer_tasks.cpp @@ -7,83 +7,122 @@ extern std::atomic vcu_awake; extern std::atomic vcu_msg_received; extern rclcpp::Time last_vcu_msg_time; +// 定时器回调：VCU 唤醒 +void wakeupTimerTask(const rclcpp::Node::SharedPtr &node, CANDriver &canctl) +{ + if (!vcu_awake.load()) + { + RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame..."); + VCUWakeUp(canctl); + } +} + +// 定时器回调：VCU watchdog + Dumper Dial处理 +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))) + { + RCLCPP_WARN(node->get_logger(), "[VCU] Timeout, resetting state."); + vcu_awake.store(false); + vcu_msg_received.store(false); + dumper.resetAwake(); + } + + if (vcu_awake.load()) + { + dumper.tryStartup(); + auto msg = get_safe_control(); + dumper.updateDial(static_cast(msg.dump), dumper.getSimpleState()); + } +} + +// 定时器回调：DBS 控制 +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()); +} + +// 定时器回调：MCU 控制 +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()); +} + +// 定时器回调：EPS 控制 +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()); +} + +// 定时器回调：VCU 灯控、扫地、除尘等功能 +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); + + vcu2_cmd.setBrakeLight(msg.brake_light); + vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift); + vcu2_cmd.setHeadlight(msg.headlight); + vcu2_cmd.setMudFlap(msg.mud_flap); + vcu2_cmd.setRightLight(msg.right_light); + 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]() - { - if (!vcu_awake.load()) - { - RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame..."); - VCUWakeUp(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]() - { - auto now = node->now(); - if (vcu_msg_received.load() && (now - last_vcu_msg_time > rclcpp::Duration::from_seconds(0.5))) - { - RCLCPP_WARN(node->get_logger(), "[VCU] Timeout, resetting state."); - vcu_awake.store(false); - vcu_msg_received.store(false); - dumper.resetAwake(); - } + static auto timer_watchdog = node->create_wall_timer( + std::chrono::milliseconds(200), + [node, &dumper]() + { vcuWatchdogTask(node, dumper); }); - if (vcu_awake.load()) - { - dumper.tryStartup(); - auto msg = get_safe_control(); - dumper.updateDial(static_cast(msg.dump), dumper.getSimpleState()); - } }); + static auto timer_dbs = node->create_wall_timer( + std::chrono::milliseconds(10), + [&canctl]() + { dbsTimerTask(canctl); }); - static auto timer_dbs = node->create_wall_timer( - std::chrono::milliseconds(10), [&canctl]() - { - auto msg = get_safe_control(); - dbs_cmd.setEnabled(msg.ehb_anable); - dbs_cmd.setPressure(msg.ehb_brake_pressure); - canctl.sendFrame(dbs_cmd.toFrame()); }); + static auto timer_mcu = node->create_wall_timer( + std::chrono::milliseconds(20), + [&canctl]() + { mcuTimerTask(canctl); }); - static auto timer_mcu = node->create_wall_timer( - std::chrono::milliseconds(20), [&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()); }); + static auto timer_eps = node->create_wall_timer( + std::chrono::milliseconds(50), + [&canctl]() + { epsTimerTask(canctl); }); - static auto timer_eps = node->create_wall_timer( - std::chrono::milliseconds(50), [&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()); }); - - static auto timer_vcu = node->create_wall_timer( - std::chrono::milliseconds(100), [&canctl, &dumper]() - { - auto msg = get_safe_control(); - vcu1_cmd.setLeftLight(msg.left_light); - vcu1_cmd.setDustShake(msg.dust_shake); - vcu1_cmd.setHeadLight(msg.headlight); - - vcu2_cmd.setBrakeLight(msg.brake_light); - vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift); - vcu2_cmd.setHeadlight(msg.headlight); - vcu2_cmd.setMudFlap(msg.mud_flap); - vcu2_cmd.setRightLight(msg.right_light); - vcu2_cmd.setSpray(msg.spray); - vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl); - - if (msg.sweep_ctrl && dumper.getSimpleState() == 0) - { - canctl.sendFrame(vcu1_cmd.toFrame()); - canctl.sendFrame(vcu2_cmd.toFrame()); - } }); -} + static auto timer_vcu = node->create_wall_timer( + std::chrono::milliseconds(100), + [&canctl, &dumper]() + { vcuTimerTask(canctl, dumper); }); +} \ No newline at end of file