feat : VCU
This commit is contained in:
Alvin-lyq
parent
1c21207965
commit
537c187480
@ -84,11 +84,14 @@ private:
|
|||||||
safe_msg.rpm = 0;
|
safe_msg.rpm = 0;
|
||||||
safe_msg.angle = 0;
|
safe_msg.angle = 0;
|
||||||
safe_msg.angle_speed = 120;
|
safe_msg.angle_speed = 120;
|
||||||
safe_msg.edge_brush_lift = false;
|
|
||||||
safe_msg.sweep_ctrl = false;
|
safe_msg.roll_brush_suction_direction = 0;
|
||||||
safe_msg.spray = false;
|
safe_msg.roll_brush_suction = 0;
|
||||||
safe_msg.mud_flap = false;
|
safe_msg.side_brush = 0;
|
||||||
safe_msg.dust_shake = false;
|
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);
|
publisher_->publish(safe_msg);
|
||||||
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
|
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
|
||||||
|
|||||||
@ -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 EXT_FLAG = true;
|
||||||
static constexpr bool RTR_FLAG = false;
|
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]{};
|
uint8_t data[8]{};
|
||||||
|
|
||||||
Byte0 &byte0() { return *reinterpret_cast<Byte0 *>(&data[0]); }
|
// 设置CAN指令使能
|
||||||
Byte3 &byte3() { return *reinterpret_cast<Byte3 *>(&data[3]); }
|
void setCANEnable(bool enable)
|
||||||
|
|
||||||
void setDustShake(bool on)
|
|
||||||
{
|
{
|
||||||
byte0().bits.dust_shake = on ? 1 : 0;
|
data[0] = enable ? 1 : 0;
|
||||||
}
|
// 清零其他字节
|
||||||
|
memset(&data[1], 0, 7);
|
||||||
void setLeftLight(bool on)
|
|
||||||
{
|
|
||||||
byte3().bits.leftLight = on ? 1 : 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
void setHeadLight(bool on)
|
|
||||||
{
|
|
||||||
byte3().bits.headlight = on ? 0x64 : 0x00;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
CANFrame toFrame() const
|
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 EXT_FLAG = true;
|
||||||
static constexpr bool RTR_FLAG = false;
|
static constexpr bool RTR_FLAG = false;
|
||||||
|
|
||||||
union Byte0
|
uint8_t data[8]{};
|
||||||
{
|
|
||||||
struct
|
|
||||||
{
|
|
||||||
uint8_t right_light : 1; // 右转灯
|
|
||||||
uint8_t brake_light : 1; // 刹车灯
|
|
||||||
uint8_t reserved0 : 6; // 保留位
|
|
||||||
} bits;
|
|
||||||
uint8_t raw = 0;
|
|
||||||
};
|
|
||||||
|
|
||||||
union Byte1
|
// 设置电机M1方向 (滚刷&吸风电机) - Byte 0
|
||||||
|
void setM1Direction(uint8_t direction)
|
||||||
{
|
{
|
||||||
struct
|
data[0] = std::clamp(direction, static_cast<uint8_t>(0), static_cast<uint8_t>(2));
|
||||||
{
|
}
|
||||||
uint8_t reserved1 : 5; // 保留位
|
|
||||||
uint8_t sweep_ctrl : 1; // 扫地控制
|
|
||||||
uint8_t reserved2 : 1; // 保留位
|
|
||||||
uint8_t headlight : 1; // 大灯
|
|
||||||
} bits;
|
|
||||||
uint8_t raw = 0;
|
|
||||||
};
|
|
||||||
|
|
||||||
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; // 边刷推杆
|
data[2] = static_cast<uint8_t>(std::clamp(speed, static_cast<int8_t>(0), static_cast<int8_t>(100)));
|
||||||
uint8_t reserved3 : 1; // 保留位
|
}
|
||||||
uint8_t spray : 1; // 喷水
|
|
||||||
uint8_t reserved4 : 3; // 保留位
|
// 设置电机M3 (振尘电机) - Byte 3
|
||||||
uint8_t mud_flap : 1; // 挡皮
|
void setM3Speed(int8_t speed)
|
||||||
uint8_t reserved5 : 1; // 保留位
|
{
|
||||||
} bits;
|
data[3] = static_cast<uint8_t>(std::clamp(speed, static_cast<int8_t>(0), static_cast<int8_t>(100)));
|
||||||
uint8_t raw = 0;
|
}
|
||||||
};
|
|
||||||
|
// 设置电机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]{};
|
uint8_t data[8]{};
|
||||||
|
|
||||||
Byte0 &byte0() { return *reinterpret_cast<Byte0 *>(&data[0]); }
|
// 设置电机M8 (喷水电机) - Byte 0
|
||||||
Byte1 &byte1() { return *reinterpret_cast<Byte1 *>(&data[1]); }
|
void setM8_Spray(int8_t speed)
|
||||||
Byte3 &byte3() { return *reinterpret_cast<Byte3 *>(&data[3]); }
|
|
||||||
|
|
||||||
// === Byte0 控制 ===
|
|
||||||
void setRightLight(bool on)
|
|
||||||
{
|
{
|
||||||
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 控制 ===
|
// 设置OUT2 (LED_OUT1) - Byte 2
|
||||||
void setSweeepCtrl(bool on)
|
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 控制 ===
|
// 设置OUT4 (LED_OUT3) - Byte 4
|
||||||
void setEdgeBrushLift(bool on)
|
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
|
CANFrame toFrame() const
|
||||||
@ -383,8 +391,9 @@ struct can_DBS_cmd
|
|||||||
|
|
||||||
extern can_MCU_cmd mcu_cmd;
|
extern can_MCU_cmd mcu_cmd;
|
||||||
extern can_EPS_cmd eps_cmd;
|
extern can_EPS_cmd eps_cmd;
|
||||||
extern can_VCU_out1_cmd vcu1_cmd;
|
extern can_VCU_enable_cmd vcu_enable_cmd;
|
||||||
extern can_VCU_out2_cmd vcu2_cmd;
|
extern can_VCU_motor1_cmd vcu_motor1_cmd;
|
||||||
|
extern can_VCU_motor2_cmd vcu_motor2_cmd;
|
||||||
extern can_DBS_cmd dbs_cmd;
|
extern can_DBS_cmd dbs_cmd;
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -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
|
|
||||||
@ -1,7 +1,6 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
#include "rclcpp/rclcpp.hpp"
|
#include "rclcpp/rclcpp.hpp"
|
||||||
#include "mc/can_driver.h"
|
#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);
|
||||||
|
|||||||
@ -2,6 +2,7 @@
|
|||||||
|
|
||||||
can_MCU_cmd mcu_cmd;
|
can_MCU_cmd mcu_cmd;
|
||||||
can_EPS_cmd eps_cmd;
|
can_EPS_cmd eps_cmd;
|
||||||
can_VCU_out1_cmd vcu1_cmd;
|
can_VCU_enable_cmd vcu_enable_cmd;
|
||||||
can_VCU_out2_cmd vcu2_cmd;
|
can_VCU_motor1_cmd vcu_motor1_cmd;
|
||||||
|
can_VCU_motor2_cmd vcu_motor2_cmd;
|
||||||
can_DBS_cmd dbs_cmd;
|
can_DBS_cmd dbs_cmd;
|
||||||
|
|||||||
@ -34,11 +34,14 @@ sweeper_interfaces::msg::McCtrl get_safe_control()
|
|||||||
msg.rpm = 0;
|
msg.rpm = 0;
|
||||||
msg.angle = 0;
|
msg.angle = 0;
|
||||||
msg.angle_speed = 120;
|
msg.angle_speed = 120;
|
||||||
msg.edge_brush_lift = false;
|
|
||||||
msg.sweep_ctrl = false;
|
msg.roll_brush_suction_direction = 0;
|
||||||
msg.spray = false;
|
msg.roll_brush_suction = 0;
|
||||||
msg.mud_flap = false;
|
msg.side_brush = 0;
|
||||||
msg.dust_shake = false;
|
msg.dust_shake = 0;
|
||||||
|
msg.brush_deck_pusher = 0;
|
||||||
|
msg.suction_squeeqee_pusher = 0;
|
||||||
|
msg.water_spray = 0;
|
||||||
}
|
}
|
||||||
return msg;
|
return msg;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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; // 斗升且已放斗
|
|
||||||
}
|
|
||||||
@ -1,7 +1,6 @@
|
|||||||
#include "rclcpp/rclcpp.hpp"
|
#include "rclcpp/rclcpp.hpp"
|
||||||
#include "mc/get_config.h"
|
#include "mc/get_config.h"
|
||||||
#include "mc/can_driver.h"
|
#include "mc/can_driver.h"
|
||||||
#include "mc/dumper.h"
|
|
||||||
#include "mc/can_utils.hpp"
|
#include "mc/can_utils.hpp"
|
||||||
#include "mc/control_cache.hpp"
|
#include "mc/control_cache.hpp"
|
||||||
#include "mc/timer_tasks.hpp"
|
#include "mc/timer_tasks.hpp"
|
||||||
@ -14,7 +13,6 @@
|
|||||||
namespace sweeperMsg = sweeper_interfaces::msg;
|
namespace sweeperMsg = sweeper_interfaces::msg;
|
||||||
|
|
||||||
CANDriver canctl;
|
CANDriver canctl;
|
||||||
DumperController dumper(canctl);
|
|
||||||
std::atomic<bool> vcu_msg_received = false;
|
std::atomic<bool> vcu_msg_received = false;
|
||||||
std::atomic<bool> vcu_awake = false;
|
std::atomic<bool> vcu_awake = false;
|
||||||
rclcpp::Time last_vcu_msg_time;
|
rclcpp::Time last_vcu_msg_time;
|
||||||
@ -22,26 +20,22 @@ rclcpp::Time last_vcu_msg_time;
|
|||||||
void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg)
|
void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg)
|
||||||
{
|
{
|
||||||
std::cout << "Sub ControlMsg:"
|
std::cout << "Sub ControlMsg:"
|
||||||
<< "\n brake: " << static_cast<int>(msg->brake) // uint8
|
<< "\n 电磁刹: " << static_cast<int>(msg->brake)
|
||||||
<< "\n gear: " << static_cast<int>(msg->gear) // uint8
|
<< "\n 挡位: " << static_cast<int>(msg->gear)
|
||||||
<< "\n rpm: " << static_cast<int>(msg->rpm) // uint8
|
<< "\n 转速: " << static_cast<int>(msg->rpm)
|
||||||
<< "\n ehb_enable: " << static_cast<int>(msg->ehb_anable) // bool
|
<< "\n ehb使能: " << static_cast<int>(msg->ehb_anable)
|
||||||
<< "\n ehb_brake_pressure: " << static_cast<float>(msg->ehb_brake_pressure) // float32
|
<< "\n ehb制动压力: " << static_cast<float>(msg->ehb_brake_pressure)
|
||||||
<< "\n angle: " << msg->angle // float32
|
<< "\n 轮端转向角度: " << msg->angle
|
||||||
<< "\n angle_speed: " << msg->angle_speed // uint16
|
<< "\n 转向角速度: " << msg->angle_speed
|
||||||
<< "\n edge_brush_lift: " << static_cast<int>(msg->edge_brush_lift) // bool
|
<< "\n 滚刷&吸风电机方向: " << static_cast<int>(msg->roll_brush_suction_direction)
|
||||||
<< "\n sweep_ctrl: " << static_cast<int>(msg->sweep_ctrl) // bool
|
<< "\n 滚刷&吸风电机: " << static_cast<int>(msg->roll_brush_suction)
|
||||||
<< "\n spray: " << static_cast<int>(msg->spray) // bool
|
<< "\n 边刷电机: " << static_cast<int>(msg->side_brush)
|
||||||
<< "\n mud_flap: " << static_cast<int>(msg->mud_flap) // bool
|
<< "\n 振尘电机: " << static_cast<int>(msg->dust_shake)
|
||||||
<< "\n dust_shake: " << static_cast<int>(msg->dust_shake) // bool
|
<< "\n 刷盘推杆电机: " << static_cast<int>(msg->brush_deck_pusher)
|
||||||
<< "\n left_light: " << static_cast<int>(msg->left_light) // bool
|
<< "\n 吸扒推杆电机: " << static_cast<int>(msg->suction_squeeqee_pusher)
|
||||||
<< "\n right_light: " << static_cast<int>(msg->right_light) // bool
|
<< "\n 喷水电机: " << static_cast<int>(msg->water_spray)
|
||||||
<< "\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::endl;
|
<< std::endl;
|
||||||
|
|
||||||
dumper.setSweeping(msg->sweep_ctrl);
|
|
||||||
control_cache.update(*msg);
|
control_cache.update(*msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -69,7 +63,7 @@ int main(int argc, char **argv)
|
|||||||
context->publisher = pub;
|
context->publisher = pub;
|
||||||
canctl.setReceiveCallback(receiveHandler, context.get());
|
canctl.setReceiveCallback(receiveHandler, context.get());
|
||||||
|
|
||||||
setupTimers(node, dumper, canctl);
|
setupTimers(node, canctl);
|
||||||
|
|
||||||
rclcpp::on_shutdown([&]()
|
rclcpp::on_shutdown([&]()
|
||||||
{ canctl.close(); });
|
{ canctl.close(); });
|
||||||
|
|||||||
@ -17,8 +17,8 @@ void wakeupTimerTask(const rclcpp::Node::SharedPtr &node, CANDriver &canctl)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 定时器回调：VCU watchdog + Dumper Dial处理
|
// 定时器回调：VCU watchdog
|
||||||
void vcuWatchdogTask(const rclcpp::Node::SharedPtr &node, DumperController &dumper)
|
void vcuWatchdogTask(const rclcpp::Node::SharedPtr &node)
|
||||||
{
|
{
|
||||||
auto now = node->now();
|
auto now = node->now();
|
||||||
if (vcu_msg_received.load() && (now - last_vcu_msg_time > rclcpp::Duration::from_seconds(0.5)))
|
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.");
|
RCLCPP_WARN(node->get_logger(), "[VCU] Timeout, resetting state.");
|
||||||
vcu_awake.store(false);
|
vcu_awake.store(false);
|
||||||
vcu_msg_received.store(false);
|
vcu_msg_received.store(false);
|
||||||
dumper.resetAwake();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (vcu_awake.load())
|
// if (vcu_awake.load())
|
||||||
{
|
// {
|
||||||
dumper.tryStartup();
|
// auto msg = get_safe_control();
|
||||||
auto msg = get_safe_control();
|
// }
|
||||||
dumper.updateDial(static_cast<int>(msg.dump), dumper.getSimpleState());
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// 定时器回调：DBS 控制
|
// 定时器回调：DBS 控制
|
||||||
@ -68,51 +65,83 @@ void epsTimerTask(CANDriver &canctl)
|
|||||||
canctl.sendFrame(eps_cmd.toFrame());
|
canctl.sendFrame(eps_cmd.toFrame());
|
||||||
}
|
}
|
||||||
|
|
||||||
// 定时器回调：VCU 灯控、扫地、除尘等功能
|
// 定时器回调：VCU 扫地控制（使用新的报文结构）
|
||||||
void vcuTimerTask(CANDriver &canctl, DumperController &dumper)
|
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();
|
auto msg = get_safe_control();
|
||||||
|
|
||||||
vcu1_cmd.setLeftLight(msg.left_light);
|
// 首次运行时初始化VCU控制
|
||||||
vcu1_cmd.setDustShake(msg.dust_shake);
|
if (!vcu_initialized)
|
||||||
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)
|
|
||||||
{
|
{
|
||||||
// 状态变更（开或关） 或者 满足开始扫地条件，发出指令
|
// 发送CAN使能指令
|
||||||
canctl.sendFrame(vcu1_cmd.toFrame());
|
vcu_enable_cmd.setCANEnable(true);
|
||||||
canctl.sendFrame(vcu2_cmd.toFrame());
|
canctl.sendFrame(vcu_enable_cmd.toFrame());
|
||||||
|
vcu_initialized = true;
|
||||||
last_sweep_ctrl = msg.sweep_ctrl; // 更新上一次状态
|
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(
|
static auto timer_wakeup = node->create_wall_timer(
|
||||||
std::chrono::seconds(1),
|
std::chrono::seconds(1),
|
||||||
[node, &canctl]()
|
[node, &canctl]()
|
||||||
{ wakeupTimerTask(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(
|
static auto timer_dbs = node->create_wall_timer(
|
||||||
std::chrono::milliseconds(10),
|
std::chrono::milliseconds(10),
|
||||||
[&canctl]()
|
[&canctl]()
|
||||||
@ -130,6 +159,13 @@ void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriv
|
|||||||
|
|
||||||
static auto timer_vcu = node->create_wall_timer(
|
static auto timer_vcu = node->create_wall_timer(
|
||||||
std::chrono::milliseconds(100),
|
std::chrono::milliseconds(100),
|
||||||
[&canctl, &dumper]()
|
[&canctl]()
|
||||||
{ vcuTimerTask(canctl, dumper); });
|
{ 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
121
src/mqtt_report/src/VCU_Fault.txt
Normal 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
|
||||||
@ -131,4 +131,119 @@ std::unordered_map<int, FaultInfo> faultMap = {
|
|||||||
{1514, {"DBS", "DBS_MECHANICAL_ERR", "机械故障", 0}},
|
{1514, {"DBS", "DBS_MECHANICAL_ERR", "机械故障", 0}},
|
||||||
{1515, {"DBS", "DBS_OTHER_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}},
|
||||||
|
|
||||||
};
|
};
|
||||||
@ -210,6 +210,90 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg)
|
|||||||
|
|
||||||
break;
|
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;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -62,7 +62,7 @@ private:
|
|||||||
{
|
{
|
||||||
static int MCU_RPM_MAX = config.mcu_rpm_max;
|
static int MCU_RPM_MAX = config.mcu_rpm_max;
|
||||||
static float EPS_ANGLE_MAX = config.eps_angle_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(); // 获取数据安全性
|
bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性
|
||||||
|
|
||||||
@ -82,39 +82,8 @@ private:
|
|||||||
uint16_t ctrl = ch_data[4]; // 手刹
|
uint16_t ctrl = ch_data[4]; // 手刹
|
||||||
uint16_t gear = ch_data[5]; // 挡位
|
uint16_t gear = ch_data[5]; // 挡位
|
||||||
uint16_t sweep = ch_data[6]; // 清扫
|
uint16_t sweep = ch_data[6]; // 清扫
|
||||||
uint16_t dump = ch_data[7]; // 垃圾倾倒
|
|
||||||
int16_t speed = ch_data[1] - 992; //[-800,800]
|
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 uint16_t last_ctrl = 992; // 初始手刹值
|
||||||
static int current_brake = 1; // 默认上电是拉手刹（1）
|
static int current_brake = 1; // 默认上电是拉手刹（1）
|
||||||
|
|
||||||
@ -163,19 +132,23 @@ private:
|
|||||||
// 一键清扫
|
// 一键清扫
|
||||||
if (sweep == 1792)
|
if (sweep == 1792)
|
||||||
{
|
{
|
||||||
msg.edge_brush_lift = false;
|
msg.roll_brush_suction_direction = 0;
|
||||||
msg.sweep_ctrl = false;
|
msg.roll_brush_suction = 0;
|
||||||
msg.spray = false;
|
msg.side_brush = 0;
|
||||||
msg.mud_flap = false;
|
msg.dust_shake = 0;
|
||||||
msg.dust_shake = false;
|
msg.brush_deck_pusher = 0;
|
||||||
|
msg.suction_squeeqee_pusher = 0;
|
||||||
|
msg.water_spray = 0;
|
||||||
}
|
}
|
||||||
else if (sweep == 192)
|
else if (sweep == 192)
|
||||||
{
|
{
|
||||||
msg.edge_brush_lift = true;
|
msg.roll_brush_suction_direction = 1;
|
||||||
msg.sweep_ctrl = true;
|
msg.roll_brush_suction = 100;
|
||||||
msg.spray = true;
|
msg.side_brush = 100;
|
||||||
msg.mud_flap = true;
|
msg.dust_shake = 100;
|
||||||
msg.dust_shake = true;
|
msg.brush_deck_pusher = 100;
|
||||||
|
msg.suction_squeeqee_pusher = 100;
|
||||||
|
msg.water_spray = 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 转向逻辑
|
// 转向逻辑
|
||||||
@ -203,35 +176,35 @@ private:
|
|||||||
msg.rpm = 0;
|
msg.rpm = 0;
|
||||||
msg.angle = 0;
|
msg.angle = 0;
|
||||||
msg.angle_speed = 120;
|
msg.angle_speed = 120;
|
||||||
msg.edge_brush_lift = false;
|
|
||||||
msg.sweep_ctrl = false;
|
msg.roll_brush_suction_direction = 0;
|
||||||
msg.spray = false;
|
msg.roll_brush_suction = 0;
|
||||||
msg.mud_flap = false;
|
msg.side_brush = 0;
|
||||||
msg.dust_shake = false;
|
msg.dust_shake = 0;
|
||||||
|
msg.brush_deck_pusher = 0;
|
||||||
|
msg.suction_squeeqee_pusher = 0;
|
||||||
|
msg.water_spray = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 发布控制消息
|
// 发布控制消息
|
||||||
pub_->publish(msg);
|
pub_->publish(msg);
|
||||||
|
|
||||||
RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:"
|
std::cout << "Sub ControlMsg:"
|
||||||
<< "\n brake: " << static_cast<int>(msg.brake) // uint8
|
<< "\n 电磁刹: " << static_cast<int>(msg.brake)
|
||||||
<< "\n gear: " << static_cast<int>(msg.gear) // uint8
|
<< "\n 挡位: " << static_cast<int>(msg.gear)
|
||||||
<< "\n rpm: " << static_cast<int>(msg.rpm) // uint8
|
<< "\n 转速: " << static_cast<int>(msg.rpm)
|
||||||
<< "\n ehb_enable: " << static_cast<int>(msg.ehb_anable) // bool
|
<< "\n ehb使能: " << static_cast<int>(msg.ehb_anable)
|
||||||
<< "\n ehb_brake_pressure: " << static_cast<float>(msg.ehb_brake_pressure) // float32
|
<< "\n ehb制动压力: " << static_cast<float>(msg.ehb_brake_pressure)
|
||||||
<< "\n angle: " << msg.angle // float32
|
<< "\n 轮端转向角度: " << msg.angle
|
||||||
<< "\n angle_speed: " << msg.angle_speed // uint16
|
<< "\n 转向角速度: " << msg.angle_speed
|
||||||
<< "\n edge_brush_lift: " << static_cast<int>(msg.edge_brush_lift) // bool
|
<< "\n 滚刷&吸风电机方向: " << static_cast<int>(msg.roll_brush_suction_direction)
|
||||||
<< "\n sweep_ctrl: " << static_cast<int>(msg.sweep_ctrl) // bool
|
<< "\n 滚刷&吸风电机: " << static_cast<int>(msg.roll_brush_suction)
|
||||||
<< "\n spray: " << static_cast<int>(msg.spray) // bool
|
<< "\n 边刷电机: " << static_cast<int>(msg.side_brush)
|
||||||
<< "\n mud_flap: " << static_cast<int>(msg.mud_flap) // bool
|
<< "\n 振尘电机: " << static_cast<int>(msg.dust_shake)
|
||||||
<< "\n dust_shake: " << static_cast<int>(msg.dust_shake) // bool
|
<< "\n 刷盘推杆电机: " << static_cast<int>(msg.brush_deck_pusher)
|
||||||
<< "\n left_light: " << static_cast<int>(msg.left_light) // bool
|
<< "\n 吸扒推杆电机: " << static_cast<int>(msg.suction_squeeqee_pusher)
|
||||||
<< "\n right_light: " << static_cast<int>(msg.right_light) // bool
|
<< "\n 喷水电机: " << static_cast<int>(msg.water_spray)
|
||||||
<< "\n brake_light: " << static_cast<int>(msg.brake_light) // bool
|
<< std::endl;
|
||||||
<< "\n headlight: " << static_cast<int>(msg.headlight) // bool
|
|
||||||
<< "\n dump: " << static_cast<int>(msg.dump) // uint8
|
|
||||||
);
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
|||||||
@ -14,16 +14,10 @@ float32 angle #轮端转向角度 分辨率0.2° [-66.0,66.0] 适当缩减
|
|||||||
uint16 angle_speed #转向角速度 120-1500rpm
|
uint16 angle_speed #转向角速度 120-1500rpm
|
||||||
|
|
||||||
#vcu部分
|
#vcu部分
|
||||||
bool edge_brush_lift #边刷推杆
|
uint8 roll_brush_suction_direction #滚刷&吸风电机方向 0：不转动 1：正转 2：反转
|
||||||
bool sweep_ctrl #扫地控制
|
uint8 roll_brush_suction #滚刷&吸风电机 0 ~ 100
|
||||||
bool spray #喷水
|
int8 side_brush #边刷电机 0 ~ 100
|
||||||
bool mud_flap #挡皮
|
int8 dust_shake #振尘电机 0 ~ 100
|
||||||
bool dust_shake #振尘
|
int8 brush_deck_pusher #刷盘推杆电机 -100 ~ 100
|
||||||
|
int8 suction_squeeqee_pusher #吸扒推杆电机 -100 ~ 100
|
||||||
bool left_light #左转向灯
|
int8 water_spray #喷水电机 -100 ~ 100
|
||||||
bool right_light #右转向灯
|
|
||||||
bool brake_light #刹车灯
|
|
||||||
bool headlight #大灯
|
|
||||||
|
|
||||||
#倒灰
|
|
||||||
uint8 dump # 0 全部收回 1 斗升 2 倒斗
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user