Auto commit at 2025-06-19 14:55:46

src/ctrl_arbiter/src/ctrl_arbiter.cpp

@@ -48,6 +48,7 @@ public:
 
         timer_ = this->create_wall_timer(20ms, [this]()
                                          { this->arbitrateAndPublish(); });
+        RCLCPP_INFO(this->get_logger(), "ArbitrationNode started, waiting for control sources...");
     }
 
 private:
@@ -60,16 +61,19 @@ private:
         if (radio_valid_ && (now - radio_last_time_).nanoseconds() < timeout_ms_ * 1000000)
         {
             publisher_->publish(radio_msg_);
+            RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using RADIO control");
             return;
         }
         if (remote_valid_ && (now - remote_last_time_).nanoseconds() < timeout_ms_ * 1000000)
         {
             publisher_->publish(remote_msg_);
+            RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using REMOTE control");
             return;
         }
         if (auto_valid_ && (now - auto_last_time_).nanoseconds() < timeout_ms_ * 1000000)
         {
             publisher_->publish(auto_msg_);
+            RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "[ARBITER] Using AUTO control");
             return;
         }
 
@@ -87,6 +91,8 @@ private:
         safe_msg.dust_shake = false;
 
         publisher_->publish(safe_msg);
+        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
+                             "[ARBITER] All sources timeout, publishing FAILSAFE control");
     }
 
     rclcpp::Publisher<sweeperMsg::McCtrl>::SharedPtr publisher_;

src/mc/include/mc/can_struct.h

@@ -57,6 +57,19 @@ union can_EPS_cmd_union
 
 #pragma pack(pop)
 
+union can_DBS_cmd_union
+{
+    struct __attribute__((packed))
+    {
+        uint8_t valid;       // Byte 0: VCU_DBS_Valid
+        uint8_t work_mode;   // Byte 1: VCU_DBS_Work_Mode
+        uint8_t pressure;    // Byte 2: VCU_DBS_HP_Pressure（0.1MPa）
+        uint8_t abs_active;  // Byte 3: ABS_Active
+        uint8_t reserved[4]; // Byte 4~7: 保留
+    } fields;
+    uint8_t bytes[8]; // 原始字节流
+};
+
 struct can_MCU_cmd
 {
     can_MCU_cmd_union data;
@@ -326,9 +339,52 @@ struct can_VCU_out2_cmd
     }
 };
 
+struct can_DBS_cmd
+{
+    can_DBS_cmd_union data;
+
+    static constexpr uint32_t CMD_ID = 0x154;
+    static constexpr bool EXT_FLAG = false;
+    static constexpr bool RTR_FLAG = false;
+
+    // 构造函数初始化固定字段
+    can_DBS_cmd()
+    {
+        memset(&data, 0, sizeof(data));
+        data.fields.work_mode = 0;
+        data.fields.abs_active = 1;
+    }
+
+    void setEnabled(bool en)
+    {
+        data.fields.work_mode = en ? 1 : 0;
+    }
+
+    // 设置压力（单位 MPa，0~8.0，有效范围 0~80）
+    can_DBS_cmd &setPressure(float mpa)
+    {
+        uint8_t val = static_cast<uint8_t>(std::clamp(mpa * 10.0f, 0.0f, 80.0f));
+        data.fields.pressure = val;
+        return *this;
+    }
+
+    // 转换为 CAN 帧
+    CANFrame toFrame() const
+    {
+        CANFrame frame;
+        frame.id = CMD_ID;
+        memcpy(frame.data, data.bytes, 8);
+        frame.dlc = 8;
+        frame.ext = EXT_FLAG;
+        frame.rtr = RTR_FLAG;
+        return frame;
+    }
+};
+
 extern can_MCU_cmd mcu_cmd;
 extern can_EPS_cmd eps_cmd;
 extern can_VCU_out1_cmd vcu1_cmd;
 extern can_VCU_out2_cmd vcu2_cmd;
+extern can_DBS_cmd dbs_cmd;
 
 #endif

src/mc/include/mc/can_utils.hpp

@@ -0,0 +1,14 @@
+#pragma once
+#include "rclcpp/rclcpp.hpp"
+#include "sweeper_interfaces/msg/can_frame.hpp"
+#include "mc/can_struct.h"
+
+extern bool g_can_print_enable;
+
+struct CanHandlerContext
+{
+    rclcpp::Node::SharedPtr node;
+    std::shared_ptr<rclcpp::Publisher<sweeper_interfaces::msg::CanFrame>> publisher;
+};
+
+void receiveHandler(const CANFrame &frame, void *userData);

src/mc/include/mc/control_cache.hpp

@@ -0,0 +1,20 @@
+#pragma once
+#include "sweeper_interfaces/msg/mc_ctrl.hpp"
+#include <mutex>
+#include <chrono>
+
+class ControlCache
+{
+public:
+    void update(const sweeper_interfaces::msg::McCtrl &msg);
+    bool get(sweeper_interfaces::msg::McCtrl &msg);
+
+private:
+    std::mutex mutex_;
+    sweeper_interfaces::msg::McCtrl latest_msg_;
+    std::chrono::steady_clock::time_point last_update_time_;
+    bool has_data_ = false;
+};
+
+extern ControlCache control_cache;
+sweeper_interfaces::msg::McCtrl get_safe_control(); // 获取带超时判断的控制指令

src/mc/include/mc/dumper.h

@@ -0,0 +1,63 @@
+// 专门用于垃圾倾倒的头文件
+#ifndef DUMPER_H
+#define DUMPER_H
+
+#include "can_driver.h"
+#include <mutex>
+#include <queue>
+#include <rclcpp/rclcpp.hpp>
+
+enum class DumperState
+{
+    IDLE,
+    RAISING,    // 斗升中
+    DUMPING,    // 放斗中
+    RETRACTING, // 收斗中
+    LOWERING    // 斗降中
+};
+
+enum class DumperAction
+{
+    RAISE,
+    DUMP,
+    RETRACT,
+    LOWER
+};
+
+class DumperController
+{
+public:
+    DumperController(CANDriver &driver);
+
+    void onStartup();                                         // 上电复位（只在内部用）
+    void tryStartup();                                        // 唤醒后自动执行复位（防重）
+    void updateDial(int new_dial, int current_state);         // 拨钮变化检测与处理
+    void resetAwake();                                        // 清除唤醒状态（VCU 掉线）
+    void setSweeping(bool sweeping) { sweeping_ = sweeping; } // 外部设置扫地状态，
+    int getSimpleState() const;
+
+private:
+    void handleDialChange(int from, int to);
+    void runActionSequence(const std::vector<DumperAction> &actions);
+    void doAction(DumperAction act);
+    void sendControlOff();
+    void logState() const;
+
+    bool raised_ = true;
+    bool dumped_ = true;
+    bool in_startup_reset_ = true;
+
+    bool awakened_ = false;
+    int last_dial_target_ = -1;
+
+    std::mutex queue_mutex_;
+    std::queue<DumperAction> action_queue_;
+    std::atomic<bool> busy_;
+    std::atomic<bool> has_pending_task_;
+    std::atomic<bool> sweeping_ = false; // 扫地任务标志
+    DumperState current_state_;
+    CANDriver &can_;
+    rclcpp::Logger logger_;
+};
+
+#endif

src/mc/include/mc/timer_tasks.hpp

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

src/mc/include/mc/vcu_wakeup.hpp

@@ -0,0 +1,4 @@
+#pragma once
+#include "mc/can_struct.h"
+
+void VCUWakeUp(CANDriver &driver); // 发送唤醒帧

src/mc/src/can_struct.cpp

@@ -4,3 +4,4 @@ can_MCU_cmd mcu_cmd;
 can_EPS_cmd eps_cmd;
 can_VCU_out1_cmd vcu1_cmd;
 can_VCU_out2_cmd vcu2_cmd;
+can_DBS_cmd dbs_cmd;

src/mc/src/can_utils.cpp

@@ -0,0 +1,39 @@
+#include "mc/can_utils.hpp"
+#include <sstream>
+#include <iomanip>
+
+bool g_can_print_enable = false;
+extern std::atomic<bool> vcu_msg_received;
+extern std::atomic<bool> vcu_awake;
+extern rclcpp::Time last_vcu_msg_time;
+
+void receiveHandler(const CANFrame &frame, void *userData)
+{
+    auto *context = static_cast<CanHandlerContext *>(userData);
+    auto node = context->node;
+    auto pub = context->publisher;
+    auto now = node->now();
+
+    sweeper_interfaces::msg::CanFrame msg;
+    msg.id = frame.id;
+    msg.dlc = frame.dlc;
+    msg.data.assign(frame.data, frame.data + frame.dlc);
+    pub->publish(msg);
+
+    if (g_can_print_enable)
+    {
+        std::stringstream ss;
+        ss << "CAN ID: " << std::hex << std::uppercase
+           << std::setw((frame.id > 0x7FF) ? 8 : 5) << std::setfill(' ') << frame.id << " Data: ";
+        for (int i = 0; i < frame.dlc; ++i)
+            ss << std::setw(2) << std::setfill('0') << std::hex << (int)frame.data[i] << " ";
+        RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str());
+    }
+
+    if (frame.id == 0x18FA0121)
+    {
+        last_vcu_msg_time = now;
+        vcu_msg_received.store(true);
+        vcu_awake.store(true);
+    }
+}

src/mc/src/control_cache.cpp

@@ -0,0 +1,44 @@
+#include "mc/control_cache.hpp"
+
+ControlCache control_cache;
+
+void ControlCache::update(const sweeper_interfaces::msg::McCtrl &msg)
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    latest_msg_ = msg;
+    last_update_time_ = std::chrono::steady_clock::now();
+    has_data_ = true;
+}
+
+bool ControlCache::get(sweeper_interfaces::msg::McCtrl &msg)
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!has_data_)
+        return false;
+
+    auto now = std::chrono::steady_clock::now();
+    if (std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time_).count() > 100)
+        return false;
+
+    msg = latest_msg_;
+    return true;
+}
+
+sweeper_interfaces::msg::McCtrl get_safe_control()
+{
+    sweeper_interfaces::msg::McCtrl msg;
+    if (!control_cache.get(msg))
+    {
+        msg.brake = 1;
+        msg.gear = 0;
+        msg.rpm = 0;
+        msg.angle = 0;
+        msg.angle_speed = 120;
+        msg.edge_brush_lift = false;
+        msg.sweep_ctrl = false;
+        msg.spray = false;
+        msg.mud_flap = false;
+        msg.dust_shake = false;
+    }
+    return msg;
+}

src/mc/src/dumper.cpp

@@ -0,0 +1,218 @@
+#include "mc/dumper.h"
+#include <thread>
+#include <chrono>
+#include <iostream>
+
+DumperController::DumperController(CANDriver &driver)
+    : busy_(false),
+      has_pending_task_(false),
+      current_state_(DumperState::IDLE),
+      can_(driver),
+      logger_(rclcpp::get_logger("DumperController"))
+{
+}
+
+// 上电复位：自动收斗 + 降斗
+void DumperController::onStartup()
+{
+    RCLCPP_INFO(logger_, "[Init] Power-on reset: retract and lower...");
+    doAction(DumperAction::RETRACT);
+    doAction(DumperAction::LOWER);
+    in_startup_reset_ = false;
+    sendControlOff();
+}
+
+void DumperController::tryStartup()
+{
+    if (awakened_)
+        return;
+
+    RCLCPP_INFO(logger_, "[Init] Dumper first wake-up: resetting...");
+    onStartup();
+    awakened_ = true;
+}
+
+void DumperController::resetAwake()
+{
+    awakened_ = false;
+    last_dial_target_ = -1;
+    RCLCPP_INFO(logger_, "[INFO] Dumper awake state reset.");
+}
+
+void DumperController::updateDial(int new_dial, int current_state)
+{
+    if (in_startup_reset_)
+    {
+        RCLCPP_WARN(logger_, "[Dumper] In startup reset, ignoring remote control dump command.");
+        return;
+    }
+
+    if (new_dial < 0 || new_dial > 2)
+        return;
+
+    if (last_dial_target_ == new_dial)
+        return;
+
+    RCLCPP_INFO(logger_, "[Dial] Changed from %d to %d", current_state, new_dial);
+    handleDialChange(current_state, new_dial);
+    last_dial_target_ = new_dial;
+}
+
+// 拨钮变更处理（如从最下拨到最上，动作会排队）
+void DumperController::handleDialChange(int from, int to)
+{
+    std::vector<DumperAction> actions;
+
+    if (from == 0 && to == 1)
+        actions = {DumperAction::RAISE};
+    else if (from == 1 && to == 2)
+        actions = {DumperAction::DUMP};
+    else if (from == 2 && to == 1)
+        actions = {DumperAction::RETRACT};
+    else if (from == 1 && to == 0)
+        actions = {DumperAction::LOWER};
+    else if (from == 0 && to == 2)
+        actions = {DumperAction::RAISE, DumperAction::DUMP};
+    else if (from == 2 && to == 0)
+        actions = {DumperAction::RETRACT, DumperAction::LOWER};
+    else
+        return;
+
+    runActionSequence(actions);
+}
+
+// 执行动作序列，如果当前正在执行，则加入队列等待
+void DumperController::runActionSequence(const std::vector<DumperAction> &actions)
+{
+    if (busy_)
+    {
+        std::lock_guard<std::mutex> lock(queue_mutex_);
+        for (const auto &a : actions)
+            action_queue_.push(a);
+        has_pending_task_ = true;
+        return;
+    }
+
+    busy_ = true;
+    has_pending_task_ = false;
+
+    std::thread([this, actions]
+                {
+                    for (auto action : actions)
+                    {
+                        doAction(action);
+                    }
+
+                    // 执行排队的动作
+                    while (true)
+                    {
+                        DumperAction next;
+                        {
+                            std::lock_guard<std::mutex> lock(queue_mutex_);
+                            if (action_queue_.empty())
+                                break;
+                            next = action_queue_.front();
+                            action_queue_.pop();
+                        }
+                        doAction(next);
+                    }
+
+                    sendControlOff();
+                    busy_ = false; })
+        .detach();
+}
+
+// 每个动作的具体 CAN 指令 + 延时
+void DumperController::doAction(DumperAction act)
+{
+    if (sweeping_)
+    {
+        RCLCPP_WARN(logger_, "[Warning] Sweeping active, dumper action blocked.");
+        return;
+    }
+
+    constexpr int delay_ms = 15000; // 可统一调整延时时间（单位：毫秒）
+    using namespace std::chrono_literals;
+
+    CANFrame frame1;
+    CANFrame frame2;
+
+    switch (act)
+    {
+    case DumperAction::RAISE:
+        if (raised_)
+            return;
+        RCLCPP_INFO(logger_, "[Action] Raising...");
+        frame1 = {0x18FA1117, {0x00, 0x40, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
+        can_.sendFrame(frame1);
+        current_state_ = DumperState::RAISING;
+        std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+        raised_ = true;
+        dumped_ = false;
+        break;
+
+    case DumperAction::DUMP:
+        if (dumped_)
+            return;
+        RCLCPP_INFO(logger_, "[Action] Dumping...");
+        frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x10, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
+        can_.sendFrame(frame1);
+        current_state_ = DumperState::DUMPING;
+        std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+        dumped_ = true;
+        break;
+
+    case DumperAction::RETRACT:
+        if (!dumped_)
+            return;
+        RCLCPP_INFO(logger_, "[Action] Retracting...");
+        frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
+        frame2 = {0x18F21117, {0x00, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 8, true, false};
+        can_.sendFrame(frame1);
+        can_.sendFrame(frame2);
+        current_state_ = DumperState::RETRACTING;
+        std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+        dumped_ = false;
+        break;
+
+    case DumperAction::LOWER:
+        if (!raised_)
+            return;
+        RCLCPP_INFO(logger_, "[Action] Lowering...");
+        frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
+        frame2 = {0x18F21117, {0x00, 0x00, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00}, 8, true, false};
+        can_.sendFrame(frame1);
+        can_.sendFrame(frame2);
+        current_state_ = DumperState::LOWERING;
+        std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+        raised_ = false;
+        break;
+    }
+    logState();
+}
+
+// 所有动作完成后关闭控制
+void DumperController::sendControlOff()
+{
+    CANFrame off1 = {0x18FA1117, {0, 0, 0, 0, 0, 0, 0, 0}, 8, true, false};
+    CANFrame off2 = {0x18F21117, {0, 0, 0, 0, 0, 0, 0, 0}, 8, true, false};
+    can_.sendFrame(off1);
+    can_.sendFrame(off2);
+    RCLCPP_INFO(logger_, "[INFO] Control OFF sent.");
+}
+
+void DumperController::logState() const
+{
+    const char *state_str[] = {"IDLE", "RAISING", "DUMPING", "RETRACTING", "LOWERING"};
+    RCLCPP_INFO(logger_, "[STATE] Dumper state: %s", state_str[static_cast<int>(current_state_)]);
+}
+
+int DumperController::getSimpleState() const
+{
+    if (!raised_)
+        return 0; // 斗未升（已收）
+    else if (!dumped_)
+        return 1; // 斗升但未放斗
+    else
+        return 2; // 斗升且已放斗
+}

src/mc/src/mc.cpp

@@ -1,258 +1,36 @@
-#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 "mc/get_config.h"
+#include "mc/can_driver.h"
+#include "mc/dumper.h"
+#include "mc/can_utils.hpp"
+#include "mc/control_cache.hpp"
+#include "mc/timer_tasks.hpp"
+#include "mc/vcu_wakeup.hpp"
 #include "sweeper_interfaces/msg/can_frame.hpp"
 #include "sweeper_interfaces/msg/mc_ctrl.hpp"
 
 namespace sweeperMsg = sweeper_interfaces::msg;
 
 CANDriver canctl;
-
+DumperController dumper(canctl);
-struct CanHandlerContext
+std::atomic<bool> vcu_msg_received = false;
-{
+std::atomic<bool> vcu_awake = false;
-  rclcpp::Node::SharedPtr node;
+rclcpp::Time last_vcu_msg_time;
-  std::shared_ptr<rclcpp::Publisher<sweeperMsg::CanFrame>> publisher;
-};
-
-std::atomic<bool> vcu_msg_received = false; // 是否收到过vcu数据帧
-std::atomic<bool> vcu_awake = false;        // 是否唤醒成功
-rclcpp::Time last_vcu_msg_time;             // 上次收到vcu数据帧的时间
-
-// 全局开关，控制是否打印 CAN 消息到终端
-bool g_can_print_enable = false;
-
-struct ControlCache
-{
-  std::mutex mutex; // 防止多线程冲突
-  sweeperMsg::McCtrl latest_msg;
-  std::chrono::steady_clock::time_point last_update_time;
-  bool has_data = false;
-
-  void update(const sweeperMsg::McCtrl &msg)
-  {
-    std::lock_guard<std::mutex> lock(mutex);
-    latest_msg = msg;
-    last_update_time = std::chrono::steady_clock::now();
-    has_data = true;
-  }
-
-  bool get(sweeperMsg::McCtrl &msg)
-  {
-    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;
-  }
-};
-
-ControlCache control_cache;
-
-// 发布检测
-sweeperMsg::McCtrl get_safe_control()
-{
-  sweeperMsg::McCtrl msg;
-  if (!control_cache.get(msg))
-  {
-    // 超时或未接收到控制数据，进入安全状态
-    msg.brake = 1;
-    msg.gear = 0;
-    msg.rpm = 0;
-    msg.angle = 0;
-    msg.angle_speed = 120;
-    msg.edge_brush_lift = false;
-    msg.sweep_ctrl = false;
-    msg.spray = false;
-    msg.mud_flap = false;
-    msg.dust_shake = false;
-  }
-  return msg;
-}
-
-// CAN消息接收回调
-void receiveHandler(const CANFrame &frame, void *userData)
-{
-  // 获取 ROS 2 Publisher
-  auto context = static_cast<CanHandlerContext *>(userData);
-  auto publisher = context->publisher;
-  auto node = context->node;
-  auto now = node->now();
-
-  // 创建并发布 CAN 消息
-  auto msg = sweeperMsg::CanFrame();
-  msg.id = frame.id;
-  msg.dlc = frame.dlc;
-  msg.data.assign(frame.data, frame.data + frame.dlc);
-  publisher->publish(msg);
-
-  // 根据开关决定是否打印 CAN 消息
-  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(' ') << 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] << " ";
-    }
-
-    RCLCPP_INFO(node->get_logger(), "%s", ss.str().c_str());
-  }
-
-  // VCU报文检查
-  if (frame.id == 0x18FA0121)
-  {
-    last_vcu_msg_time = now; // 更新时间
-    vcu_msg_received.store(true);
-    vcu_awake.store(true); // 唤醒成功
-  }
-}
 
 void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg)
 {
+  dumper.setSweeping(msg->sweep_ctrl);
   control_cache.update(*msg);
 }
 
-void VCUWakeUp()
-{
-  CANFrame frame;
-
-  // 公共设置
-  frame.id = 0x18FF8017;
-  frame.dlc = 8;
-  frame.ext = true;  // 使用扩展帧
-  frame.rtr = false; // 不是远程帧
-
-  // 第1帧：握手帧
-  frame.data[0] = 0x55;
-  frame.data[1] = 0x21;
-  for (int i = 2; i < 8; ++i)
-    frame.data[i] = 0xFF;
-  canctl.sendFrame(frame);
-
-  // 延时，例如 50ms
-  std::this_thread::sleep_for(std::chrono::milliseconds(50));
-
-  // 第2帧：启动帧
-  frame.data[0] = 0xAA;
-  frame.data[1] = 0x21;
-  for (int i = 2; i < 8; ++i)
-    frame.data[i] = 0xFF;
-  canctl.sendFrame(frame);
-}
-
-void setupTimers(rclcpp::Node::SharedPtr node)
-{
-  // VCU 唤醒帧发送定时器，1Hz
-  static rclcpp::TimerBase::SharedPtr timer_wakeup = node->create_wall_timer(
-      std::chrono::seconds(1), [node]()
-      {
-        if (!vcu_awake.load())
-        {
-          RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
-          VCUWakeUp();
-        } });
-
-  // vcu报文 watchdog 检查，200ms
-  static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer(
-      std::chrono::milliseconds(200), [node]()
-      {
-        auto now = node->now();
-        auto elapsed = now - last_vcu_msg_time;
-
-        if (vcu_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5))
-        {
-          RCLCPP_WARN(node->get_logger(), "[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 = node->create_wall_timer(
-      std::chrono::milliseconds(20), []()
-      {
-        sweeperMsg::McCtrl 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 控制，20Hz
-  static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer(
-      std::chrono::milliseconds(50), []()
-      {
-        sweeperMsg::McCtrl 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 控制，10Hz
-  static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer(
-      std::chrono::milliseconds(100), []()
-      {
-        sweeperMsg::McCtrl 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);
-
-        // canctl.sendFrame(vcu1_cmd.toFrame());
-        // canctl.sendFrame(vcu2_cmd.toFrame());
-      });
-}
-
 int main(int argc, char **argv)
 {
-  rclcpp::init(argc, argv); // 初始化 ROS 2
+  rclcpp::init(argc, argv);
-
-  // 创建一个 ROS 2 节点
   auto node = rclcpp::Node::make_shared("can_driver_node");
-
   RCLCPP_INFO(node->get_logger(), "Starting mc package...");
 
-  // 创建一个 Publisher
+  auto pub = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
-  auto can_publisher = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
+  auto sub = node->create_subscription<sweeperMsg::McCtrl>("mc_ctrl", 10, mcCtrlCallback);
-
-  // 创建 Subscriber（收控制指令）
-  auto subscriber = node->create_subscription<sweeperMsg::McCtrl>(
-      "mc_ctrl",     // 订阅的 topic 名字
-      10,            // 队列长度
-      mcCtrlCallback // 收到消息后的回调
-  );
-
   last_vcu_msg_time = rclcpp::Time(0, 0, node->get_clock()->get_clock_type());
 
   Config mc_config;
@@ -264,24 +42,16 @@ int main(int argc, char **argv)
     return -1;
   }
 
-  // 创建上下文
   auto context = std::make_shared<CanHandlerContext>();
   context->node = node;
-  context->publisher = can_publisher;
+  context->publisher = pub;
+  canctl.setReceiveCallback(receiveHandler, context.get());
 
-  // 设置接收回调并传递上下文
+  setupTimers(node, dumper, canctl);
-  canctl.setReceiveCallback(receiveHandler, (void *)context.get());
 
-  // 添加定时器设置
-  setupTimers(node);
-
-  // 在 ROS shutdown 时自动清理
   rclcpp::on_shutdown([&]()
                       { canctl.close(); });
-
-  // 事件循环
   rclcpp::spin(node);
-
+  rclcpp::shutdown();
-  rclcpp::shutdown(); // 关闭 ROS 2
   return 0;
 }

src/mc/src/timer_tasks.cpp

@@ -0,0 +1,135 @@
+#include "mc/timer_tasks.hpp"
+#include "mc/control_cache.hpp"
+#include "mc/vcu_wakeup.hpp"
+#include "mc/can_struct.h"
+
+extern std::atomic<bool> vcu_awake;
+extern std::atomic<bool> 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<int>(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)
+{
+  static bool last_sweep_ctrl = false; // 上一次扫地状态（用于比较是否发生变化）
+
+  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);
+
+  bool sweep_changed = (msg.sweep_ctrl != last_sweep_ctrl);
+  bool can_start_sweep = (msg.sweep_ctrl && dumper.getSimpleState() == 0);
+
+  if (sweep_changed || can_start_sweep)
+  {
+    // 状态变更（开或关） 或者 满足开始扫地条件，发出指令
+    canctl.sendFrame(vcu1_cmd.toFrame());
+    canctl.sendFrame(vcu2_cmd.toFrame());
+
+    last_sweep_ctrl = msg.sweep_ctrl; // 更新上一次状态
+  }
+}
+
+// 注册所有定时器任务
+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); });
+}

src/mc/src/vcu_wakeup.cpp

@@ -0,0 +1,21 @@
+#include "mc/vcu_wakeup.hpp"
+#include <thread>
+
+void VCUWakeUp(CANDriver &driver)
+{
+    CANFrame frame;
+    frame.id = 0x18FF8017;
+    frame.dlc = 8;
+    frame.ext = true;
+    frame.rtr = false;
+
+    frame.data[0] = 0x55;
+    frame.data[1] = 0x21;
+    std::fill(frame.data + 2, frame.data + 8, 0xFF);
+    driver.sendFrame(frame);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+    frame.data[0] = 0xAA;
+    driver.sendFrame(frame);
+}

src/radio_ctrl/src/radio_ctrl.cpp

@@ -8,9 +8,9 @@
 
 namespace sweeperMsg = sweeper_interfaces::msg;
 
-constexpr float EPS_GEAR_RATIO = 16.5f;
+constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f;
+constexpr float GEAR_RATIO = 7.0f;
 constexpr float DELTA_T = 0.02f; // 20ms
-const uint16_t speed[3] = {192, 992, 1792};
 
 class SBUSNode : public rclcpp::Node
 {
@@ -62,11 +62,12 @@ private:
     {
         static int MCU_RPM_MAX = config.mcu_rpm_max;
         static float EPS_ANGLE_MAX = config.eps_angle_max;
+        static bool initialized = false; // 新增初始化标志
 
         bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性
 
         auto msg = sweeperMsg::McCtrl(); // 控制消息对象
-        uint16_t ch_data[8];             // 各通道遥控数据
+        uint16_t ch_data[10];            // 各通道遥控数据
 
         if (data_safe) // 数据安全，进行数据解析并发布
         {
@@ -78,33 +79,89 @@ private:
             }
             // printf("\n");
 
-            if (ch_data[6] == 192) // 是否使能车辆控制
+            uint16_t ctrl = ch_data[4];       // 手刹
+            uint16_t gear = ch_data[5];       // 挡位
+            uint16_t sweep = ch_data[6];      // 清扫
+            uint16_t dump = ch_data[7];       // 垃圾倾倒
+            int16_t speed = ch_data[1] - 992; //[-800,800]
+
+            if (!initialized)
+            {
+                if (sweep == 1792 && dump == 1792)
+                {
+                    initialized = true;
+                    RCLCPP_INFO(this->get_logger(), "[RADIO_CTRL] Initialize Success.");
+                }
+                else
+                {
+                    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 3000,
+                                         "[RADIO_CTRL] Please set sweep and dump to bottom to initialize.");
+                    return;
+                }
+            }
+
+            switch (dump)
+            {
+            case 1792:
+                msg.dump = 0;
+                break;
+            case 992:
+                msg.dump = 1;
+                break;
+            case 192:
+                msg.dump = 2;
+                break;
+            default:
+                break;
+            }
+
+            static uint16_t last_ctrl = 992; // 初始手刹值
+            static int current_brake = 1;    // 默认上电是拉手刹（1）
+
+            // 判断是否是上拨触发（192）
+            if (last_ctrl == 992 && ctrl == 192)
+            {
+                // 上拨 → 松手刹
+                current_brake = 0;
+            }
+            // 判断是否是下拨触发（1792）
+            else if (last_ctrl == 992 && ctrl == 1792)
+            {
+                // 下拨 → 拉手刹
+                current_brake = 1;
+            }
+
+            last_ctrl = ctrl;
+            msg.brake = current_brake;
+
+            if (msg.brake == 0) // 手刹释放
             {
-                msg.brake = 0;
                 // 挡位选择
-                if (ch_data[7] == 192)
+                if (gear == 192)
                 {
                     msg.gear = 0; // D挡
                 }
-                else if (ch_data[7] == 1792)
+                else if (gear == 1792)
                 {
                     msg.gear = 1; // R挡
                 }
 
                 // 油门 / 刹车逻辑
-                if (ch_data[1] <= speed[1])
+                if (speed <= 0)
                 {
-                    msg.brake = 1;
+                    msg.ehb_anable = true;
+                    msg.ehb_brake_pressure = -0.01f * std::clamp(static_cast<int>(speed), -800, 0);
                     msg.rpm = 0;
                 }
                 else
                 {
-                    msg.brake = 0;
+                    msg.ehb_anable = false;
-                    msg.rpm = static_cast<uint8_t>(MCU_RPM_MAX * (ch_data[1] - speed[1]) / (speed[2] - speed[1]));
+                    msg.ehb_brake_pressure = 0;
+                    msg.rpm = static_cast<uint8_t>(std::clamp(MCU_RPM_MAX * speed / 800, 0, MCU_RPM_MAX));
                 }
 
                 // 一键清扫
-                if (ch_data[5] == 1792)
+                if (sweep == 1792)
                 {
                     msg.edge_brush_lift = false;
                     msg.sweep_ctrl = false;
@@ -112,23 +169,23 @@ private:
                     msg.mud_flap = false;
                     msg.dust_shake = false;
                 }
-                else if (ch_data[5] == 192)
+                else if (sweep == 192)
                 {
                     msg.edge_brush_lift = true;
                     msg.sweep_ctrl = true;
                     msg.spray = true;
                     msg.mud_flap = true;
-                    msg.dust_shake = false;
+                    msg.dust_shake = true;
                 }
 
                 // 转向逻辑
-                float target_angle = (992 - static_cast<float>(ch_data[3])) / 800 * EPS_ANGLE_MAX;
+                float target_angle = (static_cast<float>(ch_data[3]) - 992) / 800 * EPS_ANGLE_MAX;
 
                 // 角速度（度/秒）
                 float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T;
 
                 // 电机转速（单位：rpm）
-                float motor_rpm = angle_speed * EPS_GEAR_RATIO;
+                float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO;
 
                 // 限制电机转速到 [120, 1500] 范围，防止过小/过大
                 uint16_t can_speed = std::clamp(
@@ -139,7 +196,7 @@ private:
                 msg.angle = target_angle;
                 msg.angle_speed = can_speed;
             }
-            else // 未使能状态，发送安全默认控制指令
+            else // 未释放，发送安全默认控制指令
             {
                 msg.brake = 1;
                 msg.gear = 0;
@@ -157,22 +214,23 @@ private:
             pub_->publish(msg);
 
             RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:"
-                                                       << "\n  brake: " << static_cast<int>(msg.brake)                           // uint8
+                                                       << "\n  brake: " << static_cast<int>(msg.brake)                             // uint8
-                                                       << "\n  gear: " << static_cast<int>(msg.gear)                             // uint8
+                                                       << "\n  gear: " << static_cast<int>(msg.gear)                               // uint8
-                                                       << "\n  rpm: " << static_cast<int>(msg.rpm)                               // uint8
+                                                       << "\n  rpm: " << static_cast<int>(msg.rpm)                                 // uint8
-                                                       << "\n  ehb_enable: " << static_cast<int>(msg.ehb_anable)                 // bool
+                                                       << "\n  ehb_enable: " << static_cast<int>(msg.ehb_anable)                   // bool
-                                                       << "\n  ehb_brake_pressure: " << static_cast<int>(msg.ehb_brake_pressure) // uint8
+                                                       << "\n  ehb_brake_pressure: " << static_cast<float>(msg.ehb_brake_pressure) // float32
-                                                       << "\n  angle: " << msg.angle                                             // float32
+                                                       << "\n  angle: " << msg.angle                                               // float32
-                                                       << "\n  angle_speed: " << msg.angle_speed                                 // uint16
+                                                       << "\n  angle_speed: " << msg.angle_speed                                   // uint16
-                                                       << "\n  edge_brush_lift: " << static_cast<int>(msg.edge_brush_lift)       // bool
+                                                       << "\n  edge_brush_lift: " << static_cast<int>(msg.edge_brush_lift)         // bool
-                                                       << "\n  sweep_ctrl: " << static_cast<int>(msg.sweep_ctrl)                 // bool
+                                                       << "\n  sweep_ctrl: " << static_cast<int>(msg.sweep_ctrl)                   // bool
-                                                       << "\n  spray: " << static_cast<int>(msg.spray)                           // bool
+                                                       << "\n  spray: " << static_cast<int>(msg.spray)                             // bool
-                                                       << "\n  mud_flap: " << static_cast<int>(msg.mud_flap)                     // bool
+                                                       << "\n  mud_flap: " << static_cast<int>(msg.mud_flap)                       // bool
-                                                       << "\n  dust_shake: " << static_cast<int>(msg.dust_shake)                 // bool
+                                                       << "\n  dust_shake: " << static_cast<int>(msg.dust_shake)                   // bool
-                                                       << "\n  left_light: " << static_cast<int>(msg.left_light)                 // bool
+                                                       << "\n  left_light: " << static_cast<int>(msg.left_light)                   // bool
-                                                       << "\n  right_light: " << static_cast<int>(msg.right_light)               // bool
+                                                       << "\n  right_light: " << static_cast<int>(msg.right_light)                 // bool
-                                                       << "\n  brake_light: " << static_cast<int>(msg.brake_light)               // bool
+                                                       << "\n  brake_light: " << static_cast<int>(msg.brake_light)                 // bool
-                                                       << "\n  headlight: " << static_cast<int>(msg.headlight)                   // bool
+                                                       << "\n  headlight: " << static_cast<int>(msg.headlight)                     // bool
+                                                       << "\n  dump: " << static_cast<int>(msg.dump)                               // uint8
             );
         }
         else

src/sweeper_interfaces/msg/McCtrl.msg

@@ -7,7 +7,7 @@ uint8 rpm	            #转速占空比 0-100
 
 #ehb部分
 bool ehb_anable                 #ehb使能
-uint8 ehb_brake_pressure        #ehb制动压力 0-8MPa
+float32 ehb_brake_pressure      #ehb制动压力 0-8MPa
 
 #eps部分
 float32 angle	    #轮端转向角度 分辨率0.2° [-66.0,66.0] 适当缩减
@@ -24,3 +24,6 @@ bool left_light     #左转向灯
 bool right_light    #右转向灯
 bool brake_light    #刹车灯
 bool headlight      #大灯
+
+#倒灰
+uint8 dump          # 0 全部收回 1 斗升 2 倒斗