哨兵索敌自瞄

armor/src/armor_finder/armor_finder.cpp

@@ -93,10 +93,14 @@ end:
     if(is_anti_top) { // 判断当前是否为反陀螺模式
         antiTop();
     } else if(target_box.rect != cv::Rect2d()) {
+        // 有目标时正常发送
         anti_top_cnt = 0;
         time_seq.clear();
         angle_seq.clear();
         sendBoxPosition(0);
+    } else {
+        // 无目标时也调用 sendBoxPosition，进入无目标状态处理
+        sendBoxPosition(0);
     }
 
     if(target_box.rect != cv::Rect2d()){

armor/src/armor_finder/send_target/send_target.cpp

@@ -5,109 +5,176 @@
 #include <armor_finder/armor_finder.h>
 #include <config/setconfig.h>
 #include <log.h>
+#include <chrono>
+#include <thread>
+#include <mutex>
+#include <atomic>
 
 // amadeus_26 协议: 15字节帧
-// | SOF(0xBB 0x77) | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | key(1B) | crc8(1B) | EOF(0xEE) |
+// | 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE |
 constexpr uint8_t FRAME_HEADER_1 = 0xBB;
 constexpr uint8_t FRAME_HEADER_2 = 0x77;
 constexpr uint8_t FRAME_TAIL = 0xEE;
 constexpr int FRAME_LENGTH = 15;
+constexpr int SEND_INTERVAL_MS = 20;  // 50Hz
 
-static uint8_t calculateCRC8(const uint8_t *data, size_t len) {
+// 状态定义
-    uint8_t crc = 0xFF;
+enum TargetState {
-    for (size_t i = 0; i < len; i++) {
+    HAVE_TARGET,           // 有目标
-        crc ^= data[i];
+    LOST_TARGET_WAIT,      // 丢失目标，等待1秒
-        for (int j = 0; j < 8; j++) {
+    SEARCHING              // 索敌状态（持续旋转直到找到目标）
-            if (crc & 0x80) {
+};
-                crc = (crc << 1) ^ 0x31;
-            } else {
-                crc <<= 1;
-            }
-        }
-    }
-    return crc;
-}
 
-static bool sendTarget(Serial &serial, double yaw, double pitch, double dist, uint16_t shoot_delay) {
+// 共享控制数据
-    int16_t x_move = 0;      // 平动左右 [-660, 660]
+struct ControlData {
-    int16_t y_move = 0;      // 平动前后 [-660, 660]
+    int16_t yaw = 0;
-    int16_t yaw_val = 0;     // 云台偏航 [-660, 660]
+    int16_t pitch = 0;
-    int16_t pitch_val = 0;   // 云台俯仰 [-660, 660]
+    int16_t feed = 0;
-    int16_t feed = 0;        // 拨弹轮 [-660, 660]
+    uint8_t left_switch = 3;
-    uint8_t key = 0;         // 按键 [0, 15]
+    uint8_t right_switch = 3;
+};
 
-#ifdef WITH_COUNT_FPS
+static std::mutex control_mutex;
-    static time_t last_time = time(nullptr);
+static ControlData shared_control;
-    static int fps;
+static std::atomic<bool> sender_running{false};
-    time_t t = time(nullptr);
+static std::thread sender_thread;
-    if (last_time != t) {
-        last_time = t;
-        cout << "Armor: fps:" << fps << ", yaw:" << yaw << ", pitch:" << pitch << ", dist:" << dist << endl;
-        fps = 0;
-    }
-    fps += 1;
-#endif
-
-#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
-
-    // 将角度转换为协议范围 [-660, 660]
-    yaw_val = static_cast<int16_t>(yaw * 10);     // 角度放大10倍
-    pitch_val = static_cast<int16_t>(pitch * 10);
-    MINMAX(yaw_val, -660, 660);
-    MINMAX(pitch_val, -660, 660);
-
-    // 如果需要发弹，设置 feed 值
-    if (shoot_delay > 0) {
-        feed = 660;  // 开火
-    }
-
-    // 打印发送的杆量值
-    LOGM(STR_CTR(WORD_BLUE, "Armor Send: x_move=%d, y_move=%d, yaw=%d, pitch=%d, feed=%d, key=%d"), 
-         x_move, y_move, yaw_val, pitch_val, feed, key);
 
+// 发送线程函数
+void senderLoop(Serial &serial) {
     uint8_t frame[FRAME_LENGTH];
-    int idx = 0;
     
-    // 帧头
+    while (sender_running) {
+        auto start = std::chrono::steady_clock::now();
+        
+        // 获取控制数据（加锁）
+        ControlData local_control;
+        {
+            std::lock_guard<std::mutex> lock(control_mutex);
+            local_control = shared_control;
+        }
+        
+        // 构建数据帧
+        int idx = 0;
         frame[idx++] = FRAME_HEADER_1;
         frame[idx++] = FRAME_HEADER_2;
         
-    // 平动左右 (2 bytes, int16, 小端序)
+        // x_move, y_move (固定为0)
-    frame[idx++] = x_move & 0xFF;
+        frame[idx++] = 0;
-    frame[idx++] = (x_move >> 8) & 0xFF;
+        frame[idx++] = 0;
+        frame[idx++] = 0;
+        frame[idx++] = 0;
         
-    // 平动前后 (2 bytes, 小端序)
+        // yaw (小端序)
-    frame[idx++] = y_move & 0xFF;
+        frame[idx++] = local_control.yaw & 0xFF;
-    frame[idx++] = (y_move >> 8) & 0xFF;
+        frame[idx++] = (local_control.yaw >> 8) & 0xFF;
         
-    // 云台偏航 (2 bytes, 小端序)
+        // pitch (小端序)
-    frame[idx++] = yaw_val & 0xFF;
+        frame[idx++] = local_control.pitch & 0xFF;
-    frame[idx++] = (yaw_val >> 8) & 0xFF;
+        frame[idx++] = (local_control.pitch >> 8) & 0xFF;
         
-    // 云台俯仰 (2 bytes, 小端序)
+        // feed (小端序)
-    frame[idx++] = pitch_val & 0xFF;
+        frame[idx++] = local_control.feed & 0xFF;
-    frame[idx++] = (pitch_val >> 8) & 0xFF;
+        frame[idx++] = (local_control.feed >> 8) & 0xFF;
         
-    // 拨弹轮 (2 bytes, 小端序)
+        // switch
-    frame[idx++] = feed & 0xFF;
+        frame[idx++] = (local_control.left_switch << 4) | local_control.right_switch;
-    frame[idx++] = (feed >> 8) & 0xFF;
         
-    // 按键 (1 byte)
+        // CRC8
-    frame[idx++] = key;
-
-    // CRC8 (数据区: frame[2]到frame[12], 共11字节)
-    // 当前固定为0xCC，如需启用CRC校验，取消下面注释
-    // frame[idx++] = calculateCRC8(frame + 2, 11);
         frame[idx++] = 0xCC;
         
         // 帧尾
         frame[idx++] = FRAME_TAIL;
         
-    return serial.WriteData(frame, sizeof(frame));
+        // 发送数据
+        serial.WriteData(frame, FRAME_LENGTH);
+        
+        // 精确控制发送频率
+        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
+            std::chrono::steady_clock::now() - start).count();
+        int sleep_ms = SEND_INTERVAL_MS - elapsed;
+        if (sleep_ms > 0) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(sleep_ms));
+        }
+    }
+}
+
+// 启动发送线程
+void startSender(Serial &serial) {
+    if (!sender_running) {
+        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, 
+                   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) {
-    if (target_box.rect == cv::Rect2d()) return false;
+    // 确保发送线程已启动
+    static bool sender_started = false;
+    if (!sender_started) {
+        startSender(serial);
+        sender_started = true;
+    }
+    
+    // 状态管理
+    static TargetState state = HAVE_TARGET;
+    static auto state_start_time = std::chrono::steady_clock::now();
+    static int16_t pitch_scan = -100;
+    static int pitch_direction = 1;
+    
+    // 检查当前是否有目标
+    bool has_target = (target_box.rect != cv::Rect2d());
+    auto now = std::chrono::steady_clock::now();
+    
+    // 状态机处理
+    switch (state) {
+        case HAVE_TARGET:
+            if (!has_target) {
+                // 目标丢失，进入等待状态
+                state = LOST_TARGET_WAIT;
+                state_start_time = now;
+                LOGM(STR_CTR(WORD_YELLOW, "Target lost, waiting 1s before searching..."));
+            }
+            break;
+            
+        case LOST_TARGET_WAIT:
+            if (has_target) {
+                // 目标恢复
+                state = HAVE_TARGET;
+                LOGM(STR_CTR(WORD_GREEN, "Target reacquired"));
+            } else {
+                // 检查是否等待满1秒
+                auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - state_start_time).count();
+                if (elapsed >= 1000) {  // 1秒到，进入索敌状态
+                    state = SEARCHING;
+                    LOGM(STR_CTR(WORD_LIGHT_CYAN, "Entering search mode (continuous)"));
+                }
+            }
+            break;
+            
+        case SEARCHING:
+            if (has_target) {
+                // 目标恢复，退出索敌
+                state = HAVE_TARGET;
+                LOGM(STR_CTR(WORD_GREEN, "Target found, exit search mode"));
+            }
+            // 否则持续索敌，不退出
+            break;
+    }
+    
+    // 根据状态更新控制值
+    if (state == HAVE_TARGET) {
+        // 有目标时的正常处理
         if (shoot_delay) {
             LOGM(STR_CTR(WORD_BLUE, "next box %dms"), shoot_delay);
         }
@@ -118,9 +185,6 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
         // PID
         sum_yaw += dx;
         sum_pitch += dy;
-    float yaw_I_component = YAW_AIM_KI * sum_yaw;
-    float pitch_I_component = PITCH_AIM_KI * sum_pitch;
-
         double tmp_yaw = dx;
         double tmp_pitch = dy;
         dx = YAW_AIM_KP * dx + YAW_AIM_KI * sum_yaw +
@@ -130,10 +194,51 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
 
         last_yaw = tmp_yaw;
         last_pitch = tmp_pitch;
-    //
 
         double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
         double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
-    double dist = DISTANCE_HEIGHT / rect.height;
+        
-    return sendTarget(serial, yaw, -pitch, dist, shoot_delay);
+        // 转换为杆量值（注意：pitch 方向修正）
+        int16_t yaw_val = static_cast<int16_t>(yaw * 100);
+        int16_t pitch_val = static_cast<int16_t>(-pitch * 100);  // 取反修正方向
+        int16_t feed_val = shoot_delay > 0 ? 660 : 0;
+        
+        // 更新控制数据
+        updateControl(yaw_val, pitch_val, feed_val, 3, 3);
+        return true;
+        
+    } else if (state == LOST_TARGET_WAIT) {
+        // 等待1秒期间：开启小陀螺旋转索敌
+        static int wait_count = 0;
+        wait_count++;
+        if (wait_count % 50 == 0) {
+            auto remaining = 1000 - std::chrono::duration_cast<std::chrono::milliseconds>(now - state_start_time).count();
+            LOGM(STR_CTR(WORD_YELLOW, "Pre-search: yaw=150, %dms left"), remaining > 0 ? remaining : 0);
+            wait_count = 0;
+        }
+        // 等待期间也旋转：yaw=150，pitch 小范围扫描
+        pitch_scan += pitch_direction * 5;
+        if (pitch_scan >= 50) pitch_direction = -1;
+        if (pitch_scan <= -50) pitch_direction = 1;
+        
+        updateControl(150, pitch_scan, 0, 3, 2);
+        return true;
+        
+    } else {  // SEARCHING
+        // 索敌状态：持续旋转直到找到目标
+        pitch_scan += pitch_direction * 7;
+        if (pitch_scan >= 200) pitch_direction = -1;
+        if (pitch_scan <= -100) pitch_direction = 1;
+        
+        static int search_count = 0;
+        search_count++;
+        if (search_count % 50 == 0) {
+            LOGM(STR_CTR(WORD_LIGHT_CYAN, "Searching: yaw=150, pitch=%d (waiting for target)"), pitch_scan);
+            search_count = 0;
+        }
+        
+        // 索敌状态：yaw=150旋转，pitch大范围扫描，小陀螺开启
+        updateControl(150, pitch_scan, 0, 3, 2);
+        return true;
+    }
 }

energy/src/energy/send/send.cpp

@@ -60,10 +60,13 @@ void Energy::sendEnergy() {
 // 此函数用于发送数据给主控板 (amadeus_26 协议)
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::sendTarget(Serial &serial, float yaw, float pitch, int16_t feed, uint8_t key) {
+    // 控制参数
     int16_t x_move = 0;           // 平动左右 [-660, 660]
     int16_t y_move = 0;           // 平动前后 [-660, 660]
     int16_t yaw_val = 0;          // 云台偏航 [-660, 660]
     int16_t pitch_val = 0;        // 云台俯仰 [-660, 660]
+    uint8_t left_switch = 3;      // 左拨杆 [1, 3] 2 摩擦轮打开 3 关闭
+    uint8_t right_switch = 3;     // 右拨杆 [1, 3] 2 小陀螺 3 关闭
 
 #ifdef WITH_COUNT_FPS
     static auto last_time = time(nullptr);
@@ -78,16 +81,14 @@ void Energy::sendTarget(Serial &serial, float yaw, float pitch, int16_t feed, ui
     fps += 1;
 #endif
 
-    // 将角度转换为协议范围 [-660, 660]
+    // 将角度 (-3, 3) 度 转换为协议范围 [-660, 660]
-    yaw_val = static_cast<int16_t>(yaw * 10);     // 角度放大10倍
+    // 映射公式: output = input * (660 / 3) = input * 220
-    pitch_val = static_cast<int16_t>(pitch * 10);
+    yaw_val = static_cast<int16_t>(yaw * 220);
+    pitch_val = static_cast<int16_t>(pitch * 220);
     MINMAX(yaw_val, -660, 660);
     MINMAX(pitch_val, -660, 660);
 
-    // 打印发送的杆量值
+    // 构建数据帧 - 与 test_pitch_up.cpp 格式一致
-    LOGM(STR_CTR(WORD_LIGHT_PURPLE, "Energy Send: x_move=%d, y_move=%d, yaw=%d, pitch=%d, feed=%d, key=%d"), 
-         x_move, y_move, yaw_val, pitch_val, feed, key);
-
     uint8_t frame[FRAME_LENGTH];
     int idx = 0;
 
@@ -115,17 +116,26 @@ void Energy::sendTarget(Serial &serial, float yaw, float pitch, int16_t feed, ui
     frame[idx++] = feed & 0xFF;
     frame[idx++] = (feed >> 8) & 0xFF;
 
-    // 按键 (1 byte) - 用于能量机关模式标识
+    // 拨杆 (1 byte: 高4位左拨杆，低4位右拨杆)
-    frame[idx++] = key;
+    // key 参数通过低4位传递能量机关模式信息
+    frame[idx++] = (left_switch << 4) | (key & 0x0F);
 
-    // CRC8 (数据区: frame[2]到frame[12], 共11字节)
+    // CRC8 (固定为 0xCC)
-    // 当前固定为0xCC
     frame[idx++] = 0xCC;
 
     // 帧尾
     frame[idx++] = FRAME_TAIL;
 
+    // 调试输出：打印完整帧内容 - 与 test_pitch_up.cpp 格式一致
+    std::string frame_hex;
+    char buf[4];
+    for (int i = 0; i < FRAME_LENGTH; i++) {
+        snprintf(buf, sizeof(buf), "%02X ", frame[i]);
+        frame_hex += buf;
+    }
+    LOGM(STR_CTR(WORD_LIGHT_PURPLE, "[TX](%dB): %s | yaw=%d pitch=%d feed=%d key=%d"), 
+         FRAME_LENGTH, frame_hex.c_str(), yaw_val, pitch_val, feed, key);
+
     serial.WriteData(frame, sizeof(frame));
     send_cnt += 1;
-//    LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send"));
 }

test_pitch_up.cpp

@@ -0,0 +1,251 @@
+/**
+ * @file test_pitch_up.cpp
+ * @brief 测试程序：单独控制 pitch 轴运动（支持命令行参数）
+ *
+ * 用法: ./test_pitch_up [pitch角度] [运行秒数]
+ * 示例: ./test_pitch_up 1.5 10  (向上1.5度，运行10秒)
+ *
+ * 协议帧格式（15字节）：
+ * | 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE |
+ */
+
+#include <cstring>
+#include <iostream>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <chrono>
+#include <thread>
+#include <cstdlib>
+
+// 帧定义 - 与示例程序完全一致
+constexpr uint8_t FRAME_HEADER_1 = 0xBB;
+constexpr uint8_t FRAME_HEADER_2 = 0x77;
+constexpr uint8_t FRAME_TAIL = 0xEE;
+constexpr int FRAME_LENGTH = 15;
+
+// 串口配置 - 与示例程序一致
+constexpr const char *SERIAL_PORT = "/dev/ttyCH340";
+constexpr int BAUDRATE = 115200;
+
+// CRC8 计算 - 与示例程序完全一致
+uint8_t calculateCRC8(const uint8_t *data, size_t len) {
+    uint8_t crc = 0xFF;
+    for (size_t i = 0; i < len; i++) {
+        crc ^= data[i];
+        for (int j = 0; j < 8; j++) {
+            if (crc & 0x80) {
+                crc = (crc << 1) ^ 0x31;
+            } else {
+                crc <<= 1;
+            }
+        }
+    }
+    return crc;
+}
+
+// 波特率转换 - 与示例程序一致
+speed_t convertBaudrate(int baudrate) {
+    switch (baudrate) {
+        case 9600: return B9600;
+        case 19200: return B19200;
+        case 38400: return B38400;
+        case 57600: return B57600;
+        case 115200: return B115200;
+        case 230400: return B230400;
+        case 460800: return B460800;
+        case 921600: return B921600;
+        default: return B115200;
+    }
+}
+
+int initSerial(const char *port, int baudrate) {
+    std::cout << "正在打开串口 " << port << "..." << std::endl;
+
+    int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
+    if (fd < 0) {
+        std::cerr << "无法打开串口 " << port << ": " << strerror(errno) << std::endl;
+        return -1;
+    }
+
+    struct termios tty;
+    memset(&tty, 0, sizeof(tty));
+
+    if (tcgetattr(fd, &tty) != 0) {
+        std::cerr << "tcgetattr 错误: " << strerror(errno) << std::endl;
+        close(fd);
+        return -1;
+    }
+
+    // 设置波特率
+    speed_t baud = convertBaudrate(baudrate);
+    cfsetospeed(&tty, baud);
+    cfsetispeed(&tty, baud);
+
+    // 8N1 配置 - 与示例程序一致
+    tty.c_cflag &= ~PARENB;  // 无校验
+    tty.c_cflag &= ~CSTOPB;  // 1位停止位
+    tty.c_cflag &= ~CSIZE;
+    tty.c_cflag |= CS8;             // 8位数据
+    tty.c_cflag |= CREAD | CLOCAL;  // 启用接收，忽略控制线
+    tty.c_cflag &= ~CRTSCTS;        // 禁用硬件流控
+
+    // 原始模式
+    tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    tty.c_iflag &= ~(IXON | IXOFF | IXANY);
+    tty.c_oflag &= ~OPOST;
+
+    // 设置超时 - 与示例程序一致
+    tty.c_cc[VMIN] = 0;
+    tty.c_cc[VTIME] = 1;  // 100ms 超时
+
+    if (tcsetattr(fd, TCSANOW, &tty) != 0) {
+        std::cerr << "tcsetattr 错误: " << strerror(errno) << std::endl;
+        close(fd);
+        return -1;
+    }
+
+    // 清空缓冲区
+    tcflush(fd, TCIOFLUSH);
+    std::cout << "串口打开成功" << std::endl;
+    return fd;
+}
+
+void sendControlFrame(int fd, int16_t pitch_val) {
+    // 控制参数
+    int16_t x_move = -100;           // 平动左右 [-660, 660]
+    int16_t y_move = -200;           // 平动前后 [-660, 660]
+    int16_t yaw_val = -300;          // 云台偏航 [-660, 660]
+    int16_t feed = -400;             // 拨弹轮 [-660, 660]
+    uint8_t left_switch = 3;      // 左拨杆 [1, 3] 2 摩擦轮打开 3 关闭
+    uint8_t right_switch = 3;     // 右拨杆 [1, 3] 2 小陀螺 3 关闭
+
+    // 构建数据帧 - 与示例程序完全一致
+    uint8_t frame[FRAME_LENGTH];
+    int idx = 0;
+
+    // 帧头
+    frame[idx++] = FRAME_HEADER_1;
+    frame[idx++] = FRAME_HEADER_2;
+
+    // 平动左右 (2 bytes, int16, 小端序)
+    frame[idx++] = x_move & 0xFF;
+    frame[idx++] = (x_move >> 8) & 0xFF;
+
+    // 平动前后 (2 bytes, 小端序)
+    frame[idx++] = y_move & 0xFF;
+    frame[idx++] = (y_move >> 8) & 0xFF;
+
+    // 云台偏航 (2 bytes, 小端序)
+    frame[idx++] = yaw_val & 0xFF;
+    frame[idx++] = (yaw_val >> 8) & 0xFF;
+
+    // 云台俯仰 (2 bytes, 小端序) - pitch 值
+    frame[idx++] = pitch_val & 0xFF;
+    frame[idx++] = (pitch_val >> 8) & 0xFF;
+
+    // 拨弹轮 (2 bytes, 小端序)
+    frame[idx++] = feed & 0xFF;
+    frame[idx++] = (feed >> 8) & 0xFF;
+
+    // 拨杆 (1 byte: 高4位左拨杆，低4位右拨杆)
+    frame[idx++] = (left_switch << 4) | right_switch;
+
+    // CRC8 (固定为 0xCC)
+    frame[idx++] = 0xCC;
+
+    // 帧尾
+    frame[idx++] = FRAME_TAIL;
+
+    // 调试输出：打印完整帧内容 - 与示例程序一致
+    std::string frame_hex;
+    char buf[4];
+    for (int i = 0; i < FRAME_LENGTH; i++) {
+        snprintf(buf, sizeof(buf), "%02X ", frame[i]);
+        frame_hex += buf;
+    }
+    std::cout << "[TX](" << FRAME_LENGTH << "B): " << frame_hex
+              << "| pitch=" << pitch_val << std::endl;
+
+    // 发送数据
+    ssize_t written = write(fd, frame, FRAME_LENGTH);
+    if (written != FRAME_LENGTH) {
+        std::cerr << "发送数据不完整: " << written << "/" << FRAME_LENGTH << std::endl;
+    }
+}
+
+void printUsage(const char* program) {
+    std::cout << "用法: " << program << " [pitch角度] [运行秒数]" << std::endl;
+    std::cout << "  pitch角度: 目标角度，正值向上，负值向下，范围建议 (-3, 3) 度" << std::endl;
+    std::cout << "  运行秒数:  发送时长，0或不填表示无限循环" << std::endl;
+    std::cout << std::endl;
+    std::cout << "示例:" << std::endl;
+    std::cout << "  " << program << "          # 默认向上1度，无限循环" << std::endl;
+    std::cout << "  " << program << " 1.5      # 向上1.5度，无限循环" << std::endl;
+    std::cout << "  " << program << " -1 5     # 向下1度，运行5秒" << std::endl;
+    std::cout << "  " << program << " 2 10     # 向上2度，运行10秒" << std::endl;
+}
+
+int main(int argc, char* argv[]) {
+    // 默认参数
+    double pitch_angle = -1;  // 默认向上1度
+    int duration_sec = 0;      // 默认无限循环
+
+    // 解析命令行参数
+    if (argc > 1) {
+        pitch_angle = std::atof(argv[1]);
+    }
+    if (argc > 2) {
+        duration_sec = std::atoi(argv[2]);
+    }
+    if (argc > 3) {
+        printUsage(argv[0]);
+        return 1;
+    }
+
+    std::cout << "========================================" << std::endl;
+    std::cout << "Pitch 轴控制测试程序" << std::endl;
+    std::cout << "目标角度: " << (pitch_angle > 0 ? "向上 " : "向下 ") 
+              << std::abs(pitch_angle) << " 度" << std::endl;
+    if (duration_sec > 0) {
+        std::cout << "运行时长: " << duration_sec << " 秒" << std::endl;
+    } else {
+        std::cout << "运行时长: 无限循环 (按 Ctrl+C 停止)" << std::endl;
+    }
+    std::cout << "发送频率: 50 Hz" << std::endl;
+    std::cout << "========================================" << std::endl;
+
+    // 初始化串口
+    int fd = initSerial(SERIAL_PORT, BAUDRATE);
+    if (fd < 0) {
+        return -1;
+    }
+
+    // 计算 pitch 值
+    // 映射系数 220: 3度 -> 660
+    int16_t pitch_val = static_cast<int16_t>(pitch_angle * 220);
+    if (pitch_val > 660) pitch_val = 660;
+    if (pitch_val < -660) pitch_val = -660;
+
+    std::cout << "Pitch 发送值: " << pitch_val << std::endl;
+    std::cout << "----------------------------------------" << std::endl;
+
+    // 计算结束时间
+    auto start_time = std::chrono::steady_clock::now();
+    auto end_time = start_time + std::chrono::seconds(duration_sec);
+
+    // 持续发送，频率 50Hz
+    while (true) {
+        // 检查是否超时
+        if (duration_sec > 0 && std::chrono::steady_clock::now() >= end_time) {
+            std::cout << "运行时间到达 " << duration_sec << " 秒，停止发送" << std::endl;
+            break;
+        }
+
+        sendControlFrame(fd, pitch_val);
+        std::this_thread::sleep_for(std::chrono::milliseconds(20));  // 50Hz
+    }
+
+    close(fd);
+    return 0;
+}

test_yaw_swing.cpp

@@ -0,0 +1,261 @@
+/**
+ * @file test_yaw_swing.cpp
+ * @brief 测试程序：yaw 轴左右摆动测试
+ *
+ * 发送序列: yaw=100 保持1秒 -> yaw=-100 保持1秒 -> 循环
+ * 用法: ./test_yaw_swing [循环次数]
+ * 示例: ./test_yaw_swing 3  (循环3次后停止，默认无限循环)
+ *
+ * 协议帧格式（15字节）：
+ * | 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE |
+ */
+
+#include <cstring>
+#include <iostream>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <chrono>
+#include <thread>
+#include <cstdlib>
+
+// 帧定义 - 与示例程序完全一致
+constexpr uint8_t FRAME_HEADER_1 = 0xBB;
+constexpr uint8_t FRAME_HEADER_2 = 0x77;
+constexpr uint8_t FRAME_TAIL = 0xEE;
+constexpr int FRAME_LENGTH = 15;
+
+// 串口配置
+constexpr const char *SERIAL_PORT = "/dev/ttyCH340";
+constexpr int BAUDRATE = 115200;
+
+// 测试参数
+constexpr int16_t YAW_RIGHT = 100;   // 向右100
+constexpr int16_t YAW_LEFT = -100;   // 向左-100
+constexpr int HOLD_TIME_MS = 1000;   // 保持1秒
+constexpr int SEND_INTERVAL_MS = 20; // 50Hz
+
+// CRC8 计算
+uint8_t calculateCRC8(const uint8_t *data, size_t len) {
+    uint8_t crc = 0xFF;
+    for (size_t i = 0; i < len; i++) {
+        crc ^= data[i];
+        for (int j = 0; j < 8; j++) {
+            if (crc & 0x80) {
+                crc = (crc << 1) ^ 0x31;
+            } else {
+                crc <<= 1;
+            }
+        }
+    }
+    return crc;
+}
+
+speed_t convertBaudrate(int baudrate) {
+    switch (baudrate) {
+        case 9600: return B9600;
+        case 19200: return B19200;
+        case 38400: return B38400;
+        case 57600: return B57600;
+        case 115200: return B115200;
+        default: return B115200;
+    }
+}
+
+int initSerial(const char *port, int baudrate) {
+    std::cout << "正在打开串口 " << port << "..." << std::endl;
+
+    int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
+    if (fd < 0) {
+        std::cerr << "无法打开串口 " << port << ": " << strerror(errno) << std::endl;
+        return -1;
+    }
+
+    struct termios tty;
+    memset(&tty, 0, sizeof(tty));
+
+    if (tcgetattr(fd, &tty) != 0) {
+        std::cerr << "tcgetattr 错误: " << strerror(errno) << std::endl;
+        close(fd);
+        return -1;
+    }
+
+    speed_t baud = convertBaudrate(baudrate);
+    cfsetospeed(&tty, baud);
+    cfsetispeed(&tty, baud);
+
+    tty.c_cflag &= ~PARENB;
+    tty.c_cflag &= ~CSTOPB;
+    tty.c_cflag &= ~CSIZE;
+    tty.c_cflag |= CS8;
+    tty.c_cflag |= CREAD | CLOCAL;
+    tty.c_cflag &= ~CRTSCTS;
+
+    tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    tty.c_iflag &= ~(IXON | IXOFF | IXANY);
+    tty.c_oflag &= ~OPOST;
+
+    tty.c_cc[VMIN] = 0;
+    tty.c_cc[VTIME] = 1;
+
+    if (tcsetattr(fd, TCSANOW, &tty) != 0) {
+        std::cerr << "tcsetattr 错误: " << strerror(errno) << std::endl;
+        close(fd);
+        return -1;
+    }
+
+    tcflush(fd, TCIOFLUSH);
+    std::cout << "串口打开成功" << std::endl;
+    return fd;
+}
+
+void sendControlFrame(int fd, int16_t yaw_val) {
+    // 控制参数
+    int16_t x_move = 0;
+    int16_t y_move = 0;
+    int16_t pitch_val = 0;
+    int16_t feed = 0;
+    uint8_t left_switch = 3;
+    uint8_t right_switch = 3;
+
+    uint8_t frame[FRAME_LENGTH];
+    int idx = 0;
+
+    // 帧头
+    frame[idx++] = FRAME_HEADER_1;
+    frame[idx++] = FRAME_HEADER_2;
+
+    // 平动左右
+    frame[idx++] = x_move & 0xFF;
+    frame[idx++] = (x_move >> 8) & 0xFF;
+
+    // 平动前后
+    frame[idx++] = y_move & 0xFF;
+    frame[idx++] = (y_move >> 8) & 0xFF;
+
+    // 云台偏航 (yaw)
+    frame[idx++] = yaw_val & 0xFF;
+    frame[idx++] = (yaw_val >> 8) & 0xFF;
+
+    // 云台俯仰
+    frame[idx++] = pitch_val & 0xFF;
+    frame[idx++] = (pitch_val >> 8) & 0xFF;
+
+    // 拨弹轮
+    frame[idx++] = feed & 0xFF;
+    frame[idx++] = (feed >> 8) & 0xFF;
+
+    // 拨杆
+    frame[idx++] = (left_switch << 4) | right_switch;
+
+    // CRC8
+    frame[idx++] = 0xCC;
+
+    // 帧尾
+    frame[idx++] = FRAME_TAIL;
+
+    // 调试输出
+    std::string frame_hex;
+    char buf[4];
+    for (int i = 0; i < FRAME_LENGTH; i++) {
+        snprintf(buf, sizeof(buf), "%02X ", frame[i]);
+        frame_hex += buf;
+    }
+    std::string direction = (yaw_val > 0) ? "右" : "左";
+    std::cout << "[TX](" << FRAME_LENGTH << "B): " << frame_hex
+              << "| yaw=" << yaw_val << " (" << direction << ")" << std::endl;
+
+    ssize_t written = write(fd, frame, FRAME_LENGTH);
+    if (written != FRAME_LENGTH) {
+        std::cerr << "发送数据不完整: " << written << "/" << FRAME_LENGTH << std::endl;
+    }
+}
+
+// 发送指定yaw值，保持指定时间
+void sendYawForDuration(int fd, int16_t yaw_val, int duration_ms) {
+    auto start = std::chrono::steady_clock::now();
+    int count = 0;
+    
+    while (true) {
+        auto now = std::chrono::steady_clock::now();
+        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count();
+        
+        if (elapsed >= duration_ms) {
+            break;
+        }
+        
+        sendControlFrame(fd, yaw_val);
+        count++;
+        std::this_thread::sleep_for(std::chrono::milliseconds(SEND_INTERVAL_MS));
+    }
+    
+    std::cout << "  发送了 " << count << " 帧，时长 " << duration_ms << "ms" << std::endl;
+}
+
+void printUsage(const char* program) {
+    std::cout << "用法: " << program << " [循环次数]" << std::endl;
+    std::cout << "  循环次数: 测试循环次数，0或不填表示无限循环" << std::endl;
+    std::cout << std::endl;
+    std::cout << "示例:" << std::endl;
+    std::cout << "  " << program << "       # 无限循环 (按 Ctrl+C 停止)" << std::endl;
+    std::cout << "  " << program << " 5     # 循环5次后停止" << std::endl;
+}
+
+int main(int argc, char* argv[]) {
+    int loop_count = 0;  // 0表示无限循环
+    int current_loop = 0;
+
+    if (argc > 1) {
+        loop_count = std::atoi(argv[1]);
+    }
+    if (argc > 2) {
+        printUsage(argv[0]);
+        return 1;
+    }
+
+    std::cout << "========================================" << std::endl;
+    std::cout << "Yaw 轴摆动测试程序" << std::endl;
+    std::cout << "测试序列: yaw=" << YAW_RIGHT << "(右,1s) -> yaw=" << YAW_LEFT << "(左,1s)" << std::endl;
+    if (loop_count > 0) {
+        std::cout << "循环次数: " << loop_count << " 次" << std::endl;
+    } else {
+        std::cout << "循环次数: 无限 (按 Ctrl+C 停止)" << std::endl;
+    }
+    std::cout << "发送频率: 50 Hz" << std::endl;
+    std::cout << "========================================" << std::endl;
+
+    // 初始化串口
+    int fd = initSerial(SERIAL_PORT, BAUDRATE);
+    if (fd < 0) {
+        return -1;
+    }
+
+    std::cout << "----------------------------------------" << std::endl;
+
+    // 主循环
+    while (true) {
+        if (loop_count > 0 && current_loop >= loop_count) {
+            std::cout << "完成 " << loop_count << " 次循环，测试结束" << std::endl;
+            break;
+        }
+
+        current_loop++;
+        if (loop_count > 0) {
+            std::cout << "\n=== 第 " << current_loop << "/" << loop_count << " 次循环 ===" << std::endl;
+        } else {
+            std::cout << "\n=== 第 " << current_loop << " 次循环 ===" << std::endl;
+        }
+
+        // 步骤1: yaw=100 (向右) 保持1秒
+        std::cout << "【步骤1】yaw=" << YAW_RIGHT << " (向右) 保持 " << HOLD_TIME_MS << "ms" << std::endl;
+        sendYawForDuration(fd, YAW_RIGHT, HOLD_TIME_MS);
+
+        // 步骤2: yaw=-100 (向左) 保持1秒
+        std::cout << "【步骤2】yaw=" << YAW_LEFT << " (向左) 保持 " << HOLD_TIME_MS << "ms" << std::endl;
+        sendYawForDuration(fd, YAW_LEFT, HOLD_TIME_MS);
+    }
+
+    close(fd);
+    std::cout << "串口已关闭" << std::endl;
+    return 0;
+}

tools/monitor.bat

@@ -1,6 +1,6 @@
 @echo off 
 
-rem <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>س<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͳ<EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޸<EFBFBD>
+rem 定义需监控程序的进程名和程序路径，可根据需要进行修改
 
 set AppName=run.exe
 
@@ -8,43 +8,43 @@ set AppArgs= --run-with-camera --wait-uart --show-armor-box
 
 set AppPath=C:\Users\sjturm\Desktop\AutoAim\build\Release\
 
-title <EFBFBD><EFBFBD><EFBFBD>̼<EFBFBD><EFBFBD><EFBFBD>
+title 进程监控
 
 cls
 
 echo.
 
-echo <EFBFBD><EFBFBD><EFBFBD>̼<EFBFBD><EFBFBD>ؿ<EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+echo 进程监控开始……
 
 echo.
 
-rem <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+rem 定义循环体
 
 :startjc
 
-   rem <EFBFBD>ӽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD><EFBFBD>в<EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+   rem 从进程列表中查找指定进程
 
-   rem  <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҳ<EFBFBD><EFBFBD>д<EFBFBD><EFBFBD> qprocess %AppName% >nul <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>鷢<EFBFBD><EFBFBD><EFBFBD>󲹳䣩
+   rem  下面语句也可写成 qprocess %AppName% >nul （经验发布后补充）
 
    qprocess|findstr /i %AppName% >nul
 
-   rem <EFBFBD><EFBFBD><EFBFBD><EFBFBD>errorlevel<EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>̣<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>в<EFBFBD><EFBFBD>ҵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+   rem 变量errorlevel的值等于0表示查找到进程，否则没有查找到进程
 
    if %errorlevel%==0 (
 
-         echo ^>%date:~0,10% %time:~0,8% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD>
+         echo ^>%date:~0,10% %time:~0,8% 程序正在运行……
 
     )else (
 
-           echo ^>%date:~0,10% %time:~0,8% û<EFBFBD>з<EFBFBD><EFBFBD>ֳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+           echo ^>%date:~0,10% %time:~0,8% 没有发现程序进程
 
-           echo ^>%date:~0,10% %time:~0,8% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+           echo ^>%date:~0,10% %time:~0,8% 正在重新启动程序
 
-           start %AppPath%%AppName%%AppArgs% 2>nul && echo ^>%date:~0,10% %time:~0,8% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD>
+           start %AppPath%%AppName%%AppArgs% 2>nul && echo ^>%date:~0,10% %time:~0,8% 启动程序成功
 
    )
 
-   rem <EFBFBD><EFBFBD>ping<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+   rem 用ping命令来实现延时运行
 
    ping -n 2 -w 1000 1.1.1.1>nul