From 5137f2bcdd383a4e2169ef365cb14b38138f32e0 Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Fri, 2 Jan 2026 12:53:28 +0800
Subject: [PATCH] =?UTF-8?q?=E6=94=B9=E6=8E=A5=E5=8F=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 calibration/capture.cpp  | 12 ++++++------
 configs/calibration.yaml |  5 ++++-
 io/gimbal/gimbal.cpp     | 18 +++++++++++++-----
 3 files changed, 23 insertions(+), 12 deletions(-)

diff --git a/calibration/capture.cpp b/calibration/capture.cpp
index c33a70e..863d772 100644
--- a/calibration/capture.cpp
+++ b/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,7 +35,7 @@ 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();
@@ -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");
 
diff --git a/configs/calibration.yaml b/configs/calibration.yaml
index b080281..83edef5 100644
--- a/configs/calibration.yaml
+++ b/configs/calibration.yaml
@@ -4,9 +4,12 @@ center_distance_mm: 40
 
 R_gimbal2imubody: [1, 0, 0, 0, 1, 0,  0, 0, 1]
 
+#####-----gimbal参数-----#####
+com_port: "/dev/ttyACM1"
+
 camera_name: "hikrobot"
 exposure_ms: 3
-gain: 10.0
+gain: 20.0
 vid_pid: "2bdf:0001"
 
 #####-----cboard参数-----#####
diff --git a/io/gimbal/gimbal.cpp b/io/gimbal/gimbal.cpp
index a525193..5775701 100644
--- a/io/gimbal/gimbal.cpp
+++ b/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();
 
@@ -156,10 +164,10 @@ void Gimbal::read_thread()
       continue;
     }
 
-    if (!tools::check_crc16(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_))) {
-      tools::logger()->debug("[Gimbal] CRC16 check failed.");
-      continue;
-    }
+    // if (!tools::check_crc16(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_))) {
+    //   tools::logger()->debug("[Gimbal] CRC16 check failed.");
+    //   continue;
+    // }
 
     error_count = 0;
     Eigen::Quaterniond q(rx_data_.q[0], rx_data_.q[1], rx_data_.q[2], rx_data_.q[3]);