From 7fe0cb5d10dd9aac2ddb0a5e242f4ba781702248 Mon Sep 17 00:00:00 2001 From: lyf <169361657@qq.com> Date: Sat, 28 Mar 2026 08:48:45 +0800 Subject: [PATCH] =?UTF-8?q?=E7=AC=AC=E4=B8=80=E7=89=88=E6=95=B0=E6=8D=AE?= =?UTF-8?q?=E6=8E=A5=E6=94=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../armor_finder/send_target/send_target.cpp | 158 ++++++++++++++---- 1 file changed, 128 insertions(+), 30 deletions(-) diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index d940a23..d4fc98b 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/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 sender_running{false}; +static std::atomic 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 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 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; - frame[idx++] = FRAME_HEADER_2; + tx_frame[idx++] = FRAME_HEADER_1; + tx_frame[idx++] = FRAME_HEADER_2; // x_move, y_move (固定为0) - frame[idx++] = 0; - frame[idx++] = 0; - frame[idx++] = 0; - frame[idx++] = 0; + tx_frame[idx++] = 0; + tx_frame[idx++] = 0; + tx_frame[idx++] = 0; + tx_frame[idx++] = 0; // yaw (小端序) - frame[idx++] = local_control.yaw & 0xFF; - frame[idx++] = (local_control.yaw >> 8) & 0xFF; + tx_frame[idx++] = local_control.yaw & 0xFF; + tx_frame[idx++] = (local_control.yaw >> 8) & 0xFF; // pitch (小端序) - frame[idx++] = local_control.pitch & 0xFF; - frame[idx++] = (local_control.pitch >> 8) & 0xFF; + tx_frame[idx++] = local_control.pitch & 0xFF; + tx_frame[idx++] = (local_control.pitch >> 8) & 0xFF; // feed (小端序) - frame[idx++] = local_control.feed & 0xFF; - frame[idx++] = (local_control.feed >> 8) & 0xFF; + tx_frame[idx++] = local_control.feed & 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 lock(sensor_mutex); + shared_sensor = sensor; + } + } // 精确控制发送频率 auto elapsed = std::chrono::duration_cast( @@ -104,20 +210,12 @@ 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(); } } -// 更新控制数据 -void updateControl(int16_t yaw, int16_t pitch, int16_t feed, - uint8_t left_sw = 3, uint8_t right_sw = 3) { - std::lock_guard 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) { // 确保发送线程已启动 static bool sender_started = false; @@ -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; } } \ No newline at end of file