lyq/sweeper_whu
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat : VCU

Browse Source
This commit is contained in:
Alvin-lyq 2025-09-01 16:57:07 +08:00
parent 1c21207965
commit 537c187480
14 changed files with 583 additions and 532 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/ctrl_arbiter/src/ctrl_arbiter.cpp
View File

@ -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,

199
src/mc/include/mc/can_struct.h
View File

@ -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<Byte0 *>(&data[0]); }
Byte3 &byte3() { return *reinterpret_cast<Byte3 *>(&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<uint8_t>(0), static_cast<uint8_t>(2));
}
union Byte3
// 设置电机M1油门 (滚刷&吸风电机) - Byte 1
void setM1Throttle(uint8_t throttle)
{
struct
data[1] = std::clamp(throttle, static_cast<uint8_t>(0), static_cast<uint8_t>(100));
}
// 设置电机M2 (边刷电机) - Byte 2
void setM2Speed(int8_t speed)
{
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[2] = static_cast<uint8_t>(std::clamp(speed, static_cast<int8_t>(0), static_cast<int8_t>(100)));
}
// 设置电机M3 (振尘电机) - Byte 3
void setM3Speed(int8_t speed)
{
data[3] = static_cast<uint8_t>(std::clamp(speed, static_cast<int8_t>(0), static_cast<int8_t>(100)));
}
// 设置电机M4 (刷盘推杆电机) - Byte 4
void setM4Speed(int8_t speed)
{
data[4] = static_cast<uint8_t>(std::clamp(speed, static_cast<int8_t>(-100), static_cast<int8_t>(100)));
}
// 设置电机M5 (吸扒推杆电机) - Byte 5
void setM5Speed(int8_t speed)
{
data[5] = static_cast<uint8_t>(std::clamp(speed, static_cast<int8_t>(-100), static_cast<int8_t>(100)));
}
// 设置电机M6 (LED_OUT6) - Byte 6
void setM6_LED6(int8_t value)
{
data[6] = static_cast<uint8_t>(std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100)));
}
// 设置电机M7 (LED_OUT7) - Byte 7
void setM7_LED7(int8_t value)
{
data[7] = static_cast<uint8_t>(std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(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<Byte0 *>(&data[0]); }
Byte1 &byte1() { return *reinterpret_cast<Byte1 *>(&data[1]); }
Byte3 &byte3() { return *reinterpret_cast<Byte3 *>(&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<uint8_t>(std::clamp(speed, static_cast<int8_t>(-100), static_cast<int8_t>(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<uint8_t>(0), static_cast<uint8_t>(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<uint8_t>(0), static_cast<uint8_t>(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<uint8_t>(0), static_cast<uint8_t>(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<uint8_t>(0), static_cast<uint8_t>(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<uint8_t>(0), static_cast<uint8_t>(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

63
src/mc/include/mc/dumper.h
View File

@ -1,63 +0,0 @@
// 专门用于垃圾倾倒的头文件
#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

3
src/mc/include/mc/timer_tasks.hpp
View File

@ -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);

5
src/mc/src/can_struct.cpp
View File

@ -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;

13
src/mc/src/control_cache.cpp
View File

@ -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;
}

218
src/mc/src/dumper.cpp
View File

@ -1,218 +0,0 @@
#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; // 斗升且已放斗
}

36
src/mc/src/mc.cpp
View File

@ -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<bool> vcu_msg_received = false;
std::atomic<bool> 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<int>(msg->brake) // uint8
<< "\n gear: " << static_cast<int>(msg->gear) // uint8
<< "\n rpm: " << static_cast<int>(msg->rpm) // uint8
<< "\n ehb_enable: " << static_cast<int>(msg->ehb_anable) // bool
<< "\n ehb_brake_pressure: " << static_cast<float>(msg->ehb_brake_pressure) // float32
<< "\n angle: " << msg->angle // float32
<< "\n angle_speed: " << msg->angle_speed // uint16
<< "\n edge_brush_lift: " << static_cast<int>(msg->edge_brush_lift) // bool
<< "\n sweep_ctrl: " << static_cast<int>(msg->sweep_ctrl) // bool
<< "\n spray: " << static_cast<int>(msg->spray) // bool
<< "\n mud_flap: " << static_cast<int>(msg->mud_flap) // bool
<< "\n dust_shake: " << static_cast<int>(msg->dust_shake) // bool
<< "\n left_light: " << static_cast<int>(msg->left_light) // bool
<< "\n right_light: " << static_cast<int>(msg->right_light) // bool
<< "\n brake_light: " << static_cast<int>(msg->brake_light) // bool
<< "\n headlight: " << static_cast<int>(msg->headlight) // bool
<< "\n dump: " << static_cast<int>(msg->dump) // uint8
<< "\n 电磁刹: " << static_cast<int>(msg->brake)
<< "\n 挡位: " << static_cast<int>(msg->gear)
<< "\n 转速: " << static_cast<int>(msg->rpm)
<< "\n ehb使能: " << static_cast<int>(msg->ehb_anable)
<< "\n ehb制动压力: " << static_cast<float>(msg->ehb_brake_pressure)
<< "\n 轮端转向角度: " << msg->angle
<< "\n 转向角速度: " << msg->angle_speed
<< "\n 滚刷&吸风电机方向: " << static_cast<int>(msg->roll_brush_suction_direction)
<< "\n 滚刷&吸风电机: " << static_cast<int>(msg->roll_brush_suction)
<< "\n 边刷电机: " << static_cast<int>(msg->side_brush)
<< "\n 振尘电机: " << static_cast<int>(msg->dust_shake)
<< "\n 刷盘推杆电机: " << static_cast<int>(msg->brush_deck_pusher)
<< "\n 吸扒推杆电机: " << static_cast<int>(msg->suction_squeeqee_pusher)
<< "\n 喷水电机: " << static_cast<int>(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(); });

118
src/mc/src/timer_tasks.cpp
View File

@ -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<int>(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");
}

121
src/mqtt_report/src/VCU_Fault.txt Normal file
View File

@ -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

115
src/mqtt_report/src/fault_codes.cpp
View File

@ -131,4 +131,119 @@ std::unordered_map<int, FaultInfo> 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}},
};

84
src/mqtt_report/src/mqtt_report.cpp
View File

@ -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<uint32_t>(msg->data[0]) << 0) |
(static_cast<uint32_t>(msg->data[1]) << 8) |
(static_cast<uint32_t>(msg->data[2]) << 16) |
(static_cast<uint32_t>(msg->data[3]) << 24);
// 提取故障2 (32位): msg->data[4] ~ msg->data[7]
uint32_t fault2_bits = (static_cast<uint32_t>(msg->data[4]) << 0) |
(static_cast<uint32_t>(msg->data[5]) << 8) |
(static_cast<uint32_t>(msg->data[6]) << 16) |
(static_cast<uint32_t>(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<uint32_t>(msg->data[0]) << 0) |
(static_cast<uint32_t>(msg->data[1]) << 8) |
(static_cast<uint32_t>(msg->data[2]) << 16) |
(static_cast<uint32_t>(msg->data[3]) << 24);
// 提取故障4 (32位): msg->data[4] ~ msg->data[7]
uint32_t fault4_bits = (static_cast<uint32_t>(msg->data[4]) << 0) |
(static_cast<uint32_t>(msg->data[5]) << 8) |
(static_cast<uint32_t>(msg->data[6]) << 16) |
(static_cast<uint32_t>(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;
}
}
}

105
src/radio_ctrl/src/radio_ctrl.cpp
View File

@ -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<int>(msg.brake) // uint8
<< "\n gear: " << static_cast<int>(msg.gear) // uint8
<< "\n rpm: " << static_cast<int>(msg.rpm) // uint8
<< "\n ehb_enable: " << static_cast<int>(msg.ehb_anable) // bool
<< "\n ehb_brake_pressure: " << static_cast<float>(msg.ehb_brake_pressure) // float32
<< "\n angle: " << msg.angle // float32
<< "\n angle_speed: " << msg.angle_speed // uint16
<< "\n edge_brush_lift: " << static_cast<int>(msg.edge_brush_lift) // bool
<< "\n sweep_ctrl: " << static_cast<int>(msg.sweep_ctrl) // bool
<< "\n spray: " << static_cast<int>(msg.spray) // bool
<< "\n mud_flap: " << static_cast<int>(msg.mud_flap) // bool
<< "\n dust_shake: " << static_cast<int>(msg.dust_shake) // bool
<< "\n left_light: " << static_cast<int>(msg.left_light) // bool
<< "\n right_light: " << static_cast<int>(msg.right_light) // bool
<< "\n brake_light: " << static_cast<int>(msg.brake_light) // bool
<< "\n headlight: " << static_cast<int>(msg.headlight) // bool
<< "\n dump: " << static_cast<int>(msg.dump) // uint8
);
std::cout << "Sub ControlMsg:"
<< "\n 电磁刹: " << static_cast<int>(msg.brake)
<< "\n 挡位: " << static_cast<int>(msg.gear)
<< "\n 转速: " << static_cast<int>(msg.rpm)
<< "\n ehb使能: " << static_cast<int>(msg.ehb_anable)
<< "\n ehb制动压力: " << static_cast<float>(msg.ehb_brake_pressure)
<< "\n 轮端转向角度: " << msg.angle
<< "\n 转向角速度: " << msg.angle_speed
<< "\n 滚刷&吸风电机方向: " << static_cast<int>(msg.roll_brush_suction_direction)
<< "\n 滚刷&吸风电机: " << static_cast<int>(msg.roll_brush_suction)
<< "\n 边刷电机: " << static_cast<int>(msg.side_brush)
<< "\n 振尘电机: " << static_cast<int>(msg.dust_shake)
<< "\n 刷盘推杆电机: " << static_cast<int>(msg.brush_deck_pusher)
<< "\n 吸扒推杆电机: " << static_cast<int>(msg.suction_squeeqee_pusher)
<< "\n 喷水电机: " << static_cast<int>(msg.water_spray)
<< std::endl;
}
else
{

20
src/sweeper_interfaces/msg/McCtrl.msg
View File

@ -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 倒斗
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