第一版数据接收

armor/src/armor_finder/send_target/send_target.cpp

@@ -34,14 +34,111 @@ struct ControlData {
     uint8_t right_switch = 3;
 };
 
+// 接收的传感器数据
+struct SensorData {
+    float gyro_x = 0.0f;   // 陀螺仪x
+    float accel_x = 0.0f;  // 加速度计x
+    float accel_y = 0.0f;  // 加速度计y
+    float accel_z = 0.0f;  // 加速度计z
+    bool valid = false;
+};
+
 static std::mutex control_mutex;
 static ControlData shared_control;
+static std::mutex sensor_mutex;
+static SensorData shared_sensor;
 static std::atomic<bool> sender_running{false};
+static std::atomic<bool> monitor_running{false};
 static std::thread sender_thread;
+static std::thread monitor_thread;
+
+// 解析接收到的数据，从杆量位置解算出4个float
+// 协议格式：| 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE |
+// 映射关系：x_move -> gyro_x, y_move -> accel_x, yaw -> accel_y, pitch -> accel_z
+bool parseSensorData(const uint8_t* data, SensorData& sensor) {
+    // 检查帧头帧尾
+    if (data[0] != FRAME_HEADER_1 || data[1] != FRAME_HEADER_2 || data[14] != FRAME_TAIL) {
+        return false;
+    }
+    
+    // 解析4个杆量值（小端序 int16）
+    int16_t x_move = data[2] | (data[3] << 8);
+    int16_t y_move = data[4] | (data[5] << 8);
+    int16_t yaw_val = data[6] | (data[7] << 8);
+    int16_t pitch_val = data[8] | (data[9] << 8);
+    
+    // 转换为float（假设原始数据是放大了100倍的）
+    sensor.gyro_x = x_move / 100.0f;
+    sensor.accel_x = y_move / 100.0f;
+    sensor.accel_y = yaw_val / 100.0f;
+    sensor.accel_z = pitch_val / 100.0f;
+    sensor.valid = true;
+    
+    return true;
+}
+
+// 获取传感器数据接口
+bool getSensorData(SensorData& sensor) {
+    std::lock_guard<std::mutex> lock(sensor_mutex);
+    if (shared_sensor.valid) {
+        sensor = shared_sensor;
+        return true;
+    }
+    return false;
+}
+
+// 更新控制数据
+void updateControl(int16_t yaw, int16_t pitch, int16_t feed, 
+                   uint8_t left_sw = 3, uint8_t right_sw = 3) {
+    std::lock_guard<std::mutex> lock(control_mutex);
+    shared_control.yaw = yaw;
+    shared_control.pitch = pitch;
+    shared_control.feed = feed;
+    shared_control.left_switch = left_sw;
+    shared_control.right_switch = right_sw;
+}
+
+// 传感器监控线程 - 独立打印传感器数据
+void sensorMonitorLoop() {
+    LOGM(STR_CTR(WORD_GREEN, "Sensor monitor thread started"));
+    
+    while (monitor_running) {
+        SensorData sensor;
+        if (getSensorData(sensor)) {
+            // 每秒打印一次传感器数据
+            LOGM(STR_CTR(WORD_LIGHT_PURPLE, "[Monitor] gyro_x=%.3f, accel_x=%.3f, accel_y=%.3f, accel_z=%.3f"),
+                 sensor.gyro_x, sensor.accel_x, sensor.accel_y, sensor.accel_z);
+        }
+        
+        // 每秒打印一次
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+    
+    LOGM(STR_CTR(WORD_YELLOW, "Sensor monitor thread stopped"));
+}
+
+// 启动传感器监控线程
+void startSensorMonitor() {
+    if (!monitor_running) {
+        monitor_running = true;
+        monitor_thread = std::thread(sensorMonitorLoop);
+    }
+}
+
+// 停止传感器监控线程
+void stopSensorMonitor() {
+    if (monitor_running) {
+        monitor_running = false;
+        if (monitor_thread.joinable()) {
+            monitor_thread.join();
+        }
+    }
+}
 
 // 发送线程函数
 void senderLoop(Serial &serial) {
-    uint8_t frame[FRAME_LENGTH];
+    uint8_t tx_frame[FRAME_LENGTH];
+    uint8_t rx_frame[FRAME_LENGTH];
     
     while (sender_running) {
         auto start = std::chrono::steady_clock::now();
@@ -53,40 +150,49 @@ void senderLoop(Serial &serial) {
             local_control = shared_control;
         }
         
-        // 构建数据帧
+        // 构建发送数据帧
         int idx = 0;
-        frame[idx++] = FRAME_HEADER_1;
+        tx_frame[idx++] = FRAME_HEADER_1;
-        frame[idx++] = FRAME_HEADER_2;
+        tx_frame[idx++] = FRAME_HEADER_2;
         
         // x_move, y_move (固定为0)
-        frame[idx++] = 0;
+        tx_frame[idx++] = 0;
-        frame[idx++] = 0;
+        tx_frame[idx++] = 0;
-        frame[idx++] = 0;
+        tx_frame[idx++] = 0;
-        frame[idx++] = 0;
+        tx_frame[idx++] = 0;
         
         // yaw (小端序)
-        frame[idx++] = local_control.yaw & 0xFF;
+        tx_frame[idx++] = local_control.yaw & 0xFF;
-        frame[idx++] = (local_control.yaw >> 8) & 0xFF;
+        tx_frame[idx++] = (local_control.yaw >> 8) & 0xFF;
         
         // pitch (小端序)
-        frame[idx++] = local_control.pitch & 0xFF;
+        tx_frame[idx++] = local_control.pitch & 0xFF;
-        frame[idx++] = (local_control.pitch >> 8) & 0xFF;
+        tx_frame[idx++] = (local_control.pitch >> 8) & 0xFF;
         
         // feed (小端序)
-        frame[idx++] = local_control.feed & 0xFF;
+        tx_frame[idx++] = local_control.feed & 0xFF;
-        frame[idx++] = (local_control.feed >> 8) & 0xFF;
+        tx_frame[idx++] = (local_control.feed >> 8) & 0xFF;
         
         // switch
-        frame[idx++] = (local_control.left_switch << 4) | local_control.right_switch;
+        tx_frame[idx++] = (local_control.left_switch << 4) | local_control.right_switch;
         
         // CRC8
-        frame[idx++] = 0xCC;
+        tx_frame[idx++] = 0xCC;
         
         // 帧尾
-        frame[idx++] = FRAME_TAIL;
+        tx_frame[idx++] = FRAME_TAIL;
         
         // 发送数据
-        serial.WriteData(frame, FRAME_LENGTH);
+        serial.WriteData(tx_frame, FRAME_LENGTH);
+        
+        // 尝试接收数据（非阻塞）
+        if (serial.ReadData(rx_frame, FRAME_LENGTH)) {
+            SensorData sensor;
+            if (parseSensorData(rx_frame, sensor)) {
+                std::lock_guard<std::mutex> lock(sensor_mutex);
+                shared_sensor = sensor;
+            }
+        }
         
         // 精确控制发送频率
         auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
@@ -104,18 +210,10 @@ void startSender(Serial &serial) {
         sender_running = true;
         sender_thread = std::thread(senderLoop, std::ref(serial));
         LOGM(STR_CTR(WORD_GREEN, "Sender thread started (50Hz)"));
-    }
-}
         
-// 更新控制数据
+        // 同时启动传感器监控线程
-void updateControl(int16_t yaw, int16_t pitch, int16_t feed, 
+        startSensorMonitor();
-                   uint8_t left_sw = 3, uint8_t right_sw = 3) {
+    }
-    std::lock_guard<std::mutex> lock(control_mutex);
-    shared_control.yaw = yaw;
-    shared_control.pitch = pitch;
-    shared_control.feed = feed;
-    shared_control.left_switch = left_sw;
-    shared_control.right_switch = right_sw;
 }
 
 bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
@@ -234,7 +332,7 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
         }
         
         // 索敌状态：yaw=150旋转，pitch大范围扫描，小陀螺开启
-        updateControl(150, pitch_scan, 0, 3, 2);
+        updateControl(300, pitch_scan, 0, 3, 2);
         return true;
     }
 }