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

src/autonomy/fu/src/fu_node.cpp

@@ -950,6 +950,19 @@ class fu_node : public rclcpp::Node
                 return;
             }
 
+            // 若之前处于WAITING状态，且现在没有障碍物且RTK信号良好，恢复到NORMAL状态
+            auto now = system_clock::now();
+            duration<double> time_since_last_rtk = now - last_rtk_time;
+            bool rtk_signal_good = (g_rtk.reliability != 0 && time_since_last_rtk <= rtk_timeout);
+
+            if (state_machine_->getCurrentState() == StateMachine::WAITING &&
+                !state_machine_->obstacle_analysis.has_front_critical &&
+                rtk_signal_good)
+            {
+                state_machine_->setState(StateMachine::NORMAL);
+                LOG_INFO("[状态恢复] 障碍物已消失且RTK信号良好，恢复正常行驶状态");
+            }
+
             // 绕障逻辑
             if (enable_obstacle_avoid_)
             {

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;
@@ -46,6 +47,7 @@ class UartHandler
     };
 
     std::atomic<bool> reading;                 // 控制读取线程的状态
+    std::atomic<bool> has_received_data_flag;  // 标志是否已接收到至少一帧有效数据
     std::thread read_thread;                   // 读取数据的线程
 
     void read_loop();                               // 持续读取串口数据

src/control/radio_ctrl/src/radio_ctrl.cpp

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

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