该一下

assets/camera_calib/1.txt

@@ -0,0 +1 @@
+0.7945099292803862 -0.0434575467108611 0.07481947984239669 0.601055288093252

assets/camera_calib/10.txt

@@ -0,0 +1 @@
+0.8320261217979533 -0.010711187380915883 0.034853447736525024 0.5535368463721503

assets/camera_calib/11.txt

@@ -0,0 +1 @@
+0.8173712544780483 -0.011828605094770673 0.03328423249804994 0.5750273700629471

assets/camera_calib/12.txt

@@ -0,0 +1 @@
+0.7984894363309989 -0.013758457040363277 0.032468299086838105 0.6009751529656445

assets/camera_calib/13.txt

@@ -0,0 +1 @@
+0.7800486898114236 -0.012651950562476097 0.03220614946153963 0.624761341319473

assets/camera_calib/14.txt

@@ -0,0 +1 @@
+0.7710456452265627 -0.02186493764130126 0.04513486486644217 0.6348018442416371

assets/camera_calib/15.txt

@@ -0,0 +1 @@
+0.7916711278986903 -0.07209407556694548 0.11214836806575476 0.5962231235535496

assets/camera_calib/16.txt

@@ -0,0 +1 @@
+0.8045946318607127 -0.06785936367345102 0.11352663362431434 0.5789078411981385

assets/camera_calib/17.txt

@@ -0,0 +1 @@
+0.8215749957916847 -0.06455802999670587 0.11384191417076965 0.5548754866010687

assets/camera_calib/18.txt

@@ -0,0 +1 @@
+0.8368116765764088 -0.06352479955713666 0.11690362951903568 0.5310784868471715

assets/camera_calib/19.txt

@@ -0,0 +1 @@
+0.7659234104878251 -0.05152390626763256 0.07971188746430936 0.6358872788053714

assets/camera_calib/2.txt

@@ -0,0 +1 @@
+0.7711539361232197 -0.04474533855113315 0.07190790375467529 0.6309902652634334

assets/camera_calib/20.txt

@@ -0,0 +1 @@
+0.7843599785188415 -0.07036673754547611 0.10757796487882619 0.6068401171788649

assets/camera_calib/21.txt

@@ -0,0 +1 @@
+0.8107296801983674 -0.06562048651309176 0.11368406086180097 0.5705149180356358

assets/camera_calib/22.txt

@@ -0,0 +1 @@
+0.7935983038966267 -0.033809170582879966 0.06190002705631246 0.6043401845710105

assets/camera_calib/23.txt

@@ -0,0 +1 @@
+0.8237228560732456 -0.030521906459115355 0.06638363742866517 0.5622653130777103

assets/camera_calib/24.txt

@@ -0,0 +1 @@
+0.8491398787146873 -0.025532247411769002 0.05836177108338319 0.5243123824539347

assets/camera_calib/3.txt

@@ -0,0 +1 @@
+0.7548850619033333 -0.047418777216818586 0.07093119403444739 0.650283606279068

assets/camera_calib/4.txt

@@ -0,0 +1 @@
+0.7591377331693356 -0.04763573374267206 0.07336706642951026 0.6450255905875732

assets/camera_calib/5.txt

@@ -0,0 +1 @@
+0.7583547886588434 -0.022422481447733462 0.044038113363287834 0.6499660694338815

assets/camera_calib/6.txt

@@ -0,0 +1 @@
+0.7789208034949294 -0.021997736668215327 0.048119363872661865 0.624886396303184

assets/camera_calib/7.txt

@@ -0,0 +1 @@
+0.8037386906226677 -0.020333318885384567 0.048369913784180284 0.5926643441103707

assets/camera_calib/8.txt

@@ -0,0 +1 @@
+0.8256625130752344 -0.020650051549974013 0.04936930246923488 0.5616205675070786

assets/camera_calib/9.txt

@@ -0,0 +1 @@
+0.8377166207827541 -0.020123362855542434 0.049144700467202415 0.5435169840471376

build.sh

@@ -0,0 +1,2 @@
+cmake -B build
+make -C build/ -j`nproc`

cali.MD

@@ -0,0 +1,2 @@
+./build/capture configs/calibration.yaml -o assets/camera_calib
+./build/calibrate_camera assets/camera_calib

calibration/capture.cpp

@@ -5,7 +5,7 @@
 #include <opencv2/opencv.hpp>
 
 #include "io/camera.hpp"
-#include "io/cboard.hpp"
+#include "io/gimbal/gimbal.hpp"
 #include "tools/img_tools.hpp"
 #include "tools/logger.hpp"
 #include "tools/math_tools.hpp"
@@ -25,9 +25,9 @@ void write_q(const std::string q_path, const Eigen::Quaterniond & q)
 }
 
 void capture_loop(
-  const std::string & config_path, const std::string & can, const std::string & output_folder)
+  const std::string & config_path, const std::string & output_folder)
 {
-  io::CBoard cboard(config_path);
+  io::Gimbal gimbal(config_path);
   io::Camera camera(config_path);
   cv::Mat img;
   std::chrono::steady_clock::time_point timestamp;
@@ -35,14 +35,14 @@ void capture_loop(
   int count = 0;
   while (true) {
     camera.read(img, timestamp);
-    Eigen::Quaterniond q = cboard.imu_at(timestamp);
+    Eigen::Quaterniond q = gimbal.q(timestamp);
 
     // 在图像上显示欧拉角，用来判断imuabs系的xyz正方向，同时判断imu是否存在零漂
     auto img_with_ypr = img.clone();
-    Eigen::Vector3d zyx = tools::eulers(q, 2, 1, 0) * 57.3;  // degree
+    Eigen::Vector3d zyx = tools::eulers(q, 2, 1, 0);  // degree
-    tools::draw_text(img_with_ypr, fmt::format("Z {:.2f}", zyx[0]), {40, 40}, {0, 0, 255});
+    tools::draw_text(img_with_ypr, fmt::format("Yaw   (Z) {:.2f}", zyx[0]), {40, 40}, {0, 0, 255});
-    tools::draw_text(img_with_ypr, fmt::format("Y {:.2f}", zyx[1]), {40, 80}, {0, 0, 255});
+    tools::draw_text(img_with_ypr, fmt::format("Pitch (Y) {:.2f}", zyx[1]), {40, 80}, {0, 0, 255});
-    tools::draw_text(img_with_ypr, fmt::format("X {:.2f}", zyx[2]), {40, 120}, {0, 0, 255});
+    tools::draw_text(img_with_ypr, fmt::format("Roll  (X) {:.2f}", zyx[2]), {40, 120}, {0, 0, 255});
 
     std::vector<cv::Point2f> centers_2d;
     auto success = cv::findCirclesGrid(img, cv::Size(10, 7), centers_2d);  // 默认是对称圆点图案
@@ -66,7 +66,7 @@ void capture_loop(
     tools::logger()->info("[{}] Saved in {}", count, output_folder);
   }
 
-  // 离开该作用域时，camera和cboard会自动关闭
+  // 离开该作用域时，camera和gimbal会自动关闭
 }
 
 int main(int argc, char * argv[])
@@ -85,7 +85,7 @@ int main(int argc, char * argv[])
 
   tools::logger()->info("默认标定板尺寸为10列7行");
   // 主循环，保存图片和对应四元数
-  capture_loop(config_path, "can0", output_folder);
+  capture_loop(config_path, output_folder);
 
   tools::logger()->warn("注意四元数输出顺序为wxyz");
 

configs/calibration.yaml

@@ -1,12 +1,25 @@
 pattern_cols: 10
 pattern_rows: 7
-center_distance_mm: 40
+center_distance_mm: 39
 
-R_gimbal2imubody: [1, 0, 0, 0, 1, 0,  0, 0, 1]
+# 重投影误差: 0.0840px
+camera_matrix: [1834.1536403391224, 0, 748.34431374203518, 0, 1838.4254732917539, 592.07179931054554, 0, 0, 1]
+distort_coeffs: [-0.089753587550662386, 0.25043670633576026, 0.00028701704204080067, 0.00026068590699182429, 0]
+
+R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+
+# 相机同理想情况的偏角: yaw0.65 pitch0.98 roll-4.71 degree
+# 标定板到世界坐标系原点的水平距离: 1.20 m
+# 标定板同竖直摆放时的偏角: yaw76.91 pitch5.90 roll-4.25 degree
+R_camera2gimbal: [-0.012713492697510044, 0.016094161023931495, 0.99978965041870982, -0.9965458575215137, -0.082265286272855259, -0.011347974760117781, 0.082065345671248222, -0.99648050691189183, 0.017084448614971628]
+t_camera2gimbal: [0.099890931342671643, 0.0015513845612699661, 0.048389617922529167]
+
+#####-----gimbal参数-----#####
+com_port: "/dev/ttyACM0"
 
 camera_name: "hikrobot"
-exposure_ms: 3
+exposure_ms: 20
-gain: 10.0
+gain: 50.0
 vid_pid: "2bdf:0001"
 
 #####-----cboard参数-----#####

configs/camera.yaml

@@ -1,9 +1,9 @@
-camera_name: "mindvision"
+# camera_name: "mindvision"
-exposure_ms: 2
+# exposure_ms: 2
-gamma: 0.5
+# gamma: 0.5
-vid_pid: "f622:d13a"
+# vid_pid: "f622:d13a"
 
-# camera_name: "hikrobot"
+camera_name: "hikrobot"
-# exposure_ms: 3
+exposure_ms: 3
-# gain: 10.0
+gain: 10.0
-# vid_pid: "2bdf:0001"
+vid_pid: "2bdf:0001"

configs/handeye.yaml

@@ -0,0 +1,30 @@
+# 手眼标定测试配置文件
+# 该配置用于测试手眼标定结果的准确性
+
+# 测试网格参数
+height: 2.0        # 测试平面距离地面高度（米）
+grid_num: 5        # 网格数量（5x5网格）
+grid_size: 0.5     # 网格间距（米）
+delay: 15          # 时间补偿延迟（毫秒）
+
+# 相机内参（从标定结果复制）
+camera_matrix: [1824.3445767434343, 0, 745.88898611634079, 0, 1828.9000780816298, 584.45884364277663, 0, 0, 1]
+distort_coeffs: [-0.084922638867033884, 0.22051971879198334, -0.00021700148630802023, 0.00043485717343934131, 0]
+
+# 手眼标定结果（从标定结果复制）
+R_gimbal2imubody: [1, 0, 0, 0, -1, 0, 0, 0, 1]
+R_camera2gimbal: [0.093296993396547734, 0.14503826005617809, -0.9850175501700672, -0.99514483056044212, -0.017561170025079953, -0.096841992524828474, -0.03134385477239883, 0.98927018980015091, 0.14269567036452102]
+t_camera2gimbal: [-0.11903236791933375, -0.012143601357090483, -0.023682825149991701]
+
+# 相机参数
+camera_name: "hikrobot"
+exposure_ms: 50
+gain: 50.0
+vid_pid: "2bdf:0001"
+
+# 下位机参数
+com_port: "/dev/ttyACM0"
+quaternion_canid: 0x01
+bullet_speed_canid: 0x110
+send_canid: 0xff
+can_interface: "can0"

configs/sentry.yaml

@@ -1,7 +1,6 @@
 # enemy_color: "red"
 enemy_color: "blue"
 
-
 #####-----神经网络参数-----#####
 yolo_name: yolov5
 classify_model: assets/tiny_resnet.onnx
@@ -9,7 +8,7 @@ yolo11_model_path: assets/yolo11.xml
 yolov8_model_path: assets/yolov8.xml
 yolov5_model_path: assets/yolov5.xml
 device: GPU
-min_confidence: 0.8
+min_confidence: 0.75
 use_traditional: true
 
 #####-----ROI-----#####
@@ -21,24 +20,6 @@ roi:
 
 use_roi: false
 
-#####-----USB相机参数-----#####
-image_width: 1280
-image_height: 720
-fov_h: 57.7 #87.7
-fov_v: 56.7 
-new_fov_h: 27 #67
-new_fov_v: 40.9
-usb_frame_rate: 120
-usb_exposure: 500 #1-80000______250
-usb_gamma: 160
-usb_gain: 10 #0-96
-
-#####-----工业相机参数-----#####
-camera_name: "hikrobot"
-exposure_ms: 0.8
-gain: 16.9
-vid_pid: "2bdf:0001"
-
 #####-----传统方法参数-----#####
 threshold: 150
 max_angle_error: 45 # degree
@@ -49,48 +30,71 @@ min_armor_ratio: 1
 max_armor_ratio: 5
 max_side_ratio: 1.5
 max_rectangular_error: 25 # degree
-min_confidence: 0.8
-
-#####-----cboard参数-----#####
-quaternion_canid: 0x01
-bullet_speed_canid: 0x110
-send_canid: 0xff
-can_interface: "can0"
 
 #####-----tracker参数-----#####
 min_detect_count: 5
-max_temp_lost_count: 25
+max_temp_lost_count: 15
 outpost_max_temp_lost_count: 75
 
 #####-----aimer参数-----#####
-yaw_offset: -0.8 # degree -2.5
+yaw_offset: 2 # degree -2.5
-pitch_offset: -1  # degree 2
+pitch_offset: -0.5 # degree 2
-comming_angle: 60 # degree
+comming_angle: 55 # degree
 leaving_angle: 20 # degree
-left_yaw_offset: -1
+decision_speed: 1.57 # rad/s
-right_yaw_offset: -0.6
+high_speed_delay_time: 0.0 # s
-decision_speed: 10 # rad/s
+low_speed_delay_time: 0.0 # s planner use this value
-high_speed_delay_time: 0.026 # s
-low_speed_delay_time: 0.010 # s
 
 #####-----shooter参数-----#####
-first_tolerance: 5 # 近距离射击容差，degree
+first_tolerance: 3 # 近距离射击容差，degree
 second_tolerance: 2 # 远距离射击容差，degree
-judge_distance: 3 #距离判断阈值
+judge_distance: 2 #距离判断阈值
 auto_fire: true # 是否由自瞄控制射击
 
-#####-----decider参数-----#####
+camera_name: "hikrobot"
-mode: 1
+exposure_ms: 2.5
+gain: 16.9
+vid_pid: "2bdf:0001"
+
+# 重投影误差: 0.0840px
+camera_matrix: [1834.1536403391224, 0, 748.34431374203518, 0, 1838.4254732917539, 592.07179931054554, 0, 0, 1]
+distort_coeffs: [-0.089753587550662386, 0.25043670633576026, 0.00028701704204080067, 0.00026068590699182429, 0]
 
-#####-----工业相机标定参数-----#####
 R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1]
 
-# 重投影误差: 0.1833px
+# 相机同理想情况的偏角: yaw0.65 pitch0.98 roll-4.71 degree
-camera_matrix: [2414.9359264386621, 0, 717.26243105567414, 0, 2418.0489262208148, 582.68540529942845, 0, 0, 1]
-distort_coeffs: [-0.0209453389287673, 0.15028138841073832, -0.0006517722113234505, -0.0016861906197686788, 0]
-
-# 相机同理想情况的偏角: yaw-1.11 pitch0.01 roll-0.06 degree
 # 标定板到世界坐标系原点的水平距离: 1.20 m
-# 标定板同竖直摆放时的偏角: yaw123.89 pitch14.05 roll-0.86 degree
+# 标定板同竖直摆放时的偏角: yaw76.91 pitch5.90 roll-4.25 degree
-R_camera2gimbal: [0.01928451708725664, 0.00012696140743255463, 0.99981402834802846, -0.99981344688553653, -0.0010834913551969569, 0.019284643459122196, 0.0010857382619952124, -0.99999940496346484, 0.00010604311483339372]
+R_camera2gimbal: [-0.012713492697510044, 0.016094161023931495, 0.99978965041870982, -0.9965458575215137, -0.082265286272855259, -0.011347974760117781, 0.082065345671248222, -0.99648050691189183, 0.017084448614971628]
-t_camera2gimbal: [0.13089617453251859, 0.0038468007459533785, 0.094139945222010288]
+t_camera2gimbal: [0.099890931342671643, 0.0015513845612699661, 0.048389617922529167]
+
+#####-----cboard参数-----#####
+quaternion_canid: 0x100
+bullet_speed_canid: 0x101
+send_canid: 0xff
+can_interface: "can0"
+
+#####-----gimbal参数-----#####
+com_port: "/dev/ttyUSB0"
+yaw_kp: 0
+yaw_kd: 0
+pitch_kp: 0
+pitch_kd: 0
+
+#####-----planner-----#####
+fire_thresh: 0.0035
+
+max_yaw_acc: 50
+Q_yaw: [9e6, 0]
+R_yaw: [1]
+
+max_pitch_acc: 100
+Q_pitch: [9e6, 0]
+R_pitch: [1]
+
+#####-----buff_detector参数-----#####
+model: "assets/yolo11_buff_int8.xml"
+
+#####-----buff_aimer参数-----#####
+fire_gap_time: 0.700   # s
+predict_time: 0.120     # s

configs/standard3.yaml

@@ -61,15 +61,15 @@ vid_pid: "2bdf:0001"
 #  0  0  1
 R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1]
 
-# 重投影误差: 0.1820px
+# 重投影误差: 0.0833px
-camera_matrix: [1785.4881526822305, 0, 672.4806478241826, 0, 1785.026019470562, 559.89603224794314, 0, 0, 1]
+camera_matrix: [1824.3445767434343, 0, 745.88898611634079, 0, 1828.9000780816298, 584.45884364277663, 0, 0, 1]
-distort_coeffs: [-0.076005079619881746, 0.11182817466388446, 0.0005362204787722057, -0.0027546300984895122, 0]
+distort_coeffs: [-0.084922638867033884, 0.22051971879198334, -0.00021700148630802023, 0.00043485717343934131, 0]
 
-# 相机同理想情况的偏角: yaw1.44 pitch-7.28 roll0.96 degree
+# 相机同理想情况的偏角: yaw-2.19 pitch0.60 roll-1.22 degree
 # 标定板到世界坐标系原点的水平距离: 1.13 m
-# 标定板同竖直摆放时的偏角: yaw7.61 pitch13.92 roll-0.46 degree
+# 标定板同竖直摆放时的偏角: yaw-82.48 pitch10.19 roll-0.28 degree
-R_camera2gimbal: [-0.027182119030230909, -0.12616154330853446, 0.99163723074269183, -0.99949106557517331, 0.019998323121329122, -0.024853106601381177, -0.016695575474690555, -0.99180811252093692, -0.12664093215554434]
+R_camera2gimbal: [0.037945134408690975, 0.011210224413650311, 0.99921694223191693, -0.99905391724335679, -0.020835442782907131, 0.038172696587273909, 0.021247051922802046, -0.99972006841547179, 0.010409014936772491]
-t_camera2gimbal: [0.13160669975045827, 0.10377721766577375, 0.024908271912914642]
+t_camera2gimbal: [0.14266900667253352, 0.0061791593160151776, 0.051417922179356944]
 
 #####-----cboard参数-----#####
 quaternion_canid: 0x100
@@ -78,7 +78,7 @@ send_canid: 0xff
 can_interface: "can0"
 
 #####-----gimbal参数-----#####
-com_port: "/dev/gimbal"
+com_port: "/dev/ttyACM0"
 yaw_kp: 0
 yaw_kd: 0
 pitch_kp: 0

io/gimbal/gimbal.cpp

@@ -1,5 +1,6 @@
 #include "gimbal.hpp"
 
+#include "serial/serial.h"
 #include "tools/crc.hpp"
 #include "tools/logger.hpp"
 #include "tools/math_tools.hpp"
@@ -14,6 +15,13 @@ Gimbal::Gimbal(const std::string & config_path)
 
   try {
     serial_.setPort(com_port);
+    serial_.setBaudrate(115200);  // 设置波特率
+    serial_.setFlowcontrol(serial::flowcontrol_none);
+    serial_.setParity(serial::parity_none);
+    serial_.setStopbits(serial::stopbits_one);
+    serial_.setBytesize(serial::eightbits);
+    serial::Timeout timeout = serial::Timeout::simpleTimeout(100);  // 100ms超时
+    serial_.setTimeout(timeout);
     serial_.open();
   } catch (const std::exception & e) {
     tools::logger()->error("[Gimbal] Failed to open serial: {}", e.what());
@@ -145,7 +153,7 @@ void Gimbal::read_thread()
       continue;
     }
 
-    if (rx_data_.head[0] != 'S' || rx_data_.head[1] != 'P') continue;
+    if (rx_data_.head[0] != 'M' || rx_data_.head[1] != 'R') continue;
 
     auto t = std::chrono::steady_clock::now();
 
@@ -155,12 +163,47 @@ void Gimbal::read_thread()
       error_count++;
       continue;
     }
-
+    float q0 = rx_data_.q[0], q1 = rx_data_.q[1], q2 = rx_data_.q[2], q3 = rx_data_.q[3];
+    float yaw = rx_data_.yaw, yaw_vel = rx_data_.yaw_vel;
+    float pitch = rx_data_.pitch, pitch_vel = rx_data_.pitch_vel;
+    float bullet_speed = rx_data_.bullet_speed;
+    uint16_t bullet_count = rx_data_.bullet_count, crc16 = rx_data_.crc16;
+    
     if (!tools::check_crc16(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_))) {
       tools::logger()->debug("[Gimbal] CRC16 check failed.");
+      tools::logger()->debug(
+        "[Gimbal] Received crc16: {}, Calculated crc16: {}",
+        crc16,
+        tools::get_crc16(
+          reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_) - sizeof(rx_data_.crc16)));
       continue;
     }
+    /*打印受到的数据*/
+// struct __attribute__((packed)) GimbalToVision
+// {
+//   uint8_t head[2] = {'M', 'R'};
+//   uint8_t mode;  // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
+//   float q[4];    // wxyz顺序
+//   float yaw;
+//   float yaw_vel;
+//   float pitch;
+//   float pitch_vel;
+//   float bullet_speed;
+//   uint16_t bullet_count;  // 子弹累计发送次数
+//   uint16_t crc16;
+// };
 
+    // float q0 = rx_data_.q[0], q1 = rx_data_.q[1], q2 = rx_data_.q[2], q3 = rx_data_.q[3];
+    // float yaw = rx_data_.yaw, yaw_vel = rx_data_.yaw_vel;
+    // float pitch = rx_data_.pitch, pitch_vel = rx_data_.pitch_vel;
+    // float bullet_speed = rx_data_.bullet_speed;
+    // uint16_t bullet_count = rx_data_.bullet_count, crc16 = rx_data_.crc16;
+    
+    // tools::logger()->debug(
+    //   "[Gimbal] mode: {}, q: [{:.3f}, {:.3f}, {:.3f}, {:.3f}], yaw: {:.3f}, yaw_vel: {:.3f}, "
+    //   "pitch: {:.3f}, pitch_vel: {:.3f}, bullet_speed: {:.3f}, bullet_count: {}, crc16: {}",
+    //   rx_data_.mode, q0, q1, q2, q3, yaw, yaw_vel, pitch, pitch_vel, bullet_speed,
+    //   bullet_count, crc16);
     error_count = 0;
     Eigen::Quaterniond q(rx_data_.q[0], rx_data_.q[1], rx_data_.q[2], rx_data_.q[3]);
     queue_.push({q, t});

io/hikrobot/hikrobot.cpp

@@ -116,7 +116,21 @@ void HikRobot::capture_start()
       }
 
       auto timestamp = std::chrono::steady_clock::now();
-      cv::Mat img(cv::Size(raw.stFrameInfo.nWidth, raw.stFrameInfo.nHeight), CV_8U, raw.pBufAddr);
+      
+      const auto & frame_info = raw.stFrameInfo;
+      auto pixel_type = frame_info.enPixelType;
+      
+      // Determine image type based on pixel format
+      int cv_type = CV_8U;
+      bool is_color = false;
+      
+      // Check if it's a color format (RGB or BGR already converted)
+      if (pixel_type == PixelType_Gvsp_RGB8_Packed || pixel_type == PixelType_Gvsp_BGR8_Packed) {
+        cv_type = CV_8UC3;
+        is_color = true;
+      }
+      
+      cv::Mat img(cv::Size(raw.stFrameInfo.nWidth, raw.stFrameInfo.nHeight), cv_type, raw.pBufAddr);
 
       cvt_param.nWidth = raw.stFrameInfo.nWidth;
       cvt_param.nHeight = raw.stFrameInfo.nHeight;
@@ -130,18 +144,35 @@ void HikRobot::capture_start()
       cvt_param.enDstPixelType = PixelType_Gvsp_BGR8_Packed;
 
       // ret = MV_CC_ConvertPixelType(handle_, &cvt_param);
-      const auto & frame_info = raw.stFrameInfo;
-      auto pixel_type = frame_info.enPixelType;
       cv::Mat dst_image;
       const static std::unordered_map<MvGvspPixelType, cv::ColorConversionCodes> type_map = {
-        {PixelType_Gvsp_BayerGR8, cv::COLOR_BayerGR2RGB},
+        {PixelType_Gvsp_BayerGR8, cv::COLOR_BayerGR2BGR},
-        {PixelType_Gvsp_BayerRG8, cv::COLOR_BayerRG2RGB},
+        {PixelType_Gvsp_BayerRG8, cv::COLOR_BayerRG2BGR},
-        {PixelType_Gvsp_BayerGB8, cv::COLOR_BayerGB2RGB},
+        {PixelType_Gvsp_BayerGB8, cv::COLOR_BayerGB2BGR},
-        {PixelType_Gvsp_BayerBG8, cv::COLOR_BayerBG2RGB}};
+        {PixelType_Gvsp_BayerBG8, cv::COLOR_BayerBG2BGR},
-      cv::cvtColor(img, dst_image, type_map.at(pixel_type));
+        {PixelType_Gvsp_BayerRBGG8, cv::COLOR_BayerRG2BGR}};
-      img = dst_image;
+      
+      auto it = type_map.find(pixel_type);
+      if (it != type_map.end()) {
+        cv::cvtColor(img, dst_image, it->second);
+        img = dst_image;
+      } else if (is_color && pixel_type == PixelType_Gvsp_RGB8_Packed) {
+        // Convert RGB to BGR
+        cv::cvtColor(img, dst_image, cv::COLOR_RGB2BGR);
+        img = dst_image;
+      } else if (is_color && pixel_type == PixelType_Gvsp_BGR8_Packed) {
+        // BGR format is already correct for OpenCV, no conversion needed
+        img = img;
+      } else if (!is_color) {
+        tools::logger()->warn("Unsupported pixel type: {:#x}. Camera may be outputting already converted format. Using raw image.", pixel_type);
+        // If pixel format is not in the map, the image might already be in the desired format
+        img = img;
+      }
+      
+      // Swap R and B channels to fix color inversion (red becoming blue)
+      cv::cvtColor(img, img, cv::COLOR_BGR2RGB);
 
-      queue_.push({img, timestamp});
+      queue_.push({img.clone(), timestamp});
 
       ret = MV_CC_FreeImageBuffer(handle_, &raw);
       if (ret != MV_OK) {

kernel.errors.txt

@@ -0,0 +1,16 @@
+Instruction / Operand / Region Errors:
+
+/-------------------------------------------!!!KERNEL HEADER ERRORS FOUND!!!-------------------------------------------\
+Error in CISA routine with name: kernel
+                  Error Message: Input V38 = [256, 260) intersects with V37 = [256, 260)
+\----------------------------------------------------------------------------------------------------------------------/
+
+
+/-------------------------------------------!!!KERNEL HEADER ERRORS FOUND!!!-------------------------------------------\
+Error in CISA routine with name: kernel
+                  Error Message: Explicit input 2 must not follow an implicit input 0
+\----------------------------------------------------------------------------------------------------------------------/
+
+
+
+

src/auto_aim_debug_mpc.cpp

@@ -62,6 +62,12 @@ int main(int argc, char * argv[])
         plan.control, plan.fire, plan.yaw, plan.yaw_vel, plan.yaw_acc, plan.pitch, plan.pitch_vel,
         plan.pitch_acc);
 
+      tools::logger()->info(
+        "Target: yaw={:.3f}, pitch={:.3f} | Plan: yaw={:.3f}, pitch={:.3f} | Control={}, Fire={} | "
+        "Bullet_speed={:.2f}m/s",
+        plan.target_yaw, plan.target_pitch, plan.yaw, plan.pitch, plan.control, plan.fire,
+        gs.bullet_speed);
+
       auto fired = gs.bullet_count > last_bullet_count;
       last_bullet_count = gs.bullet_count;