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

优化遥控的逻辑

Browse Source
This commit is contained in:
root 2025-05-23 17:09:04 +08:00
parent 9cce7f25a0
commit 268e4e125f
2 changed files with 27 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/radio_ctrl/src/radio_ctrl.cpp
View File

@ -62,15 +62,15 @@ private:
auto msg = mc::msg::McCtrl(); // 控制消息对象
// 获取各通道遥控数据
uint16_t ch_data[8];
uint16_t ch_data[8] = {};
// 获取数据安全性
bool data_safe = uart_handler_->get_data_safe();
for (int i = 0; i < 8; ++i)
{
ch_data[i] = uart_handler_->get_channel_value(i);
printf("ch[%d]:%d ", i, ch_data[i]);
}
printf("\n");
// for (int i = 0; i < 8; ++i)
// {
// ch_data[i] = uart_handler_->get_channel_value(i);
// printf("ch[%d]:%d ", i, ch_data[i]);
// }
// printf("\n");
// 是否使能车辆控制
if (ch_data[6] == 192)
{
@ -174,30 +174,6 @@ private:
if (data_safe)
{
RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:"
<< "\n mcu_enabled: " << msg.mcu_enabled
<< "\n brake: " << static_cast<int>(msg.brake)
<< "\n gear: " << static_cast<int>(msg.gear)
<< "\n rpm: " << msg.rpm
<< "\n brake_time_ms: " << msg.brake_time_ms
<< "\n angle: " << msg.angle
<< "\n angle_speed: " << msg.angle_speed
<< "\n main_brush_lift: " << msg.main_brush_lift
<< "\n edge_brush_lift: " << msg.edge_brush_lift
<< "\n vacuum: " << msg.vacuum
<< "\n spray: " << msg.spray
<< "\n mud_flap: " << msg.mud_flap
<< "\n dust_shake: " << msg.dust_shake
<< "\n left_light: " << msg.left_light
<< "\n right_light: " << msg.right_light
<< "\n night_light: " << msg.night_light
<< "\n brake_light: " << msg.brake_light
<< "\n headlight: " << msg.headlight
<< "\n main_brush_spin: " << msg.main_brush_spin
<< "\n edge_brush_spin: " << msg.edge_brush_spin
<< "\n main_brush_pwm: " << static_cast<int>(msg.main_brush_pwm)
<< "\n edge_brush_pwm: " << static_cast<int>(msg.edge_brush_pwm));
pub_->publish(msg); // 发布控制消息
}
}

35
src/radio_ctrl/src/uart_handler.cpp
View File

@ -30,7 +30,7 @@ struct termios2
// 构造函数
UartHandler::UartHandler(const std::string &device, int baudrate)
: serial_device(device), baudrate(baudrate), fd(-1), failsafe_status(SBUS_SIGNAL_OK)
: serial_device(device), baudrate(baudrate), fd(-1), failsafe_status(SBUS_SIGNAL_LOST)
{
std::fill(std::begin(sbus_channels), std::end(sbus_channels), 0); // 初始化通道数据
}
@ -108,28 +108,32 @@ void UartHandler::stop_reading()
void UartHandler::read_loop()
{
uint8_t buf[512];
auto last_receive_time = std::chrono::steady_clock::now();
while (reading)
{
int n = read(fd, buf, sizeof(buf));
if (n > 0)
{
last_receive_time = std::chrono::steady_clock::now();
parse_data(buf, n);
}
else if (n == 0)
else if (n == 0 || (n < 0 && errno == EAGAIN))
{
// 检查超时
auto now = std::chrono::steady_clock::now();
if (std::chrono::duration_cast<std::chrono::seconds>(now - last_receive_time).count() > 2) // 超过2秒没数据
{
failsafe_status = SBUS_SIGNAL_LOST;
}
usleep(1000);
}
else
{
if (errno == EAGAIN)
{
usleep(1000);
}
else
{
perror("read");
break;
}
perror("read error, but will continue");
// 不退出，短暂休眠后继续尝试
usleep(1000);
continue;
}
}
}
@ -160,13 +164,14 @@ void UartHandler::parse_data(uint8_t *buf, int len)
// std::cout << "\n"; // 每8个换行
// }
if (status == SBUS_SIGNAL_LOST)
// 只有状态是 OK，才更新 failsafe_status
if (status == SBUS_SIGNAL_OK)
{
std::cout << "[Warning] SBUS Signal Lost!\n";
failsafe_status = SBUS_SIGNAL_OK;
}
else if (status == SBUS_SIGNAL_FAILSAFE)
else
{
std::cout << "[Warning] SBUS Failsafe!\n";
failsafe_status = status; // 丢失或故障都记录下来
}
}
else