数据接收回退，添加串口测试

armor/src/armor_finder/send_target/send_target.cpp

@@ -34,58 +34,10 @@ 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, 
@@ -98,47 +50,9 @@ void updateControl(int16_t yaw, int16_t pitch, int16_t feed,
     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 tx_frame[FRAME_LENGTH];
-    uint8_t rx_frame[FRAME_LENGTH];
     
     while (sender_running) {
         auto start = std::chrono::steady_clock::now();
@@ -185,15 +99,6 @@ void senderLoop(Serial &serial) {
         // 发送数据
         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>(
             std::chrono::steady_clock::now() - start).count();
@@ -210,9 +115,6 @@ void startSender(Serial &serial) {
         sender_running = true;
         sender_thread = std::thread(senderLoop, std::ref(serial));
         LOGM(STR_CTR(WORD_GREEN, "Sender thread started (50Hz)"));
-        
-        // 同时启动传感器监控线程
-        startSensorMonitor();
     }
 }
 
@@ -332,7 +234,7 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
         }
         
         // 索敌状态：yaw=150旋转，pitch大范围扫描，小陀螺开启
-        updateControl(300, pitch_scan, 0, 3, 2);
+        updateControl(150, pitch_scan, 0, 3, 2);
         return true;
     }
 }