Auto commit at 2025-06-06 15:33:11

src/radio_ctrl/include/radio_ctrl/uart_handler.h

@@ -4,11 +4,14 @@
 #include <thread>
 #include <atomic>
 #include <string>
+#include <cstring>
 #include <iostream>
 #include <fcntl.h>
 #include <unistd.h>
 #include <errno.h>
 #include <termios.h>
+#include <vector>
+#include <algorithm>
 
 class UartHandler
 {
@@ -44,10 +47,10 @@ private:
     std::atomic<bool> reading; // 控制读取线程的状态
     std::thread read_thread;   // 读取数据的线程
 
-    void read_loop();                       // 持续读取串口数据
-    void parse_data(uint8_t *buf, int len); // 解析数据
-    int sbus_parse();                       // SBUS 数据解析
-    void print_hex(uint8_t *buf, int len);  // 打印数据（调试用）
+    void read_loop();                              // 持续读取串口数据
+    void parse_data(std::vector<uint8_t> &buffer); // 解析数据
+    int sbus_parse();                              // SBUS 数据解析
+    void print_hex(uint8_t *buf, int len);         // 打印数据（调试用）
 };
 
 #endif // UART_HANDLER_H

src/radio_ctrl/src/uart_handler.cpp

@@ -107,29 +107,25 @@ void UartHandler::stop_reading()
 
 void UartHandler::read_loop()
 {
-    uint8_t buf[512];
+    std::vector<uint8_t> buffer;
+    uint8_t temp[512];
+
     while (reading)
     {
-        int n = read(fd, buf, sizeof(buf));
+        int n = read(fd, temp, sizeof(temp));
         if (n > 0)
         {
-            parse_data(buf, n);
+            buffer.insert(buffer.end(), temp, temp + n);
+            parse_data(buffer);
         }
-        else if (n == 0)
+        else if (n == 0 || errno == EAGAIN)
         {
             usleep(1000);
         }
         else
         {
-            if (errno == EAGAIN)
-            {
-                usleep(1000);
-            }
-            else
-            {
-                perror("read");
-                break;
-            }
+            perror("read");
+            break;
         }
     }
 }
@@ -143,36 +139,35 @@ void UartHandler::print_hex(uint8_t *buf, int len)
     printf("\n");
 }
 
-void UartHandler::parse_data(uint8_t *buf, int len)
+void UartHandler::parse_data(std::vector<uint8_t> &buffer)
 {
-    // 收到一帧完整的25字节数据
-    if (len >= kSbusFrameLength)
+    while (buffer.size() >= kSbusFrameLength)
     {
-        memcpy(sbus_data, buf, kSbusFrameLength);
-        int status = sbus_parse();
-
-        // 打印通道值
-        // std::cout << "Parsed Channels:\n";
-        // for (int i = 0; i < 8; ++i)
-        // {
-        //     std::cout << "CH" << i + 1 << ": " << sbus_channels[i] << " ";
-        //     if ((i + 1) % 8 == 0)
-        //         std::cout << "\n"; // 每8个换行
-        // }
-
-        if (status == SBUS_SIGNAL_LOST)
+        // 寻找帧头（SBUS 通常帧头是 0x0F）
+        auto it = std::find(buffer.begin(), buffer.end(), 0x0F);
+        if (it == buffer.end())
         {
-            // std::cout << "[Warning] SBUS Signal Lost!\n";
+            buffer.clear(); // 无帧头，清空
+            failsafe_status = SBUS_SIGNAL_LOST;
+            return;
         }
-        else if (status == SBUS_SIGNAL_FAILSAFE)
+
+        // 如果帧头之后数据不够 25 字节，等待下次接收
+        size_t index = std::distance(buffer.begin(), it);
+        if (buffer.size() - index < kSbusFrameLength)
         {
-            // std::cout << "[Warning] SBUS Failsafe!\n";
+            // 保留还没凑够的部分
+            if (index > 0)
+                buffer.erase(buffer.begin(), buffer.begin() + index); // 移除前面无用部分
+            return;
         }
-    }
-    else
-    {
-        std::cerr << "Received data too short to parse.\n";
-        failsafe_status = SBUS_SIGNAL_LOST;
+
+        // 拷贝并解析数据
+        std::memcpy(sbus_data, &buffer[index], kSbusFrameLength);
+        sbus_parse();
+
+        // 移除已处理数据
+        buffer.erase(buffer.begin(), buffer.begin() + index + kSbusFrameLength);
     }
 }