cxh/sweeper_e800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Auto commit at 2025-06-18 16:59:19

Browse Source
This commit is contained in:
cxh 2025-06-18 16:59:19 +08:00
parent 624c7d1ea2
commit bffaa1f396
2 changed files with 113 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@ -7,83 +7,122 @@ extern std::atomic<bool> vcu_awake;
extern std::atomic<bool> vcu_msg_received;
extern rclcpp::Time last_vcu_msg_time;
// 定时器回调：VCU 唤醒
void wakeupTimerTask(const rclcpp::Node::SharedPtr &node, CANDriver &canctl)
{
if (!vcu_awake.load())
{
RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
VCUWakeUp(canctl);
}
}
// 定时器回调：VCU watchdog + Dumper Dial处理
void vcuWatchdogTask(const rclcpp::Node::SharedPtr &node, DumperController &dumper)
{
auto now = node->now();
if (vcu_msg_received.load() && (now - last_vcu_msg_time > rclcpp::Duration::from_seconds(0.5)))
{
RCLCPP_WARN(node->get_logger(), "[VCU] Timeout, resetting state.");
vcu_awake.store(false);
vcu_msg_received.store(false);
dumper.resetAwake();
}
if (vcu_awake.load())
{
dumper.tryStartup();
auto msg = get_safe_control();
dumper.updateDial(static_cast<int>(msg.dump), dumper.getSimpleState());
}
}
// 定时器回调：DBS 控制
void dbsTimerTask(CANDriver &canctl)
{
auto msg = get_safe_control();
dbs_cmd.setEnabled(msg.ehb_anable);
dbs_cmd.setPressure(msg.ehb_brake_pressure);
canctl.sendFrame(dbs_cmd.toFrame());
}
// 定时器回调：MCU 控制
void mcuTimerTask(CANDriver &canctl)
{
auto msg = get_safe_control();
mcu_cmd.setEnabled(true);
mcu_cmd.setGear(msg.gear);
mcu_cmd.setRPM(msg.rpm);
mcu_cmd.setBrake(msg.brake);
canctl.sendFrame(mcu_cmd.toFrame());
}
// 定时器回调：EPS 控制
void epsTimerTask(CANDriver &canctl)
{
auto msg = get_safe_control();
eps_cmd.setCenterCmd(0);
eps_cmd.setAngle(msg.angle);
eps_cmd.setAngularSpeed(msg.angle_speed);
eps_cmd.pack();
canctl.sendFrame(eps_cmd.toFrame());
}
// 定时器回调：VCU 灯控、扫地、除尘等功能
void vcuTimerTask(CANDriver &canctl, DumperController &dumper)
{
auto msg = get_safe_control();
vcu1_cmd.setLeftLight(msg.left_light);
vcu1_cmd.setDustShake(msg.dust_shake);
vcu1_cmd.setHeadLight(msg.headlight);
vcu2_cmd.setBrakeLight(msg.brake_light);
vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift);
vcu2_cmd.setHeadlight(msg.headlight);
vcu2_cmd.setMudFlap(msg.mud_flap);
vcu2_cmd.setRightLight(msg.right_light);
vcu2_cmd.setSpray(msg.spray);
vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl);
// 只有在扫地中并且斗处于收斗状态时才发VCU指令
if (msg.sweep_ctrl && dumper.getSimpleState() == 0)
{
canctl.sendFrame(vcu1_cmd.toFrame());
canctl.sendFrame(vcu2_cmd.toFrame());
}
}
// 注册所有定时器任务
void setupTimers(rclcpp::Node::SharedPtr node, DumperController &dumper, CANDriver &canctl)
{
static auto timer_wakeup = node->create_wall_timer(
std::chrono::seconds(1), [node, &canctl]()
{
if (!vcu_awake.load())
{
RCLCPP_INFO(node->get_logger(), "[TIMER][VCU] Not awake, sending wake-up frame...");
VCUWakeUp(canctl);
} });
static auto timer_wakeup = node->create_wall_timer(
std::chrono::seconds(1),
[node, &canctl]()
{ wakeupTimerTask(node, canctl); });
static auto timer_watchdog = node->create_wall_timer(
std::chrono::milliseconds(200), [node, &dumper]()
{
auto now = node->now();
if (vcu_msg_received.load() && (now - last_vcu_msg_time > rclcpp::Duration::from_seconds(0.5)))
{
RCLCPP_WARN(node->get_logger(), "[VCU] Timeout, resetting state.");
vcu_awake.store(false);
vcu_msg_received.store(false);
dumper.resetAwake();
}
static auto timer_watchdog = node->create_wall_timer(
std::chrono::milliseconds(200),
[node, &dumper]()
{ vcuWatchdogTask(node, dumper); });
if (vcu_awake.load())
{
dumper.tryStartup();
auto msg = get_safe_control();
dumper.updateDial(static_cast<int>(msg.dump), dumper.getSimpleState());
} });
static auto timer_dbs = node->create_wall_timer(
std::chrono::milliseconds(10),
[&canctl]()
{ dbsTimerTask(canctl); });
static auto timer_dbs = node->create_wall_timer(
std::chrono::milliseconds(10), [&canctl]()
{
auto msg = get_safe_control();
dbs_cmd.setEnabled(msg.ehb_anable);
dbs_cmd.setPressure(msg.ehb_brake_pressure);
canctl.sendFrame(dbs_cmd.toFrame()); });
static auto timer_mcu = node->create_wall_timer(
std::chrono::milliseconds(20),
[&canctl]()
{ mcuTimerTask(canctl); });
static auto timer_mcu = node->create_wall_timer(
std::chrono::milliseconds(20), [&canctl]()
{
auto msg = get_safe_control();
mcu_cmd.setEnabled(true);
mcu_cmd.setGear(msg.gear);
mcu_cmd.setRPM(msg.rpm);
mcu_cmd.setBrake(msg.brake);
canctl.sendFrame(mcu_cmd.toFrame()); });
static auto timer_eps = node->create_wall_timer(
std::chrono::milliseconds(50),
[&canctl]()
{ epsTimerTask(canctl); });
static auto timer_eps = node->create_wall_timer(
std::chrono::milliseconds(50), [&canctl]()
{
auto msg = get_safe_control();
eps_cmd.setCenterCmd(0);
eps_cmd.setAngle(msg.angle);
eps_cmd.setAngularSpeed(msg.angle_speed);
eps_cmd.pack();
canctl.sendFrame(eps_cmd.toFrame()); });
static auto timer_vcu = node->create_wall_timer(
std::chrono::milliseconds(100), [&canctl, &dumper]()
{
auto msg = get_safe_control();
vcu1_cmd.setLeftLight(msg.left_light);
vcu1_cmd.setDustShake(msg.dust_shake);
vcu1_cmd.setHeadLight(msg.headlight);
vcu2_cmd.setBrakeLight(msg.brake_light);
vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift);
vcu2_cmd.setHeadlight(msg.headlight);
vcu2_cmd.setMudFlap(msg.mud_flap);
vcu2_cmd.setRightLight(msg.right_light);
vcu2_cmd.setSpray(msg.spray);
vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl);
if (msg.sweep_ctrl && dumper.getSimpleState() == 0)
{
canctl.sendFrame(vcu1_cmd.toFrame());
canctl.sendFrame(vcu2_cmd.toFrame());
} });
}
static auto timer_vcu = node->create_wall_timer(
std::chrono::milliseconds(100),
[&canctl, &dumper]()
{ vcuTimerTask(canctl, dumper); });
}