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

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<bool> reading;  // 控制读取线程的状态
+    std::atomic<bool> reading;                 // 控制读取线程的状态
-    std::thread read_thread;    // 读取数据的线程
+    std::atomic<bool> has_received_data_flag;  // 标志是否已接收到至少一帧有效数据
+    std::thread read_thread;                   // 读取数据的线程
 
     void read_loop();                               // 持续读取串口数据
     void parse_data(std::vector<uint8_t>& buffer);  // 解析数据

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;
         }

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<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; }