diff --git a/src/ctrl_arbiter/src/ctrl_arbiter.cpp b/src/ctrl_arbiter/src/ctrl_arbiter.cpp index 8d20122..2f6f6ae 100644 --- a/src/ctrl_arbiter/src/ctrl_arbiter.cpp +++ b/src/ctrl_arbiter/src/ctrl_arbiter.cpp @@ -84,11 +84,14 @@ private: safe_msg.rpm = 0; safe_msg.angle = 0; safe_msg.angle_speed = 120; - safe_msg.edge_brush_lift = false; - safe_msg.sweep_ctrl = false; - safe_msg.spray = false; - safe_msg.mud_flap = false; - safe_msg.dust_shake = false; + + safe_msg.roll_brush_suction_direction = 0; + safe_msg.roll_brush_suction = 0; + safe_msg.side_brush = 0; + safe_msg.dust_shake = 0; + safe_msg.brush_deck_pusher = 0; + safe_msg.suction_squeeqee_pusher = 0; + safe_msg.water_spray = 0; publisher_->publish(safe_msg); RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, diff --git a/src/mc/include/mc/can_struct.h b/src/mc/include/mc/can_struct.h index 3677f9d..197254d 100644 --- a/src/mc/include/mc/can_struct.h +++ b/src/mc/include/mc/can_struct.h @@ -180,52 +180,21 @@ struct can_EPS_cmd } }; -struct can_VCU_out1_cmd +// CAN指令使能信号 (0x210) +struct can_VCU_enable_cmd { - static constexpr uint32_t CMD_ID = 0x18f21117; + static constexpr uint32_t CMD_ID = 0x210; static constexpr bool EXT_FLAG = true; static constexpr bool RTR_FLAG = false; - union Byte0 - { - struct - { - uint8_t reserved1 : 3; // 保留位 - uint8_t dust_shake : 1; // 振尘 - uint8_t reserved2 : 4; // 保留位 - } bits; - uint8_t raw = 0; - }; - - union Byte3 - { - struct - { - uint8_t headlight : 4; // 大灯 - uint8_t reserved : 3; // 保留位 - uint8_t leftLight : 1; // 左转灯 - } bits; - uint8_t raw = 0; - }; - uint8_t data[8]{}; - Byte0 &byte0() { return *reinterpret_cast(&data[0]); } - Byte3 &byte3() { return *reinterpret_cast(&data[3]); } - - void setDustShake(bool on) + // 设置CAN指令使能 + void setCANEnable(bool enable) { - byte0().bits.dust_shake = on ? 1 : 0; - } - - void setLeftLight(bool on) - { - byte3().bits.leftLight = on ? 1 : 0; - } - - void setHeadLight(bool on) - { - byte3().bits.headlight = on ? 0x64 : 0x00; + data[0] = enable ? 1 : 0; + // 清零其他字节 + memset(&data[1], 0, 7); } CANFrame toFrame() const @@ -240,91 +209,130 @@ struct can_VCU_out1_cmd } }; -struct can_VCU_out2_cmd +// 电机控制指令1 (0x211) - 控制M1~M7 +struct can_VCU_motor1_cmd { - static constexpr uint32_t CMD_ID = 0x18fa1117; + static constexpr uint32_t CMD_ID = 0x211; static constexpr bool EXT_FLAG = true; static constexpr bool RTR_FLAG = false; - union Byte0 - { - struct - { - uint8_t right_light : 1; // 右转灯 - uint8_t brake_light : 1; // 刹车灯 - uint8_t reserved0 : 6; // 保留位 - } bits; - uint8_t raw = 0; - }; + uint8_t data[8]{}; - union Byte1 + // 设置电机M1方向 (滚刷&吸风电机) - Byte 0 + void setM1Direction(uint8_t direction) { - struct - { - uint8_t reserved1 : 5; // 保留位 - uint8_t sweep_ctrl : 1; // 扫地控制 - uint8_t reserved2 : 1; // 保留位 - uint8_t headlight : 1; // 大灯 - } bits; - uint8_t raw = 0; - }; + data[0] = std::clamp(direction, static_cast(0), static_cast(2)); + } - union Byte3 + // 设置电机M1油门 (滚刷&吸风电机) - Byte 1 + void setM1Throttle(uint8_t throttle) { - struct - { - uint8_t edge_brush_lift : 1; // 边刷推杆 - uint8_t reserved3 : 1; // 保留位 - uint8_t spray : 1; // 喷水 - uint8_t reserved4 : 3; // 保留位 - uint8_t mud_flap : 1; // 挡皮 - uint8_t reserved5 : 1; // 保留位 - } bits; - uint8_t raw = 0; - }; + data[1] = std::clamp(throttle, static_cast(0), static_cast(100)); + } + + // 设置电机M2 (边刷电机) - Byte 2 + void setM2Speed(int8_t speed) + { + data[2] = static_cast(std::clamp(speed, static_cast(0), static_cast(100))); + } + + // 设置电机M3 (振尘电机) - Byte 3 + void setM3Speed(int8_t speed) + { + data[3] = static_cast(std::clamp(speed, static_cast(0), static_cast(100))); + } + + // 设置电机M4 (刷盘推杆电机) - Byte 4 + void setM4Speed(int8_t speed) + { + data[4] = static_cast(std::clamp(speed, static_cast(-100), static_cast(100))); + } + + // 设置电机M5 (吸扒推杆电机) - Byte 5 + void setM5Speed(int8_t speed) + { + data[5] = static_cast(std::clamp(speed, static_cast(-100), static_cast(100))); + } + + // 设置电机M6 (LED_OUT6) - Byte 6 + void setM6_LED6(int8_t value) + { + data[6] = static_cast(std::clamp(value, static_cast(0), static_cast(100))); + } + + // 设置电机M7 (LED_OUT7) - Byte 7 + void setM7_LED7(int8_t value) + { + data[7] = static_cast(std::clamp(value, static_cast(0), static_cast(100))); + } + + CANFrame toFrame() const + { + CANFrame frame; + frame.id = CMD_ID; + std::copy(std::begin(data), std::end(data), frame.data); + frame.dlc = 8; + frame.ext = EXT_FLAG; + frame.rtr = RTR_FLAG; + return frame; + } +}; + +// 电机控制指令2 (0x212) - 控制M8和LED输出 +struct can_VCU_motor2_cmd +{ + static constexpr uint32_t CMD_ID = 0x212; + static constexpr bool EXT_FLAG = true; + static constexpr bool RTR_FLAG = false; uint8_t data[8]{}; - Byte0 &byte0() { return *reinterpret_cast(&data[0]); } - Byte1 &byte1() { return *reinterpret_cast(&data[1]); } - Byte3 &byte3() { return *reinterpret_cast(&data[3]); } - - // === Byte0 控制 === - void setRightLight(bool on) + // 设置电机M8 (喷水电机) - Byte 0 + void setM8_Spray(int8_t speed) { - byte0().bits.right_light = on ? 1 : 0; + data[0] = static_cast(std::clamp(speed, static_cast(-100), static_cast(100))); } - void setBrakeLight(bool on) + // 设置OUT1 (预留20mA) - Byte 1 + void setOUT1_Reserved(uint8_t value) { - byte0().bits.brake_light = on ? 1 : 0; + data[1] = value; // 预留字段 } - // === Byte1 控制 === - void setSweeepCtrl(bool on) + // 设置OUT2 (LED_OUT1) - Byte 2 + void setOUT2_LED1(uint8_t brightness) { - byte1().bits.sweep_ctrl = on ? 1 : 0; + data[2] = std::clamp(brightness, static_cast(0), static_cast(100)); } - void setHeadlight(bool on) + // 设置OUT3 (LED_OUT2) - Byte 3 + void setOUT3_LED2(uint8_t brightness) { - byte1().bits.headlight = on ? 1 : 0; + data[3] = std::clamp(brightness, static_cast(0), static_cast(100)); } - // === Byte3 控制 === - void setEdgeBrushLift(bool on) + // 设置OUT4 (LED_OUT3) - Byte 4 + void setOUT4_LED3(uint8_t brightness) { - byte3().bits.edge_brush_lift = on ? 1 : 0; + data[4] = std::clamp(brightness, static_cast(0), static_cast(100)); } - void setSpray(bool on) + // 设置OUT5 (LED_OUT4) - Byte 5 + void setOUT5_LED4(uint8_t brightness) { - byte3().bits.spray = on ? 1 : 0; + data[5] = std::clamp(brightness, static_cast(0), static_cast(100)); } - void setMudFlap(bool on) + // 设置OUT6 (LED_OUT5) - Byte 6 + void setOUT6_LED5(uint8_t brightness) { - byte3().bits.mud_flap = on ? 1 : 0; + data[6] = std::clamp(brightness, static_cast(0), static_cast(100)); + } + + // Byte 7 未使用，保持为0 + void clearByte7() + { + data[7] = 0; } CANFrame toFrame() const @@ -383,8 +391,9 @@ struct can_DBS_cmd 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_VCU_enable_cmd vcu_enable_cmd; +extern can_VCU_motor1_cmd vcu_motor1_cmd; +extern can_VCU_motor2_cmd vcu_motor2_cmd; extern can_DBS_cmd dbs_cmd; #endif diff --git a/src/mc/include/mc/dumper.h b/src/mc/include/mc/dumper.h deleted file mode 100644 index 6ec3022..0000000 --- a/src/mc/include/mc/dumper.h +++ /dev/null @@ -1,63 +0,0 @@ -// 专门用于垃圾倾倒的头文件 -#ifndef DUMPER_H -#define DUMPER_H - -#include "can_driver.h" -#include -#include -#include - -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 &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 action_queue_; - std::atomic busy_; - std::atomic has_pending_task_; - std::atomic sweeping_ = false; // 扫地任务标志 - DumperState current_state_; - CANDriver &can_; - rclcpp::Logger logger_; -}; - -#endif \ No newline at end of file diff --git a/src/mc/include/mc/timer_tasks.hpp b/src/mc/include/mc/timer_tasks.hpp index c37327f..556b93c 100644 --- a/src/mc/include/mc/timer_tasks.hpp +++ b/src/mc/include/mc/timer_tasks.hpp @@ -1,7 +1,6 @@ #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); +void setupTimers(rclcpp::Node::SharedPtr node, CANDriver &canctl); diff --git a/src/mc/src/can_struct.cpp b/src/mc/src/can_struct.cpp index 301bbe3..9d03410 100644 --- a/src/mc/src/can_struct.cpp +++ b/src/mc/src/can_struct.cpp @@ -2,6 +2,7 @@ can_MCU_cmd mcu_cmd; can_EPS_cmd eps_cmd; -can_VCU_out1_cmd vcu1_cmd; -can_VCU_out2_cmd vcu2_cmd; +can_VCU_enable_cmd vcu_enable_cmd; +can_VCU_motor1_cmd vcu_motor1_cmd; +can_VCU_motor2_cmd vcu_motor2_cmd; can_DBS_cmd dbs_cmd; diff --git a/src/mc/src/control_cache.cpp b/src/mc/src/control_cache.cpp index 3f0aa32..35b8e02 100644 --- a/src/mc/src/control_cache.cpp +++ b/src/mc/src/control_cache.cpp @@ -34,11 +34,14 @@ sweeper_interfaces::msg::McCtrl get_safe_control() 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; + + msg.roll_brush_suction_direction = 0; + msg.roll_brush_suction = 0; + msg.side_brush = 0; + msg.dust_shake = 0; + msg.brush_deck_pusher = 0; + msg.suction_squeeqee_pusher = 0; + msg.water_spray = 0; } return msg; } diff --git a/src/mc/src/dumper.cpp b/src/mc/src/dumper.cpp deleted file mode 100644 index 183d371..0000000 --- a/src/mc/src/dumper.cpp +++ /dev/null @@ -1,218 +0,0 @@ -#include "mc/dumper.h" -#include -#include -#include - -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 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 &actions) -{ - if (busy_) - { - std::lock_guard 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 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(current_state_)]); -} - -int DumperController::getSimpleState() const -{ - if (!raised_) - return 0; // 斗未升（已收） - else if (!dumped_) - return 1; // 斗升但未放斗 - else - return 2; // 斗升且已放斗 -} diff --git a/src/mc/src/mc.cpp b/src/mc/src/mc.cpp index 9e46210..15e8cc2 100644 --- a/src/mc/src/mc.cpp +++ b/src/mc/src/mc.cpp @@ -1,7 +1,6 @@ #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" @@ -14,7 +13,6 @@ namespace sweeperMsg = sweeper_interfaces::msg; CANDriver canctl; -DumperController dumper(canctl); std::atomic vcu_msg_received = false; std::atomic vcu_awake = false; rclcpp::Time last_vcu_msg_time; @@ -22,26 +20,22 @@ rclcpp::Time last_vcu_msg_time; void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg) { std::cout << "Sub ControlMsg:" - << "\n brake: " << static_cast(msg->brake) // uint8 - << "\n gear: " << static_cast(msg->gear) // uint8 - << "\n rpm: " << static_cast(msg->rpm) // uint8 - << "\n ehb_enable: " << static_cast(msg->ehb_anable) // bool - << "\n ehb_brake_pressure: " << static_cast(msg->ehb_brake_pressure) // float32 - << "\n angle: " << msg->angle // float32 - << "\n angle_speed: " << msg->angle_speed // uint16 - << "\n edge_brush_lift: " << static_cast(msg->edge_brush_lift) // bool - << "\n sweep_ctrl: " << static_cast(msg->sweep_ctrl) // bool - << "\n spray: " << static_cast(msg->spray) // bool - << "\n mud_flap: " << static_cast(msg->mud_flap) // bool - << "\n dust_shake: " << static_cast(msg->dust_shake) // bool - << "\n left_light: " << static_cast(msg->left_light) // bool - << "\n right_light: " << static_cast(msg->right_light) // bool - << "\n brake_light: " << static_cast(msg->brake_light) // bool - << "\n headlight: " << static_cast(msg->headlight) // bool - << "\n dump: " << static_cast(msg->dump) // uint8 + << "\n 电磁刹: " << static_cast(msg->brake) + << "\n 挡位: " << static_cast(msg->gear) + << "\n 转速: " << static_cast(msg->rpm) + << "\n ehb使能: " << static_cast(msg->ehb_anable) + << "\n ehb制动压力: " << static_cast(msg->ehb_brake_pressure) + << "\n 轮端转向角度: " << msg->angle + << "\n 转向角速度: " << msg->angle_speed + << "\n 滚刷&吸风电机方向: " << static_cast(msg->roll_brush_suction_direction) + << "\n 滚刷&吸风电机: " << static_cast(msg->roll_brush_suction) + << "\n 边刷电机: " << static_cast(msg->side_brush) + << "\n 振尘电机: " << static_cast(msg->dust_shake) + << "\n 刷盘推杆电机: " << static_cast(msg->brush_deck_pusher) + << "\n 吸扒推杆电机: " << static_cast(msg->suction_squeeqee_pusher) + << "\n 喷水电机: " << static_cast(msg->water_spray) << std::endl; - dumper.setSweeping(msg->sweep_ctrl); control_cache.update(*msg); } @@ -69,7 +63,7 @@ int main(int argc, char **argv) context->publisher = pub; canctl.setReceiveCallback(receiveHandler, context.get()); - setupTimers(node, dumper, canctl); + setupTimers(node, canctl); rclcpp::on_shutdown([&]() { canctl.close(); }); diff --git a/src/mc/src/timer_tasks.cpp b/src/mc/src/timer_tasks.cpp index 70f717d..a902345 100644 --- a/src/mc/src/timer_tasks.cpp +++ b/src/mc/src/timer_tasks.cpp @@ -17,8 +17,8 @@ void wakeupTimerTask(const rclcpp::Node::SharedPtr &node, CANDriver &canctl) } } -// 定时器回调：VCU watchdog + Dumper Dial处理 -void vcuWatchdogTask(const rclcpp::Node::SharedPtr &node, DumperController &dumper) +// 定时器回调：VCU watchdog +void vcuWatchdogTask(const rclcpp::Node::SharedPtr &node) { auto now = node->now(); if (vcu_msg_received.load() && (now - last_vcu_msg_time > rclcpp::Duration::from_seconds(0.5))) @@ -26,15 +26,12 @@ void vcuWatchdogTask(const rclcpp::Node::SharedPtr &node, DumperController &dump 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()); - } + // if (vcu_awake.load()) + // { + // auto msg = get_safe_control(); + // } } // 定时器回调：DBS 控制 @@ -68,51 +65,83 @@ void epsTimerTask(CANDriver &canctl) canctl.sendFrame(eps_cmd.toFrame()); } -// 定时器回调：VCU 灯控、扫地、除尘等功能 -void vcuTimerTask(CANDriver &canctl, DumperController &dumper) +// 定时器回调：VCU 扫地控制（使用新的报文结构） +void vcuTimerTask(CANDriver &canctl) { - static bool last_sweep_ctrl = false; // 上一次扫地状态（用于比较是否发生变化） + static bool vcu_initialized = false; + + static int last_roll_brush_suction_direction = 0; + static int last_roll_brush_suction = 0; + static int last_side_brush = 0; + static int last_dust_shake = 0; + static int last_brush_deck_pusher = 0; + static int last_suction_squeeqee_pusher = 0; + static int last_water_spray = 0; 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) + // 首次运行时初始化VCU控制 + if (!vcu_initialized) { - // 状态变更（开或关） 或者 满足开始扫地条件，发出指令 - canctl.sendFrame(vcu1_cmd.toFrame()); - canctl.sendFrame(vcu2_cmd.toFrame()); - - last_sweep_ctrl = msg.sweep_ctrl; // 更新上一次状态 + // 发送CAN使能指令 + vcu_enable_cmd.setCANEnable(true); + canctl.sendFrame(vcu_enable_cmd.toFrame()); + vcu_initialized = true; + RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[VCU] CAN control enabled"); } + + // 检查状态变化 + bool roll_brush_suction_direction_changed = (msg.roll_brush_suction_direction != last_roll_brush_suction_direction); + bool roll_brush_suction_changed = (msg.roll_brush_suction != last_roll_brush_suction); + bool side_brush_changed = (msg.side_brush != last_side_brush); + bool dust_shake_changed = (msg.dust_shake != last_dust_shake); + bool brush_deck_pusher_changed = (msg.brush_deck_pusher != last_brush_deck_pusher); + bool suction_squeeqee_pusher_changed = (msg.suction_squeeqee_pusher != last_suction_squeeqee_pusher); + bool water_spray_changed = (msg.water_spray != last_water_spray); + + bool motor1_needs_update = roll_brush_suction_direction_changed || roll_brush_suction_changed || side_brush_changed || dust_shake_changed || brush_deck_pusher_changed || suction_squeeqee_pusher_changed || water_spray_changed; + bool motor2_needs_update = water_spray_changed; + + // ===== 控制0x211报文 (电机M1-M7) ===== + if (motor1_needs_update) + { + vcu_motor1_cmd.setM1Direction(msg.roll_brush_suction_direction); + vcu_motor1_cmd.setM1Throttle(msg.roll_brush_suction); + vcu_motor1_cmd.setM2Speed(msg.side_brush); + vcu_motor1_cmd.setM3Speed(msg.dust_shake); + vcu_motor1_cmd.setM4Speed(msg.brush_deck_pusher); + vcu_motor1_cmd.setM5Speed(msg.suction_squeeqee_pusher); + + canctl.sendFrame(vcu_motor1_cmd.toFrame()); + } + + // ===== 控制0x212报文 (电机M8和LED输出) ===== + if (motor2_needs_update) + { + // 喷水电机控制 + vcu_motor2_cmd.setM8_Spray(msg.water_spray); + + canctl.sendFrame(vcu_motor2_cmd.toFrame()); + } + + // 更新状态记录 + last_roll_brush_suction_direction = msg.roll_brush_suction_direction; + last_roll_brush_suction = msg.roll_brush_suction; + last_side_brush = msg.side_brush; + last_dust_shake = msg.dust_shake; + last_brush_deck_pusher = msg.brush_deck_pusher; + last_suction_squeeqee_pusher = msg.suction_squeeqee_pusher; + last_water_spray = msg.water_spray; } // 注册所有定时器任务 -void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriver &canctl) +void setupTimers(rclcpp::Node::SharedPtr node, 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]() @@ -130,6 +159,13 @@ void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriv static auto timer_vcu = node->create_wall_timer( std::chrono::milliseconds(100), - [&canctl, &dumper]() - { vcuTimerTask(canctl, dumper); }); + [&canctl]() + { vcuTimerTask(canctl); }); + + static auto timer_vcu_watchdog = node->create_wall_timer( + std::chrono::milliseconds(200), + [node]() + { vcuWatchdogTask(node); }); + + RCLCPP_INFO(node->get_logger(), "[TIMER] All timers setup completed"); } \ No newline at end of file diff --git a/src/mqtt_report/src/VCU_Fault.txt b/src/mqtt_report/src/VCU_Fault.txt new file mode 100644 index 0000000..15b6cd0 --- /dev/null +++ b/src/mqtt_report/src/VCU_Fault.txt @@ -0,0 +1,121 @@ +ID 数据 起始位 长度 备注 +0x216 故障1 0 32 详见《故障代码表》 + 故障2 32 32 详见《故障代码表》 +0x217 故障3 0 32 详见《故障代码表》 + 故障4 0 32 详见《故障代码表》 + + +急停信号 故障代码1-BIT0 +EEPROM故障 故障代码1-BIT1 +电池电压非常低错误 故障代码1-BIT2 +电池电压高错误 故障代码1-BIT3 +预充1错误 故障代码1-BIT4 +钥匙开关电压错误 故障代码1-BIT5 +给CPU的12V电压错误 故障代码1-BIT6 +给驱动的12V电压错误 故障代码1-BIT7 +VDD1电压错误 故障代码1-BIT8 +<控制器内部故障>M1电机MOS管短路错误 故障代码1-BIT9 +<控制器内部故障>M2电机MOS管短路错误 故障代码1-BIT10 +<控制器内部故障>M3电机MOS管短路错误 故障代码1-BIT11 +<控制器内部故障>M4电机MOS管短路错误 故障代码1-BIT12 +<控制器内部故障>M5电机MOS管短路错误 故障代码1-BIT13 +继电器1连接错误 故障代码1-BIT14 +VDD1通道短路 故障代码1-BIT15 +M1电机短路 故障代码1-BIT16 +M2电机短路 故障代码1-BIT17 +M3电机短路 故障代码1-BIT18 +M4电机短路 故障代码1-BIT19 +M5电机短路 故障代码1-BIT20 +CAN主机断线错误 故障代码1-BIT21 +油门连接错误 故障代码1-BIT22 +高踏板静止 故障代码1-BIT23 +限速油门连接错误 故障代码1-BIT24 +温度传感器异常 故障代码1-BIT25 +<控制器内部故障>电流ADC采样错误 故障代码1-BIT26 +M1电机开路 故障代码1-BIT27 +M2电机开路错误 故障代码1-BIT28 +M3电机开路错误 故障代码1-BIT29 +M4电机开路 故障代码1-BIT30 +M5电机开路 故障代码1-BIT31 +M1电机过载 故障代码2-BIT0 +M2电机过载 故障代码2-BIT1 +M3电机过载 故障代码2-BIT2 +M4电机过载 故障代码2-BIT3 +M5电机过载 故障代码2-BIT4 +OUT1开路故障 故障代码2-BIT5 +OUT2开路故障 故障代码2-BIT6 +OUT3开路故障 故障代码2-BIT7 +OUT4开路故障 故障代码2-BIT8 +OUT5开路故障 故障代码2-BIT9 +OUT6开路故障 故障代码2-BIT10 +OUT7开路故障 故障代码2-BIT11 +OUT1短路故障 故障代码2-BIT12 +OUT2短路故障 故障代码2-BIT13 +OUT3短路故障 故障代码2-BIT14 +OUT4短路故障 故障代码2-BIT15 +OUT5短路故障 故障代码2-BIT16 +OUT6短路故障 故障代码2-BIT17 +OUT7短路故障 故障代码2-BIT18 +M1电机MOS管过热错误 故障代码2-BIT19 +M1电机高温故障 故障代码2-BIT20 +产品型号参数错误 故障代码2-BIT21 +无清水告警 故障代码2-BIT22 +污水满告警 故障代码2-BIT23 +座位开关开路错误 故障代码2-BIT24 +转弯限速告警 故障代码2-BIT25 +M2电机欠压告警 故障代码2-BIT26 +M3电机欠压告警 故障代码2-BIT27 +M1电机欠压告警 故障代码2-BIT28 +电池电压非常高错误 故障代码2-BIT29 +电池电压极限高错误 故障代码2-BIT30 +电池电压极限低错误 故障代码2-BIT31 +M1电机A开路告警 故障代码3-BIT0 +M1电机B开路告警 故障代码3-BIT1 +VDD2电压错误 故障代码3-BIT2 +继电器2连接错误 故障代码3-BIT3 +VDD2通道短路 故障代码3-BIT4 +M6电机电机MOS故障 故障代码3-BIT5 +M7电机电机MOS故障 故障代码3-BIT6 +M8电机电机MOS故障 故障代码3-BIT7 +M6电机电机短路 故障代码3-BIT8 +M7电机电机短路 故障代码3-BIT9 +M8电机电机短路 故障代码3-BIT10 +M6电机电机过载 故障代码3-BIT11 +M7电机电机过载 故障代码3-BIT12 +M8电机电机过载 故障代码3-BIT13 +M1电机温度传感器异常 故障代码3-BIT14 +M2电机温度传感器异常 故障代码3-BIT15 +M3电机温度传感器异常 故障代码3-BIT16 +电刹开路错误 故障代码3-BIT17 +电磁刹车短路故障 故障代码3-BIT18 +预充2错误 故障代码3-BIT19 +限速油门故障 故障代码3-BIT20 +喇叭开路告警 故障代码3-BIT21 +喇叭短路告警 故障代码3-BIT22 +清水指示灯开路告警 故障代码3-BIT23 +清水指示灯短路故障 故障代码3-BIT24 +污水指示灯开路告警 故障代码3-BIT25 +污水指示灯短路故障 故障代码3-BIT26 +警灯开路告警 故障代码3-BIT27 +警灯短路故障 故障代码3-BIT28 +电磁阀开路告警 故障代码3-BIT29 +电磁阀短路故障 故障代码3-BIT30 +故障灯开路告警 故障代码3-BIT31 +故障灯短路故障 故障代码4-BIT1 +水泵开路告警 故障代码4-BIT2 +水泵短路故障 故障代码4-BIT3 +车大灯开路告警 故障代码4-BIT4 +车大灯短路故障 故障代码4-BIT5 +灯条开路告警 故障代码4-BIT6 +灯条短路故障 故障代码4-BIT7 +清洁剂泵开路告警 故障代码4-BIT8 +清洁剂泵短路故障 故障代码4-BIT9 +M2电机MOS管过热错误 故障代码4-BIT10 +M2电机MOS管高温告警 故障代码4-BIT11 +M3电机MOS管过热错误 故障代码4-BIT12 +M3电机MOS管高温告警 故障代码4-BIT13 +继电器1断开故障 故障代码4-BIT14 +继电器2断开故障 故障代码4-BIT15 +控制器低温告警 故障代码4-BIT16 +控制器严重低温告警 故障代码4-BIT17 +控制器电流传感器故障 故障代码4-BIT18 \ No newline at end of file diff --git a/src/mqtt_report/src/fault_codes.cpp b/src/mqtt_report/src/fault_codes.cpp index ccbf204..8308821 100644 --- a/src/mqtt_report/src/fault_codes.cpp +++ b/src/mqtt_report/src/fault_codes.cpp @@ -131,4 +131,119 @@ std::unordered_map faultMap = { {1514, {"DBS", "DBS_MECHANICAL_ERR", "机械故障", 0}}, {1515, {"DBS", "DBS_OTHER_ERR", "其他故障", 0}}, + {1701, {"VCU", "VCU_EMERGENCY_STOP_ERR", "急停信号", 0}}, + {1702, {"VCU", "VCU_EEPROM_ERR", "EEPROM故障", 0}}, + {1703, {"VCU", "VCU_BATTERY_VOL_VERY_LOW_ERR", "电池电压非常低错误", 0}}, + {1704, {"VCU", "VCU_BATTERY_VOL_HIGH_ERR", "电池电压高错误", 0}}, + {1705, {"VCU", "VCU_PRECHARGE_1_ERR", "预充1错误", 0}}, + {1706, {"VCU", "VCU_KEY_SWITCH_VOL_ERR", "钥匙开关电压错误", 0}}, + {1707, {"VCU", "VCU_CPU_12V_ERR", "给CPU的12V电压错误", 0}}, + {1708, {"VCU", "VCU_DRIVE_12V_ERR", "给驱动的12V电压错误", 0}}, + {1709, {"VCU", "VCU_VDD1_VOL_ERR", "VDD1电压错误", 0}}, + {1710, {"VCU", "VCU_M1_MOS_SHORT_ERR", "<控制器内部故障>M1电机MOS管短路错误", 0}}, + {1711, {"VCU", "VCU_M2_MOS_SHORT_ERR", "<控制器内部故障>M2电机MOS管短路错误", 0}}, + {1712, {"VCU", "VCU_M3_MOS_SHORT_ERR", "<控制器内部故障>M3电机MOS管短路错误", 0}}, + {1713, {"VCU", "VCU_M4_MOS_SHORT_ERR", "<控制器内部故障>M4电机MOS管短路错误", 0}}, + {1714, {"VCU", "VCU_M5_MOS_SHORT_ERR", "<控制器内部故障>M5电机MOS管短路错误", 0}}, + {1715, {"VCU", "VCU_RELAY1_CONN_ERR", "继电器1连接错误", 0}}, + {1716, {"VCU", "VCU_VDD1_SHORT_ERR", "VDD1通道短路", 0}}, + {1717, {"VCU", "VCU_M1_SHORT_ERR", "M1电机短路", 0}}, + {1718, {"VCU", "VCU_M2_SHORT_ERR", "M2电机短路", 0}}, + {1719, {"VCU", "VCU_M3_SHORT_ERR", "M3电机短路", 0}}, + {1720, {"VCU", "VCU_M4_SHORT_ERR", "M4电机短路", 0}}, + {1721, {"VCU", "VCU_M5_SHORT_ERR", "M5电机短路", 0}}, + {1722, {"VCU", "VCU_CAN_MASTER_DISCONNECT_ERR", "CAN主机断线错误", 0}}, + {1723, {"VCU", "VCU_THROTTLE_CONN_ERR", "油门连接错误", 0}}, + {1724, {"VCU", "VCU_HIGH_PEDAL_STALL_ERR", "高踏板静止", 0}}, + {1725, {"VCU", "VCU_SPEED_LIMIT_THROTTLE_ERR", "限速油门连接错误", 0}}, + {1726, {"VCU", "VCU_TEMP_SENSOR_ABNORMAL_ERR", "温度传感器异常", 0}}, + {1727, {"VCU", "VCU_CURRENT_ADC_ERR", "<控制器内部故障>电流ADC采样错误", 0}}, + {1728, {"VCU", "VCU_M1_OPEN_ERR", "M1电机开路", 0}}, + {1729, {"VCU", "VCU_M2_OPEN_ERR", "M2电机开路错误", 0}}, + {1730, {"VCU", "VCU_M3_OPEN_ERR", "M3电机开路错误", 0}}, + {1731, {"VCU", "VCU_M4_OPEN_ERR", "M4电机开路", 0}}, + {1732, {"VCU", "VCU_M5_OPEN_ERR", "M5电机开路", 0}}, + {1733, {"VCU", "VCU_M1_OVERLOAD_ERR", "M1电机过载", 0}}, + {1734, {"VCU", "VCU_M2_OVERLOAD_ERR", "M2电机过载", 0}}, + {1735, {"VCU", "VCU_M3_OVERLOAD_ERR", "M3电机过载", 0}}, + {1736, {"VCU", "VCU_M4_OVERLOAD_ERR", "M4电机过载", 0}}, + {1737, {"VCU", "VCU_M5_OVERLOAD_ERR", "M5电机过载", 0}}, + {1738, {"VCU", "VCU_OUT1_OPEN_ERR", "OUT1开路故障", 0}}, + {1739, {"VCU", "VCU_OUT2_OPEN_ERR", "OUT2开路故障", 0}}, + {1740, {"VCU", "VCU_OUT3_OPEN_ERR", "OUT3开路故障", 0}}, + {1741, {"VCU", "VCU_OUT4_OPEN_ERR", "OUT4开路故障", 0}}, + {1742, {"VCU", "VCU_OUT5_OPEN_ERR", "OUT5开路故障", 0}}, + {1743, {"VCU", "VCU_OUT6_OPEN_ERR", "OUT6开路故障", 0}}, + {1744, {"VCU", "VCU_OUT7_OPEN_ERR", "OUT7开路故障", 0}}, + {1745, {"VCU", "VCU_OUT1_SHORT_ERR", "OUT1短路故障", 0}}, + {1746, {"VCU", "VCU_OUT2_SHORT_ERR", "OUT2短路故障", 0}}, + {1747, {"VCU", "VCU_OUT3_SHORT_ERR", "OUT3短路故障", 0}}, + {1748, {"VCU", "VCU_OUT4_SHORT_ERR", "OUT4短路故障", 0}}, + {1749, {"VCU", "VCU_OUT5_SHORT_ERR", "OUT5短路故障", 0}}, + {1750, {"VCU", "VCU_OUT6_SHORT_ERR", "OUT6短路故障", 0}}, + {1751, {"VCU", "VCU_OUT7_SHORT_ERR", "OUT7短路故障", 0}}, + {1752, {"VCU", "VCU_M1_MOS_OVERHEAT_ERR", "M1电机MOS管过热错误", 0}}, + {1753, {"VCU", "VCU_M1_HIGH_TEMP_ERR", "M1电机高温故障", 0}}, + {1754, {"VCU", "VCU_MODEL_PARAM_ERR", "产品型号参数错误", 0}}, + {1755, {"VCU", "VCU_NO_CLEAN_WATER_ERR", "无清水告警", 0}}, + {1756, {"VCU", "VCU_DIRTY_WATER_FULL_ERR", "污水满告警", 0}}, + {1757, {"VCU", "VCU_SEAT_SWITCH_OPEN_ERR", "座位开关开路错误", 0}}, + {1758, {"VCU", "VCU_TURN_SPEED_LIMIT_ERR", "转弯限速告警", 0}}, + {1759, {"VCU", "VCU_M2_UNDERVOLTAGE_ERR", "M2电机欠压告警", 0}}, + {1760, {"VCU", "VCU_M3_UNDERVOLTAGE_ERR", "M3电机欠压告警", 0}}, + {1761, {"VCU", "VCU_M1_UNDERVOLTAGE_ERR", "M1电机欠压告警", 0}}, + {1762, {"VCU", "VCU_BATTERY_VOL_VERY_HIGH_ERR", "电池电压非常高错误", 0}}, + {1763, {"VCU", "VCU_BATTERY_VOL_EXTREME_HIGH_ERR", "电池电压极限高错误", 0}}, + {1764, {"VCU", "VCU_BATTERY_VOL_EXTREME_LOW_ERR", "电池电压极限低错误", 0}}, + {1765, {"VCU", "VCU_M1_PHASE_A_OPEN_ERR", "M1电机A开路告警", 0}}, + {1766, {"VCU", "VCU_M1_PHASE_B_OPEN_ERR", "M1电机B开路告警", 0}}, + {1767, {"VCU", "VCU_VDD2_VOL_ERR", "VDD2电压错误", 0}}, + {1768, {"VCU", "VCU_RELAY2_CONN_ERR", "继电器2连接错误", 0}}, + {1769, {"VCU", "VCU_VDD2_SHORT_ERR", "VDD2通道短路", 0}}, + {1770, {"VCU", "VCU_M6_MOS_ERR", "M6电机电机MOS故障", 0}}, + {1771, {"VCU", "VCU_M7_MOS_ERR", "M7电机电机MOS故障", 0}}, + {1772, {"VCU", "VCU_M8_MOS_ERR", "M8电机电机MOS故障", 0}}, + {1773, {"VCU", "VCU_M6_SHORT_ERR", "M6电机电机短路", 0}}, + {1774, {"VCU", "VCU_M7_SHORT_ERR", "M7电机电机短路", 0}}, + {1775, {"VCU", "VCU_M8_SHORT_ERR", "M8电机电机短路", 0}}, + {1776, {"VCU", "VCU_M6_OVERLOAD_ERR", "M6电机电机过载", 0}}, + {1777, {"VCU", "VCU_M7_OVERLOAD_ERR", "M7电机电机过载", 0}}, + {1778, {"VCU", "VCU_M8_OVERLOAD_ERR", "M8电机电机过载", 0}}, + {1779, {"VCU", "VCU_M1_TEMP_SENSOR_ERR", "M1电机温度传感器异常", 0}}, + {1780, {"VCU", "VCU_M2_TEMP_SENSOR_ERR", "M2电机温度传感器异常", 0}}, + {1781, {"VCU", "VCU_M3_TEMP_SENSOR_ERR", "M3电机温度传感器异常", 0}}, + {1782, {"VCU", "VCU_ELECTRIC_BRAKE_OPEN_ERR", "电刹开路错误", 0}}, + {1783, {"VCU", "VCU_ELECTROMAGNETIC_BRAKE_SHORT_ERR", "电磁刹车短路故障", 0}}, + {1784, {"VCU", "VCU_PRECHARGE_2_ERR", "预充2错误", 0}}, + {1785, {"VCU", "VCU_SPEED_LIMIT_THROTTLE_FAULT_ERR", "限速油门故障", 0}}, + {1786, {"VCU", "VCU_HORN_OPEN_ERR", "喇叭开路告警", 0}}, + {1787, {"VCU", "VCU_HORN_SHORT_ERR", "喇叭短路告警", 0}}, + {1788, {"VCU", "VCU_CLEAN_WATER_LED_OPEN_ERR", "清水指示灯开路告警", 0}}, + {1789, {"VCU", "VCU_CLEAN_WATER_LED_SHORT_ERR", "清水指示灯短路故障", 0}}, + {1790, {"VCU", "VCU_DIRTY_WATER_LED_OPEN_ERR", "污水指示灯开路告警", 0}}, + {1791, {"VCU", "VCU_DIRTY_WATER_LED_SHORT_ERR", "污水指示灯短路故障", 0}}, + {1792, {"VCU", "VCU_WARNING_LIGHT_OPEN_ERR", "警灯开路告警", 0}}, + {1793, {"VCU", "VCU_WARNING_LIGHT_SHORT_ERR", "警灯短路故障", 0}}, + {1794, {"VCU", "VCU_SOLENOID_VALVE_OPEN_ERR", "电磁阀开路告警", 0}}, + {1795, {"VCU", "VCU_SOLENOID_VALVE_SHORT_ERR", "电磁阀短路故障", 0}}, + {1796, {"VCU", "VCU_FAULT_LIGHT_OPEN_ERR", "故障灯开路告警", 0}}, + {1797, {"VCU", "VCU_FAULT_LIGHT_SHORT_ERR", "故障灯短路故障", 0}}, + {1798, {"VCU", "VCU_WATER_PUMP_OPEN_ERR", "水泵开路告警", 0}}, + {1799, {"VCU", "VCU_WATER_PUMP_SHORT_ERR", "水泵短路故障", 0}}, + {1800, {"VCU", "VCU_HEADLIGHT_OPEN_ERR", "车大灯开路告警", 0}}, + {1801, {"VCU", "VCU_HEADLIGHT_SHORT_ERR", "车大灯短路故障", 0}}, + {1802, {"VCU", "VCU_LIGHT_STRIP_OPEN_ERR", "灯条开路告警", 0}}, + {1803, {"VCU", "VCU_LIGHT_STRIP_SHORT_ERR", "灯条短路故障", 0}}, + {1804, {"VCU", "VCU_CLEANER_PUMP_OPEN_ERR", "清洁剂泵开路告警", 0}}, + {1805, {"VCU", "VCU_CLEANER_PUMP_SHORT_ERR", "清洁剂泵短路故障", 0}}, + {1806, {"VCU", "VCU_M2_MOS_OVERHEAT_ERR", "M2电机MOS管过热错误", 0}}, + {1807, {"VCU", "VCU_M2_MOS_HIGH_TEMP_ERR", "M2电机MOS管高温告警", 0}}, + {1808, {"VCU", "VCU_M3_MOS_OVERHEAT_ERR", "M3电机MOS管过热错误", 0}}, + {1809, {"VCU", "VCU_M3_MOS_HIGH_TEMP_ERR", "M3电机MOS管高温告警", 0}}, + {1810, {"VCU", "VCU_RELAY1_DISCONNECT_ERR", "继电器1断开故障", 0}}, + {1811, {"VCU", "VCU_RELAY2_DISCONNECT_ERR", "继电器2断开故障", 0}}, + {1812, {"VCU", "VCU_CONTROLLER_LOW_TEMP_ERR", "控制器低温告警", 0}}, + {1813, {"VCU", "VCU_CONTROLLER_SEVERE_LOW_TEMP_ERR", "控制器严重低温告警", 0}}, + {1814, {"VCU", "VCU_CONTROLLER_CURRENT_SENSOR_ERR", "控制器电流传感器故障", 0}}, + }; \ No newline at end of file diff --git a/src/mqtt_report/src/mqtt_report.cpp b/src/mqtt_report/src/mqtt_report.cpp index ee41cde..585c03a 100644 --- a/src/mqtt_report/src/mqtt_report.cpp +++ b/src/mqtt_report/src/mqtt_report.cpp @@ -210,6 +210,90 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg) break; } + + // VCU_Fault_1和Fault_2报文解析 (ID: 0x216) + case 0x216: + { + // 提取故障1 (32位): msg->data[0] ~ msg->data[3] + uint32_t fault1_bits = (static_cast(msg->data[0]) << 0) | + (static_cast(msg->data[1]) << 8) | + (static_cast(msg->data[2]) << 16) | + (static_cast(msg->data[3]) << 24); + + // 提取故障2 (32位): msg->data[4] ~ msg->data[7] + uint32_t fault2_bits = (static_cast(msg->data[4]) << 0) | + (static_cast(msg->data[5]) << 8) | + (static_cast(msg->data[6]) << 16) | + (static_cast(msg->data[7]) << 24); + + // 清空VCU故障码（1701 ~ 1764) + for (int code = 1701; code <= 1764; ++code) + { + vehicle_error_code.removeErrorCode(code); + } + + // 处理故障1的bit0 ~ bit31 → 1701 ~ 1732 + for (uint8_t bit = 0; bit <= 31; ++bit) + { + if ((fault1_bits >> bit) & 0x01) + { + vehicle_error_code.addErrorCode(1701 + bit); + } + } + + // 处理故障2的bit0 ~ bit31 → 1733 ~ 1764 + for (uint8_t bit = 0; bit <= 31; ++bit) + { + if ((fault2_bits >> bit) & 0x01) + { + vehicle_error_code.addErrorCode(1733 + bit); + } + } + + break; + } + + // VCU_Fault_3和Fault_4报文解析 (ID: 0x217) + case 0x217: + { + // 提取故障3 (32位): msg->data[0] ~ msg->data[3] + uint32_t fault3_bits = (static_cast(msg->data[0]) << 0) | + (static_cast(msg->data[1]) << 8) | + (static_cast(msg->data[2]) << 16) | + (static_cast(msg->data[3]) << 24); + + // 提取故障4 (32位): msg->data[4] ~ msg->data[7] + uint32_t fault4_bits = (static_cast(msg->data[4]) << 0) | + (static_cast(msg->data[5]) << 8) | + (static_cast(msg->data[6]) << 16) | + (static_cast(msg->data[7]) << 24); + + // 清空VCU故障码（1765 ~ 1814） + for (int code = 1765; code <= 1814; ++code) + { + vehicle_error_code.removeErrorCode(code); + } + + // 处理故障3的bit0 ~ bit31 → 1765 ~ 1796 + for (uint8_t bit = 0; bit <= 31; ++bit) + { + if ((fault3_bits >> bit) & 0x01) + { + vehicle_error_code.addErrorCode(1765 + bit); + } + } + + // 处理故障4的bit0 ~ bit18 → 1797 ~ 1814 + for (uint8_t bit = 0; bit <= 18; ++bit) + { + if ((fault4_bits >> bit) & 0x01) + { + vehicle_error_code.addErrorCode(1797 + bit); + } + } + + break; + } } } diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index 0c8e43d..56f146f 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -62,7 +62,7 @@ private: { static int MCU_RPM_MAX = config.mcu_rpm_max; static float EPS_ANGLE_MAX = config.eps_angle_max; - static bool initialized = false; // 新增初始化标志 + // static bool initialized = false; // 新增初始化标志 bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性 @@ -82,39 +82,8 @@ private: 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） @@ -163,19 +132,23 @@ private: // 一键清扫 if (sweep == 1792) { - msg.edge_brush_lift = false; - msg.sweep_ctrl = false; - msg.spray = false; - msg.mud_flap = false; - msg.dust_shake = false; + msg.roll_brush_suction_direction = 0; + msg.roll_brush_suction = 0; + msg.side_brush = 0; + msg.dust_shake = 0; + msg.brush_deck_pusher = 0; + msg.suction_squeeqee_pusher = 0; + msg.water_spray = 0; } else if (sweep == 192) { - msg.edge_brush_lift = true; - msg.sweep_ctrl = true; - msg.spray = true; - msg.mud_flap = true; - msg.dust_shake = true; + msg.roll_brush_suction_direction = 1; + msg.roll_brush_suction = 100; + msg.side_brush = 100; + msg.dust_shake = 100; + msg.brush_deck_pusher = 100; + msg.suction_squeeqee_pusher = 100; + msg.water_spray = 100; } // 转向逻辑 @@ -203,35 +176,35 @@ private: 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; + + msg.roll_brush_suction_direction = 0; + msg.roll_brush_suction = 0; + msg.side_brush = 0; + msg.dust_shake = 0; + msg.brush_deck_pusher = 0; + msg.suction_squeeqee_pusher = 0; + msg.water_spray = 0; } // 发布控制消息 pub_->publish(msg); - RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" - << "\n brake: " << static_cast(msg.brake) // uint8 - << "\n gear: " << static_cast(msg.gear) // uint8 - << "\n rpm: " << static_cast(msg.rpm) // uint8 - << "\n ehb_enable: " << static_cast(msg.ehb_anable) // bool - << "\n ehb_brake_pressure: " << static_cast(msg.ehb_brake_pressure) // float32 - << "\n angle: " << msg.angle // float32 - << "\n angle_speed: " << msg.angle_speed // uint16 - << "\n edge_brush_lift: " << static_cast(msg.edge_brush_lift) // bool - << "\n sweep_ctrl: " << static_cast(msg.sweep_ctrl) // bool - << "\n spray: " << static_cast(msg.spray) // bool - << "\n mud_flap: " << static_cast(msg.mud_flap) // bool - << "\n dust_shake: " << static_cast(msg.dust_shake) // bool - << "\n left_light: " << static_cast(msg.left_light) // bool - << "\n right_light: " << static_cast(msg.right_light) // bool - << "\n brake_light: " << static_cast(msg.brake_light) // bool - << "\n headlight: " << static_cast(msg.headlight) // bool - << "\n dump: " << static_cast(msg.dump) // uint8 - ); + std::cout << "Sub ControlMsg:" + << "\n 电磁刹: " << static_cast(msg.brake) + << "\n 挡位: " << static_cast(msg.gear) + << "\n 转速: " << static_cast(msg.rpm) + << "\n ehb使能: " << static_cast(msg.ehb_anable) + << "\n ehb制动压力: " << static_cast(msg.ehb_brake_pressure) + << "\n 轮端转向角度: " << msg.angle + << "\n 转向角速度: " << msg.angle_speed + << "\n 滚刷&吸风电机方向: " << static_cast(msg.roll_brush_suction_direction) + << "\n 滚刷&吸风电机: " << static_cast(msg.roll_brush_suction) + << "\n 边刷电机: " << static_cast(msg.side_brush) + << "\n 振尘电机: " << static_cast(msg.dust_shake) + << "\n 刷盘推杆电机: " << static_cast(msg.brush_deck_pusher) + << "\n 吸扒推杆电机: " << static_cast(msg.suction_squeeqee_pusher) + << "\n 喷水电机: " << static_cast(msg.water_spray) + << std::endl; } else { diff --git a/src/sweeper_interfaces/msg/McCtrl.msg b/src/sweeper_interfaces/msg/McCtrl.msg index 968965d..b462268 100644 --- a/src/sweeper_interfaces/msg/McCtrl.msg +++ b/src/sweeper_interfaces/msg/McCtrl.msg @@ -14,16 +14,10 @@ float32 angle #轮端转向角度 分辨率0.2° [-66.0,66.0] 适当缩减 uint16 angle_speed #转向角速度 120-1500rpm #vcu部分 -bool edge_brush_lift #边刷推杆 -bool sweep_ctrl #扫地控制 -bool spray #喷水 -bool mud_flap #挡皮 -bool dust_shake #振尘 - -bool left_light #左转向灯 -bool right_light #右转向灯 -bool brake_light #刹车灯 -bool headlight #大灯 - -#倒灰 -uint8 dump # 0 全部收回 1 斗升 2 倒斗 \ No newline at end of file +uint8 roll_brush_suction_direction #滚刷&吸风电机方向 0：不转动 1：正转 2：反转 +uint8 roll_brush_suction #滚刷&吸风电机 0 ~ 100 +int8 side_brush #边刷电机 0 ~ 100 +int8 dust_shake #振尘电机 0 ~ 100 +int8 brush_deck_pusher #刷盘推杆电机 -100 ~ 100 +int8 suction_squeeqee_pusher #吸扒推杆电机 -100 ~ 100 +int8 water_spray #喷水电机 -100 ~ 100