From a8ed7156316a5ba9520e6dcbb196775171990e1e Mon Sep 17 00:00:00 2001 From: lyq Date: Mon, 2 Feb 2026 17:35:28 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E9=81=A5=E6=8E=A7=E6=8E=A5?= =?UTF-8?q?=E6=94=B6=E5=99=A8=E6=9D=BE=E4=BA=86=E8=BF=98=E5=9C=A8=E5=8F=91?= =?UTF-8?q?=E9=81=A5=E6=8E=A7=E6=8C=87=E4=BB=A4BUG?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/radio_ctrl/uart_handler.h | 6 +++-- src/control/radio_ctrl/src/radio_ctrl.cpp | 26 ++++++++++++++----- src/control/radio_ctrl/src/uart_handler.cpp | 11 ++++++-- 3 files changed, 33 insertions(+), 10 deletions(-) diff --git a/src/control/radio_ctrl/include/radio_ctrl/uart_handler.h b/src/control/radio_ctrl/include/radio_ctrl/uart_handler.h index 140e98d..e8efecc 100644 --- a/src/control/radio_ctrl/include/radio_ctrl/uart_handler.h +++ b/src/control/radio_ctrl/include/radio_ctrl/uart_handler.h @@ -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; @@ -45,8 +46,9 @@ class UartHandler SBUS_SIGNAL_OK }; - std::atomic reading; // 控制读取线程的状态 - std::thread read_thread; // 读取数据的线程 + std::atomic reading; // 控制读取线程的状态 + std::atomic has_received_data_flag; // 标志是否已接收到至少一帧有效数据 + std::thread read_thread; // 读取数据的线程 void read_loop(); // 持续读取串口数据 void parse_data(std::vector& buffer); // 解析数据 diff --git a/src/control/radio_ctrl/src/radio_ctrl.cpp b/src/control/radio_ctrl/src/radio_ctrl.cpp index 228d814..9f6cf7b 100644 --- a/src/control/radio_ctrl/src/radio_ctrl.cpp +++ b/src/control/radio_ctrl/src/radio_ctrl.cpp @@ -63,12 +63,13 @@ class SBUSNode : public rclcpp::Node static int MCU_RPM_MAX = config.mcu_rpm_max; static float EPS_ANGLE_MAX = config.eps_angle_max; - bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性 + 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,11 +170,24 @@ class SBUSNode : public rclcpp::Node } else { - // 低频率打印等待信息（每2次回调打印一次，避免刷屏） - if (++print_counter >= PRINT_INTERVAL / 2) + // 未接收到数据或数据不安全 + if (!has_received) { - LOG_INFO("Waiting for radio control data..."); - print_counter = 0; + // 未接收到过任何数据 + 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; } diff --git a/src/control/radio_ctrl/src/uart_handler.cpp b/src/control/radio_ctrl/src/uart_handler.cpp index 0228131..6f7bb6e 100644 --- a/src/control/radio_ctrl/src/uart_handler.cpp +++ b/src/control/radio_ctrl/src/uart_handler.cpp @@ -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& 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); @@ -228,4 +233,6 @@ bool UartHandler::get_data_safe() return true; else return false; -} \ No newline at end of file +} + +bool UartHandler::has_received_data() const { return has_received_data_flag; } \ No newline at end of file