修改颜色和 readme

Reviewed-on: VSAG/vision_sjtu_19#1

.gitignore

@@ -1,6 +1,9 @@
 cmake-build-debug
 build
 .idea
+.xmake
+.vscode/.cache
+.vscode/compile_commands.json
 Mark
 armor_box_photo
 tools/TrainCNN/.idea
@@ -9,4 +12,4 @@ tools/TrainCNN/__pycache__
 others/include/config/config.h
 others/MV-UB31-Group0.config
 .DS_Store
-video
+video

.vscode/settings.json

@@ -0,0 +1,8 @@
+{
+        "clangd.arguments": [
+        "--compile-commands-dir=${workspaceFolder}/build",
+        "--completion-style=detailed",
+        "--query-driver=/usr/bin/clang",
+        "--header-insertion=never"
+    ],
+}

CMakeLists.txt

@@ -8,6 +8,8 @@ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"
 SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}")
 SET(BIN_NAME "run")
 
+SET(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
 
 FIND_FILE(CONFIG_FOUND "config.h" "others/include/config")
 if (CONFIG_FOUND)
@@ -24,7 +26,7 @@ IF(CCACHE_FOUND)
 ENDIF()
 
 FIND_PACKAGE(Eigen3 REQUIRED)
-FIND_PACKAGE(OpenCV 3 REQUIRED)
+FIND_PACKAGE(OpenCV 4 REQUIRED)
 FIND_PACKAGE(Threads)
 
 LINK_DIRECTORIES(${PROJECT_SOURCE_DIR}/others)

README.md

@@ -1,4 +1,4 @@
-# 上海交通大学 RoboMaster 2019赛季 视觉代码
+# 黑龙江大学 RoboMaster 2026 赛季 哨兵代码
 
 本代码是上海交通大学RoboMaster2019赛季步兵车辆的视觉部分，分为三个模块:**装甲板识别**，**能量机关**，以及**封装的设备驱动和配置文件**。可以提取能量机关以外的模块并修改main函数直接作为哨兵识别代码。 
 
@@ -13,6 +13,10 @@
 
 **如有BUG或者想交流的朋友欢迎积极联系我们**
 
+**分享部分比赛时摄像头录制的视频:**
+
+链接: https://pan.baidu.com/s/1LwxEpeYYblX3cSzb59MTVg 提取码: 84ju 复制这段内容后打开百度网盘手机App，操作更方便哦
+
 ---
 
 运行效果：自瞄帧率120（摄像头最大帧率）,识别距离根据环境不同大约8米左右(5mm焦距镜头)。
@@ -26,6 +30,8 @@
 | ---------------------------------------------------- | -------------------------------------------- | ------------------------------------------------------------ | ---------------------------------------------------------- |
 | IntelNUC<br />MindVision工业相机×１<br />USB转TTL×１ | Ubuntu18.04<br />Ubuntu16.04<br />Ｗindows10 | OpenCV3.4.5<br />OpenCV_contrib3.4.5<br />Eigen3<br />MindVision相机驱动 | Ubuntu18/16 : cmake3+gcc7+g++7 <br />Win10 : cmake3+VS2019 |
 
+MindVision相机型号：MV-UBS31GC
+
 **关于Windows环境下的运行支持，仅保证程序可以编译运行。对与部分辅助功能，如生成自启动脚本则不支持。**
 
 **实际装载在步兵和哨兵上的运行环境为Ubuntu18.04。**
@@ -62,6 +68,8 @@ sudo ./run
 
 ```./run --help```可以查看所有命令行参数及其作用。所有调试选项都集成到了命令行参数中。
 
+**不使用任何参数直接运行将没有任何图像显示。**
+
 需要调参的部分：主要需要根据车辆情况而调参的参数存放在others/include/config/setconfig.h中
 
 ### 3.工作条件

armor/include/armor_finder/armor_finder.h

@@ -9,7 +9,7 @@
 #include <systime.h>
 #include <constants.h>
 #include <opencv2/core.hpp>
-#include <opencv2/tracking.hpp>
+#include <opencv2/tracking/tracking.hpp>
 #include <serial.h>
 #include <armor_finder/classifier/classifier.h>
 #include <additions.h>
@@ -20,8 +20,8 @@
 #define BOX_RED     ENEMY_RED
 #define BOX_BLUE    ENEMY_BLUE
 
-#define IMAGE_CENTER_X      (320)
+#define IMAGE_CENTER_X      (640)
-#define IMAGE_CENTER_Y      (240-20)
+#define IMAGE_CENTER_Y      (512-20)
 
 #define DISTANCE_HEIGHT_5MM (10700.0)     // 单位: cm*pixel
 #define DISTANCE_HEIGHT     DISTANCE_HEIGHT_5MM
@@ -120,6 +120,10 @@ private:
     vector<systime> time_seq;                           // 一个周期内的时间采样点
     vector<float> angle_seq;                            // 一个周期内的角度采样点
 
+    float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
+    float last_yaw, last_pitch;//PID中微分项
+    float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项
+
     bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs);
     bool findArmorBox(const cv::Mat &src, ArmorBox &box);
     bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBoxes &armor_boxes);

armor/src/armor_finder/armor_finder.cpp

@@ -67,7 +67,7 @@ void ArmorFinder::run(cv::Mat &src) {
                 if ((target_box.rect & cv::Rect2d(0, 0, 640, 480)) == target_box.rect) { // 判断装甲板区域是否脱离图像区域
                     if (!classifier) {                                          /* 如果分类器不可用 */
                         cv::Mat roi = src(target_box.rect).clone(), roi_gray;  /* 就使用装甲区域亮点数判断是否跟丢 */
-                        cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
+                        cv::cvtColor(roi, roi_gray, cv::COLOR_RGB2GRAY);
                         cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
                         contour_area = cv::countNonZero(roi_gray);
                     }
@@ -92,11 +92,15 @@ void ArmorFinder::run(cv::Mat &src) {
 end:
     if(is_anti_top) { // 判断当前是否为反陀螺模式
         antiTop();
-    }else if(target_box.rect != cv::Rect2d()) {
+    } 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/find/find_light_blobs.cpp

@@ -83,11 +83,11 @@ bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
     }else{
         light_threshold = 200;
     }
-    cv::threshold(color_channel, src_bin_light, light_threshold, 255, CV_THRESH_BINARY); // 二值化对应通道
+    cv::threshold(color_channel, src_bin_light, light_threshold, 255, cv::THRESH_BINARY); // 二值化对应通道
     if (src_bin_light.empty()) return false;
     imagePreProcess(src_bin_light);                                  // 开闭运算
 
-    cv::threshold(color_channel, src_bin_dim, 140, 255, CV_THRESH_BINARY); // 二值化对应通道
+    cv::threshold(color_channel, src_bin_dim, 140, 255, cv::THRESH_BINARY); // 二值化对应通道
     if (src_bin_dim.empty()) return false;
     imagePreProcess(src_bin_dim);                                  // 开闭运算
 
@@ -100,8 +100,8 @@ bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
     std::vector<std::vector<cv::Point>> light_contours_light, light_contours_dim;
     LightBlobs light_blobs_light, light_blobs_dim;
     std::vector<cv::Vec4i> hierarchy_light, hierarchy_dim;
-    cv::findContours(src_bin_light, light_contours_light, hierarchy_light, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE);
+    cv::findContours(src_bin_light, light_contours_light, hierarchy_light, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE);
-    cv::findContours(src_bin_dim, light_contours_dim, hierarchy_dim, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE);
+    cv::findContours(src_bin_dim, light_contours_dim, hierarchy_dim, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE);
     for (int i = 0; i < light_contours_light.size(); i++) {
         if (hierarchy_light[i][2] == -1) {
             cv::RotatedRect rect = cv::minAreaRect(light_contours_light[i]);

armor/src/armor_finder/send_target/send_target.cpp

@@ -5,52 +5,236 @@
 #include <armor_finder/armor_finder.h>
 #include <config/setconfig.h>
 #include <log.h>
+#include <chrono>
+#include <thread>
+#include <mutex>
+#include <atomic>
 
-static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) {
+// amadeus_26 协议: 15字节帧
-    static short x_tmp, y_tmp, z_tmp;
+// | 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE |
-    uint8_t buff[10];
+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
 
-#ifdef WITH_COUNT_FPS
+// 状态定义
-    static time_t last_time = time(nullptr);
+enum TargetState {
-    static int fps;
+    HAVE_TARGET,           // 有目标
-    time_t t = time(nullptr);
+    LOST_TARGET_WAIT,      // 丢失目标，等待1秒
-    if (last_time != t) {
+    SEARCHING              // 索敌状态（持续旋转直到找到目标）
-        last_time = t;
+};
-        cout << "Armor: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
+
-        fps = 0;
+// 共享控制数据
+struct ControlData {
+    int16_t yaw = 0;
+    int16_t pitch = 0;
+    int16_t feed = 0;
+    uint8_t left_switch = 3;
+    uint8_t right_switch = 3;
+};
+
+static std::mutex control_mutex;
+static ControlData shared_control;
+static std::atomic<bool> sender_running{false};
+static std::thread sender_thread;
+
+// 更新控制数据
+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 senderLoop(Serial &serial) {
+    uint8_t tx_frame[FRAME_LENGTH];
+    
+    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;
+        tx_frame[idx++] = FRAME_HEADER_1;
+        tx_frame[idx++] = FRAME_HEADER_2;
+        
+        // x_move, y_move (固定为0)
+        tx_frame[idx++] = 0;
+        tx_frame[idx++] = 0;
+        tx_frame[idx++] = 0;
+        tx_frame[idx++] = 0;
+        
+        // yaw (小端序)
+        tx_frame[idx++] = local_control.yaw & 0xFF;
+        tx_frame[idx++] = (local_control.yaw >> 8) & 0xFF;
+        
+        // pitch (小端序)
+        tx_frame[idx++] = local_control.pitch & 0xFF;
+        tx_frame[idx++] = (local_control.pitch >> 8) & 0xFF;
+        
+        // feed (小端序)
+        tx_frame[idx++] = local_control.feed & 0xFF;
+        tx_frame[idx++] = (local_control.feed >> 8) & 0xFF;
+        
+        // switch
+        tx_frame[idx++] = (local_control.left_switch << 4) | local_control.right_switch;
+        
+        // CRC8
+        tx_frame[idx++] = 0xCC;
+        
+        // 帧尾
+        tx_frame[idx++] = FRAME_TAIL;
+        
+        // 发送数据
+        serial.WriteData(tx_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));
+        }
     }
-    fps += 1;
+}
-#endif
 
-    x_tmp = static_cast<short>(x * (32768 - 1) / 100);
+// 启动发送线程
-    y_tmp = static_cast<short>(y * (32768 - 1) / 100);
+void startSender(Serial &serial) {
-    z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
+    if (!sender_running) {
-
+        sender_running = true;
-    buff[0] = 's';
+        sender_thread = std::thread(senderLoop, std::ref(serial));
-    buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
+        LOGM(STR_CTR(WORD_GREEN, "Sender thread started (50Hz)"));
-    buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
+    }
-    buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
-    buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
-    buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
-    buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
-    buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
-    buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
-    buff[9] = 'e';
-//    if(buff[7]<<8 | buff[8])
-//        cout << (buff[7]<<8 | buff[8]) << endl;
-    return serial.WriteData(buff, sizeof(buff));
 }
 
 bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
-    if (target_box.rect == cv::Rect2d()) return false;
+    // 确保发送线程已启动
-    if (shoot_delay) {
+    static bool sender_started = false;
-        LOGM(STR_CTR(WORD_BLUE, "next box %dms"), shoot_delay);
+    if (!sender_started) {
+        startSender(serial);
+        sender_started = true;
     }
-    auto rect = target_box.rect;
+    
-    double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X;
+    // 状态管理
-    double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y;
+    static TargetState state = HAVE_TARGET;
-    double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
+    static auto state_start_time = std::chrono::steady_clock::now();
-    double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
+    static int16_t pitch_scan = -100;
-    double dist = DISTANCE_HEIGHT / rect.height;
+    static int pitch_direction = 1;
-    return sendTarget(serial, yaw, -pitch, dist, shoot_delay);
+    
-}
+    // 检查当前是否有目标
+    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);
+        }
+        auto rect = target_box.rect;
+        double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X;
+        double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y;
+
+        // PID
+        sum_yaw += dx;
+        sum_pitch += dy;
+        double tmp_yaw = dx;
+        double tmp_pitch = dy;
+        dx = YAW_AIM_KP * dx + YAW_AIM_KI * sum_yaw +
+                       YAW_AIM_KD * (dx - last_yaw);
+        dy = PITCH_AIM_KP * dy + PITCH_AIM_KI * sum_pitch +
+                         PITCH_AIM_KD * (dy - last_pitch);
+
+        last_yaw = tmp_yaw;
+        last_pitch = tmp_pitch;
+
+        double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
+        double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
+        
+        // 转换为杆量值（注意：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, "Waiting... %dms left before search"), remaining > 0 ? remaining : 0);
+            wait_count = 0;
+        }
+        // 等待期间不旋转，发送0值
+        updateControl(0, 0, 0, 3, 3);
+        return true;
+        
+    } else {  // SEARCHING
+        // 索敌状态：持续旋转直到找到目标，加快pitch扫描速度
+        pitch_scan += pitch_direction * 15;  // 从7加快到15
+        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;
+    }
+}

armor/src/armor_finder/tracking_state/tracking_state.cpp

@@ -8,13 +8,13 @@
 #include <show_images/show_images.h>
 
 bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
-    auto pos = target_box.rect;
+    cv::Rect pos(target_box.rect);
     if(!tracker->update(src, pos)){ // 使用KCFTracker进行追踪
         target_box = ArmorBox();
         LOGW("Track fail!");
         return false;
     }
-    if((pos & cv::Rect2d(0, 0, 640, 480)) != pos){
+    if((cv::Rect2d(pos) & cv::Rect2d(0, 0, 640, 480)) != cv::Rect2d(pos)){
         target_box = ArmorBox();
         LOGW("Track out range!");
         return false;
@@ -52,7 +52,7 @@ bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
             }
         }else{ //　分类器不可用，使用常规方法判断
             cv::Mat roi_gray;
-            cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
+            cv::cvtColor(roi, roi_gray, cv::COLOR_RGB2GRAY);
             cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
             contour_area = cv::countNonZero(roi_gray);
             if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){

energy/include/energy/energy.h

@@ -35,7 +35,7 @@ public:
     Serial &serial;//串口
     void setEnergyInit();//设置能量机关初始化
     void sendEnergy();//发送能量机关数据
-    void sendTarget(Serial& serial, float x, float y, float z, uint16_t u);//发送数据
+    void sendTarget(Serial& serial, float yaw, float pitch, int16_t feed, uint8_t key);//发送数据 (amadeus_26 协议)
 
 
 private:

energy/src/energy/find/energy_finder.cpp

@@ -21,12 +21,12 @@ int Energy::findFans(const cv::Mat &src) {
     static Mat src_bin;
     src_bin = src.clone();
     if (src.type() == CV_8UC3) {
-        cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
+        cvtColor(src_bin, src_bin, cv::COLOR_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
     }
     std::vector<vector<Point> > fan_contours;
     FanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
     if (show_process)imshow("fan struct", src_bin);
-    findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, fan_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
 
     for (auto &fan_contour : fan_contours) {
         if (!isValidFanContour(src_bin, fan_contour)) {
@@ -54,16 +54,16 @@ int Energy::findArmors(const cv::Mat &src) {
     static Mat src_bin;
     src_bin = src.clone();
     if (src.type() == CV_8UC3) {
-        cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
+        cvtColor(src_bin, src_bin, cv::COLOR_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
     }
     std::vector<vector<Point> > armor_contours;
     std::vector<vector<Point> > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。
 
     ArmorStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
-    findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, armor_contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);
     if (show_process)imshow("armor struct", src_bin);
 
-    findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, armor_contours_external, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
     for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓
     {
         unsigned long external_contour_size = armor_contours_external[i].size();
@@ -103,12 +103,12 @@ bool Energy::findCenterR(const cv::Mat &src) {
     static Mat src_bin;
     src_bin = src.clone();
     if (src.type() == CV_8UC3) {
-        cvtColor(src_bin, src_bin, CV_BGR2GRAY);
+        cvtColor(src_bin, src_bin, cv::COLOR_BGR2GRAY);
     }
     std::vector<vector<Point> > center_R_contours;
     CenterRStruct(src_bin);
     if (show_process)imshow("R struct", src_bin);
-    findContours(src_bin, center_R_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, center_R_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
     for (auto &center_R_contour : center_R_contours) {
         if (!isValidCenterRContour(center_R_contour)) {
             continue;
@@ -139,13 +139,13 @@ bool Energy::findFlowStripFan(const cv::Mat &src) {
     src_bin = src.clone();
     src_copy = src.clone();
     if (src.type() == CV_8UC3) {
-        cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
+        cvtColor(src_bin, src_bin, cv::COLOR_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
     }
     std::vector<vector<Point> > flow_strip_fan_contours;
     FlowStripFanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
     if (show_process)imshow("flow strip fan struct", src_bin);
 
-    findContours(src_bin, flow_strip_fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, flow_strip_fan_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
     std::vector<cv::RotatedRect> candidate_flow_strip_fans;
 
     for (auto &flow_strip_fan_contour : flow_strip_fan_contours) {
@@ -175,7 +175,7 @@ bool Energy::findFlowStrip(const cv::Mat &src) {
 
     if (src_bin.type() == CV_8UC1) // 黑白图像
     {
-        cvtColor(src_bin, src_bin, COLOR_GRAY2RGB);
+        cvtColor(src_bin, src_bin, cv::COLOR_GRAY2RGB);
 
     }
     std::vector<cv::RotatedRect> candidate_target_armors = target_armors;
@@ -189,13 +189,13 @@ bool Energy::findFlowStrip(const cv::Mat &src) {
         }
     }
 
-    cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
+    cvtColor(src_bin, src_bin, cv::COLOR_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
 
     FlowStripStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
     if (show_process)imshow("flow strip struct", src_bin);
 
     std::vector<vector<Point> > flow_strip_contours;
-    findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, flow_strip_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
 
     for (auto candidate_flow_strip_fan: flow_strip_fans) {
         for (auto &flow_strip_contour : flow_strip_contours) {
@@ -261,7 +261,7 @@ bool Energy::findFlowStripWeak(const cv::Mat &src) {
 
     if (src_bin.type() == CV_8UC1) // 黑白图像
     {
-        cvtColor(src_bin, src_bin, COLOR_GRAY2RGB);
+        cvtColor(src_bin, src_bin, cv::COLOR_GRAY2RGB);
 
     }
     std::vector<cv::RotatedRect> candidate_armors = armors;
@@ -275,13 +275,13 @@ bool Energy::findFlowStripWeak(const cv::Mat &src) {
         }
     }
 
-    cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
+    cvtColor(src_bin, src_bin, cv::COLOR_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
 
     FlowStripStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
     if (show_process)imshow("weak struct", src_bin);
 
     std::vector<vector<Point> > flow_strip_contours;
-    findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+    findContours(src_bin, flow_strip_contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
 
     for (auto &flow_strip_contour : flow_strip_contours) {
         if (!isValidFlowStripContour(flow_strip_contour)) {
@@ -335,6 +335,4 @@ bool Energy::findCenterROI(const cv::Mat &src) {
                                  Size2f(length * 1.7, length * 1.7), -90);
     return true;
 
-}
+}
-
-

energy/src/energy/send/send.cpp

@@ -8,6 +8,13 @@
 
 using namespace std;
 
+// amadeus_26 协议: 15字节帧
+// | SOF(0xBB 0x77) | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | key(1B) | crc8(1B) | EOF(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;
+
 #define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
 
 //----------------------------------------------------------------------------------------------------------------------
@@ -31,23 +38,35 @@ void Energy::sendEnergy() {
     last_yaw = tmp_yaw;
     last_pitch = tmp_pitch;
 
-    if (change_target) {
+    // 能量机关模式通过 key 字段区分
-        sendTarget(serial, yaw_rotation, pitch_rotation, 3, 0);//表示目标切换
+    // 模式编码: 0=普通跟随, 3=目标切换, 4=猜测模式
-    } else if (is_guessing) {
+    uint8_t mode_key = 0;
-        sendTarget(serial, yaw_rotation, pitch_rotation, 4, 0);//表示猜测模式
+    int16_t feed = 0;
-    } else {
+    if (shoot) {
-        sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0);//跟随或发弹
+        feed = 660;  // 开火
     }
 
+    if (change_target) {
+        mode_key = 3;  // 目标切换
+    } else if (is_guessing) {
+        mode_key = 4;  // 猜测模式
+    }
+
+    sendTarget(serial, yaw_rotation, pitch_rotation, feed, mode_key);
 }
 
 
 //----------------------------------------------------------------------------------------------------------------------
-// 此函数用于发送数据给主控板
+// 此函数用于发送数据给主控板 (amadeus_26 协议)
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::sendTarget(Serial &serial, float x, float y, float z, uint16_t u) {
+void Energy::sendTarget(Serial &serial, float yaw, float pitch, int16_t feed, uint8_t key) {
-    short x_tmp, y_tmp, z_tmp;
+    // 控制参数
-    uint8_t buff[10];
+    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);
@@ -55,27 +74,68 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z, uint16_t u) {
     time_t t = time(nullptr);
     if (last_time != t) {
         last_time = t;
-        cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << "," << u << ")" << endl;
+        cout << "Energy: fps:" << fps << ", (" << yaw << "," << pitch << ", feed:" << feed << ", key:" << (int)key << ")" << endl;
         curr_fps = fps;
         fps = 0;
     }
     fps += 1;
 #endif
 
-    x_tmp = static_cast<short>(x * (32768 - 1) / 100);
+    // 将角度 (-3, 3) 度 转换为协议范围 [-660, 660]
-    y_tmp = static_cast<short>(y * (32768 - 1) / 100);
+    // 映射公式: output = input * (660 / 3) = input * 220
-    z_tmp = static_cast<short>(z * (32768 - 1) / 100);
+    yaw_val = static_cast<int16_t>(yaw * 220);
-    buff[0] = 's';
+    pitch_val = static_cast<int16_t>(pitch * 220);
-    buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
+    MINMAX(yaw_val, -660, 660);
-    buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
+    MINMAX(pitch_val, -660, 660);
-    buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
+
-    buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
+    // 构建数据帧 - 与 test_pitch_up.cpp 格式一致
-    buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
+    uint8_t frame[FRAME_LENGTH];
-    buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
+    int idx = 0;
-    buff[7] = static_cast<char>((u >> 8) & 0xFF);
+
-    buff[8] = static_cast<char>((u >> 0) & 0xFF);
+    // 帧头
-    buff[9] = 'e';
+    frame[idx++] = FRAME_HEADER_1;
-    serial.WriteData(buff, sizeof(buff));
+    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, 小端序)
+    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位右拨杆)
+    // key 参数通过低4位传递能量机关模式信息
+    frame[idx++] = (left_switch << 4) | (key & 0x0F);
+
+    // CRC8 (固定为 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"));
 }

main.cpp

@@ -44,7 +44,9 @@ WrapperHead *video = nullptr;    // 云台摄像头视频源
 Serial serial(115200);                  // 串口对象
 uint8_t last_state = ARMOR_STATE;     // 上次状态，用于初始化
 // 自瞄主程序对象
-ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/", mcu_data.anti_top);
+uint8_t enemy_color = ENEMY_BLUE;//RED ro BLUE
+uint8_t forced_anti_top = 1;
+ArmorFinder armor_finder(enemy_color, serial, PROJECT_DIR"/tools/para/", forced_anti_top);
 // 能量机关主程序对象
 Energy energy(serial, mcu_data.enemy_color);
 

others/armor.Config

@@ -5,17 +5,17 @@ resolution :
   image_size : 
   {
     iIndex = 1;
-    acDescription = "640X480 ROI";
+    acDescription = "1280X1024 ROI";
     uBinSumMode = 0;
     uBinAverageMode = 0;
     uSkipMode = 0;
     uResampleMask = 0;
     iHOffsetFOV = 56;
     iVOffsetFOV = 0;
-    iWidthFOV = 640;
+    iWidthFOV = 1280;
-    iHeightFOV = 480;
+    iHeightFOV = 1024;
-    iWidth = 640;
+    iWidth = 1280;
-    iHeight = 480;
+    iHeight = 1024;
     iWidthZoomHd = 0;
     iHeightZoomHd = 0;
     iWidthZoomSw = 0;

others/include/camera/camera_wrapper.h

@@ -6,7 +6,7 @@
 #define _CAMERA_WRAPPER_H_
 
 #include <additions.h>
-#include <opencv2/core/core.hpp>
+#include <opencv2/core.hpp>
 #include <camera/wrapper_head.h>
 
 #ifdef Windows
@@ -19,9 +19,9 @@ class CameraWrapper: public WrapperHead {
     friend void cameraCallback(CameraHandle hCamera, BYTE *pFrameBuffer, tSdkFrameHead* pFrameHead,PVOID pContext);
 private:
     const std::string name;
-    int mode;
+    //int mode;
 
-    bool init_done;
+    //bool init_done;
 
     unsigned char* rgb_buffer;
     int camera_cnts;
@@ -33,13 +33,16 @@ private:
     tSdkCameraCapbility tCapability;
     tSdkFrameHead frame_info;
     BYTE *pby_buffer;
-    IplImage* iplImage;
+    cv::Mat image_header;
     int channel;
 
     RoundQueue<cv::Mat, 2> src_queue;
 public:
     int gain;
     int exposure;
+    
+    int mode;
+    bool init_done;
 
     CameraWrapper(int exposure, int gain, int camera_mode=1, const std::string &n="NULL");
     ~CameraWrapper() final;
@@ -52,4 +55,4 @@ public:
 };
 
 
-#endif /* _CAMERA_WRAPPER_H_ */
+#endif /* _CAMERA_WRAPPER_H_ */

others/include/config/setconfig.h

@@ -4,6 +4,7 @@
 #ifndef _SETCONFIG_H_
 #define _SETCONFIG_H_
 
+#define WITH_CONFIG
 
 #ifdef WITH_CONFIG
     #include <config/config.h>
@@ -49,13 +50,13 @@
     #define YAW_AIM_KI (0.01)
 #endif
 #ifndef PITCH_AIM_KD
-    #define PITCH_AIM_KD (0.4)
+    #define PITCH_AIM_KD (0)
 #endif
 #ifndef PITCH_AIM_KP
-    #define PITCH_AIM_KP (0.75)
+    #define PITCH_AIM_KP (0)
 #endif
 #ifndef PITCH_AIM_KI
-    #define PITCH_AIM_KI (0.01)
+    #define PITCH_AIM_KI (0)
 #endif
 
 #ifndef RED_COMPENSATE_YAW

others/include/constants.h

@@ -19,6 +19,6 @@
 
 #define FOCUS_PIXAL_8MM  (1488)
 #define FOCUS_PIXAL_5MM  (917)
-#define FOCUS_PIXAL      FOCUS_PIXAL_5MM
+#define FOCUS_PIXAL      FOCUS_PIXAL_8MM
 
 #endif /* _CONSTANTS_H */

others/include/log.h

@@ -32,6 +32,7 @@
 
 #include <stdio.h>
 #include <systime.h>
+#include <iostream>
 
 /************** Define the control code *************/
 #define START_CTR           "\033[0"

others/src/additions.cpp

@@ -36,10 +36,10 @@ void uartReceive(Serial *pSerial) {
         pSerial->ReadData((uint8_t *) buffer, sizeof(mcu_data)+1);
         if (buffer[sizeof(mcu_data)] == '\n') {
             memcpy(&mcu_data, buffer, sizeof(mcu_data));
-//            LOGM("Get, state:%c, mark:%d!", mcu_data.state, (int) mcu_data.mark);
+           LOGM("Get, state:%c, mark:%d!", mcu_data.state, (int) mcu_data.mark);
-//            LOGM("Get yaw: %f, pitch: %f!", mcu_data.curr_yaw, mcu_data.curr_pitch);
+           LOGM("Get yaw: %f, pitch: %f!", mcu_data.curr_yaw, mcu_data.curr_pitch);
-//            LOGM("Get delta x: %d, delta y: %d!", mcu_data.delta_x, mcu_data.delta_y);
+            LOGM("Get delta x: %d, delta y: %d!", mcu_data.delta_x, mcu_data.delta_y);
-//            static int t = time(nullptr);
+ //           static int t = time(nullptr);
 //            static int cnt = 0;
 //            if(time(nullptr) > t){
 //                t = time(nullptr);
@@ -77,8 +77,12 @@ bool checkReconnect(bool is_camera_connect) {
     if (!is_camera_connect) {
         int curr_gain = ((CameraWrapper* )video)->gain;
         int curr_exposure = ((CameraWrapper* )video)->exposure;
+        int curr_mode = ((CameraWrapper* )video)->mode; // 获取原始模式
+        
         delete video;
-        video = new CameraWrapper(curr_exposure, curr_gain, 0/*, "armor"*/);
+        std::this_thread::sleep_for(std::chrono::milliseconds(500)); // 等待硬件释放
+        video = new CameraWrapper(curr_exposure, curr_gain, curr_mode/*, "armor"*/);
+        //video = new CameraWrapper(curr_exposure, curr_gain, 0/*, "armor"*/);
         is_camera_connect = video->init();
     }
     return is_camera_connect;
@@ -103,14 +107,14 @@ void extract(cv::Mat &src) {//图像预处理，将视频切成640×480的大小
     if (src.empty()) return;
     float length = static_cast<float>(src.cols);
     float width = static_cast<float>(src.rows);
-    if (length / width > 640.0 / 480.0) {
+    if (length / width > 1280.0 / 1024.0) {
-        length *= 480.0 / width;
+        length *= 1024 / width;
-        resize(src, src, cv::Size(length, 480));
+        resize(src, src, cv::Size(length, 1024));
-        src = src(Rect((length - 640) / 2, 0, 640, 480));
+        src = src(Rect((length - 1280) / 2, 0, 1280, 1024));
     } else {
-        width *= 640.0 / length;
+        width *= 1280.0 / length;
-        resize(src, src, cv::Size(640, width));
+        resize(src, src, cv::Size(1280, width));
-        src = src(Rect(0, (width - 480) / 2, 640, 480));
+        src = src(Rect(0, (width - 1024) / 2, 1280, 1024));
     }
 }
 

others/src/camera/camera_wrapper.cpp

@@ -17,7 +17,6 @@ CameraWrapper::CameraWrapper(int exposure, int gain, int camera_mode, const std:
         mode(camera_mode),
         camera_cnts(2),
         camera_status(-1),
-        iplImage(nullptr),
         rgb_buffer(nullptr),
         channel(3),
         gain(gain),
@@ -27,9 +26,11 @@ CameraWrapper::CameraWrapper(int exposure, int gain, int camera_mode, const std:
 void cameraCallback(CameraHandle hCamera, BYTE *pFrameBuffer, tSdkFrameHead* pFrameHead,PVOID pContext){
     CameraWrapper *c = (CameraWrapper*)pContext;
     CameraImageProcess(hCamera, pFrameBuffer, c->rgb_buffer, pFrameHead);
-    auto iplImage = cvCreateImageHeader(cvSize(pFrameHead->iWidth, pFrameHead->iHeight), IPL_DEPTH_8U, c->channel);
+    // 使用 cv::Mat 替代 IplImage
-    cvSetData(iplImage, c->rgb_buffer, pFrameHead->iWidth * c->channel);  //此处只是设置指针，无图像块数据拷贝，不需担心转换效率
+    cv::Mat img(pFrameHead->iHeight, pFrameHead->iWidth, 
-    c->src_queue.push(cv::cvarrToMat(iplImage).clone());
+                c->channel == 3 ? CV_8UC3 : CV_8UC1, 
+                c->rgb_buffer, pFrameHead->iWidth * c->channel);
+    c->src_queue.push(img.clone());
 }
 
 bool CameraWrapper::init() {
@@ -37,6 +38,7 @@ bool CameraWrapper::init() {
     int camera_enumerate_device_status = CameraEnumerateDevice(camera_enum_list, &camera_cnts);
     if (camera_enumerate_device_status != CAMERA_STATUS_SUCCESS) {
         LOGE("CameraEnumerateDevice fail with %d!", camera_enumerate_device_status);
+        return false;
     }
     if (camera_cnts == 0) {
         LOGE("No camera device detected!");
@@ -134,15 +136,9 @@ bool CameraWrapper::read(cv::Mat &src) {
 
 bool CameraWrapper::readRaw(cv::Mat &src) {
     if (CameraGetImageBuffer(h_camera, &frame_info, &pby_buffer, 500) == CAMERA_STATUS_SUCCESS) {
-        if (iplImage) {
+        // 使用 cv::Mat 替代 IplImage
-            cvReleaseImageHeader(&iplImage);
+        cv::Mat img(frame_info.iHeight, frame_info.iWidth, CV_8UC1, pby_buffer, frame_info.iWidth);
-        }
+        src = img.clone();
-
-        iplImage = cvCreateImageHeader(cvSize(frame_info.iWidth, frame_info.iHeight), IPL_DEPTH_8U, 1);
-
-        cvSetData(iplImage, pby_buffer, frame_info.iWidth);  //此处只是设置指针，无图像块数据拷贝，不需担心转换效率
-
-        src = cv::cvarrToMat(iplImage).clone();
 
         //在成功调用CameraGetImageBuffer后，必须调用CameraReleaseImageBuffer来释放获得的buffer。
         //否则再次调用CameraGetImageBuffer时，程序将被挂起一直阻塞，直到其他线程中调用CameraReleaseImageBuffer来释放了buffer
@@ -156,16 +152,13 @@ bool CameraWrapper::readRaw(cv::Mat &src) {
 }
 
 bool CameraWrapper::readProcessed(cv::Mat &src) {
-//	cerr << "Get-1" << endl;
     if (CameraGetImageBuffer(h_camera, &frame_info, &pby_buffer, 500) == CAMERA_STATUS_SUCCESS) {
         CameraImageProcess(h_camera, pby_buffer, rgb_buffer,
                            &frame_info);  // this function is super slow, better not to use it.
-        if (iplImage) {
+        // 使用 cv::Mat 替代 IplImage
-            cvReleaseImageHeader(&iplImage);
+        int mat_type = (channel == 3) ? CV_8UC3 : CV_8UC1;
-        }
+        cv::Mat img(frame_info.iHeight, frame_info.iWidth, mat_type, rgb_buffer, frame_info.iWidth * channel);
-        iplImage = cvCreateImageHeader(cvSize(frame_info.iWidth, frame_info.iHeight), IPL_DEPTH_8U, channel);
+        src = img.clone();
-        cvSetData(iplImage, rgb_buffer, frame_info.iWidth * channel);  //此处只是设置指针，无图像块数据拷贝，不需担心转换效率
-        src = cv::cvarrToMat(iplImage).clone();
         //在成功调用CameraGetImageBuffer后，必须调用CameraReleaseImageBuffer来释放获得的buffer。
         //否则再次调用CameraGetImageBuffer时，程序将被挂起一直阻塞，直到其他线程中调用CameraReleaseImageBuffer来释放了buffer
         CameraReleaseImageBuffer(h_camera, pby_buffer);
@@ -193,4 +186,4 @@ CameraWrapper::~CameraWrapper() {
     //注意，先反初始化后再free
     if (rgb_buffer != nullptr)
         free(rgb_buffer);
-}
+}

others/src/options.cpp

@@ -6,19 +6,20 @@
 #include <log.h>
 #include <cstring>
 #include <map>
+#include <string>
 
-bool show_armor_box = false;
+bool show_armor_box = true;
 bool show_armor_boxes = false;
 bool show_light_blobs = false;
 bool show_origin = false;
-bool run_with_camera = false;
+bool run_with_camera = true;
 bool save_video = false;
 bool wait_uart = false;
 bool save_labelled_boxes = false;
 bool show_process = false;
 bool show_energy = false;
 bool save_mark = false;
-bool show_info = false;
+bool show_info = true;
 bool run_by_frame = false;
 
 // 使用map保存所有选项及其描述和操作，加快查找速度。

others/src/serial.cpp

@@ -181,7 +181,7 @@ bool Serial::ReadData(unsigned char *buffer,  unsigned int length) {
 using namespace std;
 
 string get_uart_dev_name() {
-    FILE *ls = popen("ls /dev/ttyUSB* --color=never", "r");
+    FILE *ls = popen("ls /dev/ttyCH340 --color=never", "r");
     char name[20] = {0};
     fscanf(ls, "%s", name);
     return name;

serial_debug.cpp

@@ -0,0 +1,269 @@
+/**
+ * @file serial_debug.cpp
+ * @brief 串口调试程序：接收数据并打印解算结果和原始数据
+ * 
+ * 用法: ./serial_debug [串口设备]
+ * 示例: ./serial_debug /dev/ttyCH340
+ */
+
+#include <iostream>
+#include <iomanip>
+#include <cstring>
+#include <vector>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <chrono>
+#include <thread>
+#include <csignal>
+
+// 协议定义
+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 *DEFAULT_SERIAL_PORT = "/dev/ttyCH340";
+constexpr int BAUDRATE = 115200;
+
+// 传感器数据结构
+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;
+};
+
+// 全局标志用于退出
+volatile bool running = true;
+
+void signalHandler(int) {
+    running = false;
+}
+
+// 解析接收到的数据
+bool parseFrame(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;
+}
+
+// 打印原始数据帧（十六进制）
+void printRawData(const uint8_t* data, int len) {
+    std::cout << "Raw Data: ";
+    for (int i = 0; i < len; i++) {
+        std::cout << std::hex << std::uppercase << std::setw(2) << std::setfill('0') 
+                  << static_cast<int>(data[i]) << " ";
+    }
+    std::cout << std::dec << std::nouppercase << std::endl;
+}
+
+// 打印解算的传感器数据
+void printSensorData(const SensorData& sensor) {
+    std::cout << "========================================" << std::endl;
+    std::cout << "  gyro_x: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.gyro_x << std::endl;
+    std::cout << " accel_x: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.accel_x << std::endl;
+    std::cout << " accel_y: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.accel_y << std::endl;
+    std::cout << " accel_z: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.accel_z << std::endl;
+    std::cout << "========================================" << std::endl;
+}
+
+// 解析并打印帧中其他字段
+void printFrameDetails(const uint8_t* data) {
+    int16_t x_move = data[2] | (data[3] << 8);
+    int16_t y_move = data[4] | (data[5] << 8);
+    int16_t yaw = data[6] | (data[7] << 8);
+    int16_t pitch = data[8] | (data[9] << 8);
+    int16_t feed = data[10] | (data[11] << 8);
+    uint8_t switches = data[12];
+    uint8_t left_switch = (switches >> 4) & 0x0F;
+    uint8_t right_switch = switches & 0x0F;
+    uint8_t crc = data[13];
+    
+    std::cout << "Frame Details:" << std::endl;
+    std::cout << "  Header: 0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') 
+              << static_cast<int>(data[0]) << " 0x" << std::setw(2) << static_cast<int>(data[1]) << std::endl;
+    std::cout << std::dec << std::nouppercase;
+    std::cout << "  x_move:  " << std::setw(5) << x_move << std::endl;
+    std::cout << "  y_move:  " << std::setw(5) << y_move << std::endl;
+    std::cout << "  yaw:     " << std::setw(5) << yaw << std::endl;
+    std::cout << "  pitch:   " << std::setw(5) << pitch << std::endl;
+    std::cout << "  feed:    " << std::setw(5) << feed << std::endl;
+    std::cout << "  switch:  L=" << static_cast<int>(left_switch) << " R=" << static_cast<int>(right_switch) << std::endl;
+    std::cout << "  CRC:     0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') 
+              << static_cast<int>(crc) << std::endl;
+    std::cout << std::dec << std::nouppercase;
+    std::cout << "  Tail:    0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') 
+              << static_cast<int>(data[14]) << std::endl;
+    std::cout << std::dec << std::nouppercase << std::endl;
+}
+
+// 初始化串口
+int initSerial(const char* port, int baudrate) {
+    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;
+    }
+    
+    // 设置波特率
+    cfsetospeed(&tty, B115200);
+    cfsetispeed(&tty, B115200);
+    
+    // 8N1
+    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 << "串口 " << port << " 打开成功" << std::endl;
+    return fd;
+}
+
+int main(int argc, char* argv[]) {
+    // 设置信号处理
+    signal(SIGINT, signalHandler);
+    signal(SIGTERM, signalHandler);
+    
+    // 获取串口设备
+    const char* port = (argc > 1) ? argv[1] : DEFAULT_SERIAL_PORT;
+    
+    std::cout << "========================================" << std::endl;
+    std::cout << "  串口调试程序 - Serial Debugger" << std::endl;
+    std::cout << "  设备: " << port << std::endl;
+    std::cout << "  波特率: " << BAUDRATE << std::endl;
+    std::cout << "  按 Ctrl+C 停止" << std::endl;
+    std::cout << "========================================" << std::endl << std::endl;
+    
+    // 初始化串口
+    int fd = initSerial(port, BAUDRATE);
+    if (fd < 0) {
+        return -1;
+    }
+    
+    uint8_t buffer[256];
+    std::vector<uint8_t> frame_buffer;
+    frame_buffer.reserve(FRAME_LENGTH);
+    
+    int frame_count = 0;
+    int valid_count = 0;
+    auto start_time = std::chrono::steady_clock::now();
+    
+    while (running) {
+        // 读取数据
+        ssize_t n = read(fd, buffer, sizeof(buffer));
+        
+        if (n > 0) {
+            for (ssize_t i = 0; i < n; i++) {
+                frame_buffer.push_back(buffer[i]);
+                
+                // 查找帧头
+                if (frame_buffer.size() >= 2 && 
+                    frame_buffer[0] == FRAME_HEADER_1 && 
+                    frame_buffer[1] == FRAME_HEADER_2) {
+                    
+                    // 等待完整帧
+                    if (frame_buffer.size() >= FRAME_LENGTH) {
+                        frame_count++;
+                        
+                        // 打印帧号和时间
+                        auto now = std::chrono::steady_clock::now();
+                        auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - start_time).count();
+                        std::cout << "\n========== Frame #" << frame_count 
+                                  << " (Time: " << elapsed << "s) ==========" << std::endl;
+                        
+                        // 打印原始数据
+                        printRawData(frame_buffer.data(), FRAME_LENGTH);
+                        
+                        // 检查帧尾
+                        if (frame_buffer[FRAME_LENGTH - 1] == FRAME_TAIL) {
+                            // 解析并打印数据
+                            SensorData sensor;
+                            if (parseFrame(frame_buffer.data(), sensor)) {
+                                valid_count++;
+                                std::cout << "Status: VALID" << std::endl;
+                                printSensorData(sensor);
+                            }
+                            
+                            // 打印帧详情
+                            printFrameDetails(frame_buffer.data());
+                        } else {
+                            std::cout << "Status: INVALID (Frame tail error)" << std::endl;
+                            std::cout << "Expected: 0x" << std::hex << std::uppercase 
+                                      << std::setw(2) << std::setfill('0') 
+                                      << static_cast<int>(FRAME_TAIL) << std::endl;
+                            std::cout << "Got:      0x" << std::setw(2) 
+                                      << static_cast<int>(frame_buffer[FRAME_LENGTH - 1]) << std::endl;
+                            std::cout << std::dec << std::nouppercase << std::endl;
+                        }
+                        
+                        // 清空缓冲区
+                        frame_buffer.clear();
+                    }
+                } else if (frame_buffer.size() > FRAME_LENGTH) {
+                    // 缓冲区溢出，丢弃数据
+                    frame_buffer.erase(frame_buffer.begin());
+                }
+            }
+        } else if (n < 0 && errno != EAGAIN) {
+            std::cerr << "读取错误: " << strerror(errno) << std::endl;
+        }
+        
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    
+    // 打印统计信息
+    std::cout << "\n========================================" << std::endl;
+    std::cout << "  统计信息:" << std::endl;
+    std::cout << "  总帧数: " << frame_count << std::endl;
+    std::cout << "  有效帧: " << valid_count << std::endl;
+    std::cout << "  有效率: " << (frame_count > 0 ? (100.0 * valid_count / frame_count) : 0) << "%" << std::endl;
+    std::cout << "========================================" << std::endl;
+    
+    close(fd);
+    std::cout << "串口已关闭" << std::endl;
+    return 0;
+}

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/99-usb-serial.rules

@@ -0,0 +1,5 @@
+# CH340 串口模块 (ttyUSB0 -> ttyCH340)
+SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", SYMLINK+="ttyCH340", MODE="0666", GROUP="dialout"
+
+# IMU 串口模块 (ttyUSB1 -> ttyIMU) 
+SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", SYMLINK+="ttyIMU", MODE="0666", GROUP="dialout"

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
    

tools/tty-permission.sh

@@ -0,0 +1,38 @@
+#!/bin/bash
+# USB 串口设备权限和固定名称设置脚本
+
+set -e
+
+echo "=================================="
+echo "USB 串口设备权限设置"
+echo "=================================="
+
+# 1. 检查当前设备
+echo ""
+echo "当前连接的 USB 串口设备："
+ls -la /dev/ttyUSB* 2>/dev/null || echo "未发现 ttyUSB 设备"
+
+echo ""
+echo "设备详细信息："
+for dev in /dev/ttyUSB*; do
+    if [ -e "$dev" ]; then
+        echo "--- $dev ---"
+        udevadm info -a -n "$dev" | grep -E "(idVendor|idProduct|serial)" | head -5
+    fi
+done
+
+# 2. 安装规则文件
+echo ""
+echo "安装 udev 规则"
+sudo cp 99-usb-serial.rules /etc/udev/rules.d/
+
+# 3. 重新加载 udev 规则
+echo "重新加载 udev 规则"
+sudo udevadm control --reload-rules
+sudo udevadm trigger
+
+# 4. 添加用户到 dialout 组
+echo "将当前用户添加到 dialout 组"
+sudo usermod -aG dialout $USER
+
+echo "设置完成，请重新插拔设备"