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

修复遥控接收器松了还在发遥控指令BUG

Browse Source
This commit is contained in:
lyq 2026-02-02 17:35:28 +08:00
parent 3cdea52783
commit a8ed715631
3 changed files with 33 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/control/radio_ctrl/include/radio_ctrl/uart_handler.h
View File

@ -25,6 +25,7 @@ class UartHandler
void stop_reading(); // 停止读取数据的线程
int get_channel_value(int channel); // 获取指定通道的数据
bool get_data_safe(); // 获取数据安全性
bool has_received_data() const; // 检查是否已接收到至少一帧有效数据
private:
std::string serial_device;
@ -46,6 +47,7 @@ class UartHandler
};
std::atomic<bool> reading; // 控制读取线程的状态
std::atomic<bool> has_received_data_flag; // 标志是否已接收到至少一帧有效数据
std::thread read_thread; // 读取数据的线程
void read_loop(); // 持续读取串口数据

18
src/control/radio_ctrl/src/radio_ctrl.cpp
View File

@ -64,11 +64,12 @@ class SBUSNode : public rclcpp::Node
static float EPS_ANGLE_MAX = config.eps_angle_max;
bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性
bool has_received = uart_handler_->has_received_data(); // 检查是否已接收到过数据
auto msg = sweeperMsg::McCtrl(); // 控制消息对象
uint16_t ch_data[10]; // 各通道遥控数据
if (data_safe) // 数据安全，进行数据解析并发布
if (has_received && data_safe) // 已接收到数据且数据安全，进行数据解析并发布
{
// 赋值与打印（注释掉原有的高频打印）
for (int i = 0; i < 10; ++i)
@ -169,12 +170,25 @@ class SBUSNode : public rclcpp::Node
}
else
{
// 低频率打印等待信息（每2次回调打印一次，避免刷屏）
// 未接收到数据或数据不安全
if (!has_received)
{
// 未接收到过任何数据
if (++print_counter >= PRINT_INTERVAL / 2)
{
LOG_INFO("Waiting for radio control data...");
print_counter = 0;
}
}
else
{
// 已接收到过数据但现在数据不安全
if (++print_counter >= PRINT_INTERVAL / 2)
{
LOG_WARN("Radio control data lost or failsafe!");
print_counter = 0;
}
}
return;
}
}

9
src/control/radio_ctrl/src/uart_handler.cpp
View File

@ -34,7 +34,11 @@ struct termios2
// 构造函数
UartHandler::UartHandler(const std::string& device, int baudrate)
: serial_device(device), baudrate(baudrate), fd(-1), failsafe_status(SBUS_SIGNAL_LOST)
: serial_device(device),
baudrate(baudrate),
fd(-1),
failsafe_status(SBUS_SIGNAL_LOST),
has_received_data_flag(false)
{
std::fill(std::begin(sbus_channels), std::end(sbus_channels), 0); // 初始化通道数据
}
@ -183,6 +187,7 @@ void UartHandler::parse_data(std::vector<uint8_t>& buffer)
int UartHandler::sbus_parse()
{
has_received_data_flag = true; // 标记已接收到有效数据
sbus_channels[0] = ((sbus_data[1] | sbus_data[2] << 8) & 0x07FF);
sbus_channels[1] = ((sbus_data[2] >> 3 | sbus_data[3] << 5) & 0x07FF);
sbus_channels[2] = ((sbus_data[3] >> 6 | sbus_data[4] << 2 | sbus_data[5] << 10) & 0x07FF);
@ -229,3 +234,5 @@ bool UartHandler::get_data_safe()
else
return false;
}
bool UartHandler::has_received_data() const { return has_received_data_flag; }