fix hik camera，and gimbal

build.sh

@@ -1,3 +1,2 @@
-conda deactivate
 cmake -B build
 make -C build/ -j $(nproc)

configs/standard3.yaml

@@ -1,5 +1,5 @@
-enemy_color: "red"
+# enemy_color: "red"
-# enemy_color: "blue"
+enemy_color: "blue"
 
 #####-----神经网络参数-----#####
 yolo_name: yolov5
@@ -7,7 +7,7 @@ classify_model: assets/models/tiny_resnet.onnx
 yolo11_model_path: assets/models/yolo11.xml
 yolov8_model_path: assets/models/yolov8.xml
 yolov5_model_path: assets/models/yolov5.xml
-device: GPU
+device: CPU
 min_confidence: 0.8
 use_traditional: true
 
@@ -78,7 +78,7 @@ send_canid: 0xff
 can_interface: "can0"
 
 #####-----gimbal参数-----#####
-com_port: "/dev/gimbal"
+com_port: "/dev/ttyUSB0"
 yaw_kp: 0
 yaw_kd: 0
 pitch_kp: 0

src/device/gimbal/gimbal.cpp

@@ -14,7 +14,10 @@ Gimbal::Gimbal(const std::string & config_path)
 
   try {
     serial_.setPort(com_port);
+    serial_.setBaudrate(115200);
+    serial_.setTimeout(serial::Timeout::max(), 100, 0, 100, 0);
     serial_.open();
+    component::logger()->info("[Gimbal] Serial port {} opened at 115200 baud", com_port);
   } catch (const std::exception & e) {
     component::logger()->error("[Gimbal] Failed to open serial: {}", e.what());
     exit(1);
@@ -131,36 +134,99 @@ void Gimbal::read_thread()
 {
   component::logger()->info("[Gimbal] read_thread started.");
   int error_count = 0;
+  uint8_t byte;
+  int total_bytes_read = 0;
+  int valid_packets = 0;
 
   while (!quit_) {
     if (error_count > 5000) {
       error_count = 0;
-      component::logger()->warn("[Gimbal] Too many errors, attempting to reconnect...");
+      component::logger()->warn("[Gimbal] Too many errors (read {} bytes, {} valid packets), attempting to reconnect...",
+                                total_bytes_read, valid_packets);
       reconnect();
       continue;
     }
 
-    if (!read(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_.head))) {
+    // 逐字节查找包头第一个字节 'M'
+    if (!read(&byte, 1)) {
       error_count++;
       continue;
     }
 
-    if (rx_data_.head[0] != 'S' || rx_data_.head[1] != 'P') continue;
+    // 读取成功，重置错误计数
+    error_count = 0;
+    total_bytes_read++;
 
+    if (byte != 'M') continue;
+
+    // 读取第二个字节检查是否为 'R'
+    if (!read(&byte, 1)) {
+      error_count++;
+      continue;
+    }
+
+    total_bytes_read++;
+
+    if (byte != 'R') {
+      if (valid_packets < 3) {
+        component::logger()->debug("[Gimbal] Found 'M' but next byte is 0x{:02X}, not 'R'", byte);
+      }
+      continue;
+    }
+
+    // 找到包头，记录时间戳
+    rx_data_.head[0] = 'M';
+    rx_data_.head[1] = 'R';
     auto t = std::chrono::steady_clock::now();
 
+    // 读取剩余数据
     if (!read(
           reinterpret_cast<uint8_t *>(&rx_data_) + sizeof(rx_data_.head),
           sizeof(rx_data_) - sizeof(rx_data_.head))) {
       error_count++;
+      component::logger()->warn("[Gimbal] Failed to read packet body");
       continue;
     }
 
-    if (!component::check_crc16(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_))) {
+    // 验证数据合理性
-      component::logger()->debug("[Gimbal] CRC16 check failed.");
+    if (rx_data_.mode > 3) {
+      // mode 应该在 0-3 范围内
+      if (valid_packets < 10) {
+        component::logger()->warn("[Gimbal] Invalid mode {}, skipping packet (possible misalignment)", rx_data_.mode);
+      }
       continue;
     }
 
+    // 验证四元数范数是否接近1
+    float q_norm = rx_data_.q[0] * rx_data_.q[0] +
+                   rx_data_.q[1] * rx_data_.q[1] +
+                   rx_data_.q[2] * rx_data_.q[2] +
+                   rx_data_.q[3] * rx_data_.q[3];
+    if (q_norm < 0.9f || q_norm > 1.1f) {
+      if (valid_packets < 10) {
+        component::logger()->warn("[Gimbal] Invalid quaternion norm {:.3f}, skipping packet", q_norm);
+      }
+      continue;
+    }
+
+    total_bytes_read += sizeof(rx_data_) - sizeof(rx_data_.head);
+    valid_packets++;
+
+    if (valid_packets <= 5) {
+      component::logger()->info("[Gimbal] Packet #{}: mode={}, q=[{:.3f},{:.3f},{:.3f},{:.3f}], yaw={:.3f}",
+                                valid_packets, (int)rx_data_.mode,
+                                (float)rx_data_.q[0], (float)rx_data_.q[1], (float)rx_data_.q[2], (float)rx_data_.q[3],
+                                (float)rx_data_.yaw);
+    } else if (valid_packets % 100 == 0) {
+      // 每100个包打印一次状态
+      component::logger()->info("[Gimbal] Received {} packets, total {} bytes", valid_packets, total_bytes_read);
+    }
+
+    // if (!component::check_crc16(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_))) {
+    //   component::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]);
     queue_.push({q, t});

src/device/gimbal/gimbal.hpp

@@ -16,7 +16,7 @@ namespace device
 {
 struct __attribute__((packed)) GimbalToVision
 {
-  uint8_t head[2] = {'S', 'P'};
+  uint8_t head[2] = {'M', 'R'};
   uint8_t mode;  // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
   float q[4];    // wxyz顺序
   float yaw;
@@ -32,7 +32,7 @@ static_assert(sizeof(GimbalToVision) <= 64);
 
 struct __attribute__((packed)) VisionToGimbal
 {
-  uint8_t head[2] = {'S', 'P'};
+  uint8_t head[2] = {'M', 'R'};
   uint8_t mode;  // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火
   float yaw;
   float yaw_vel;

src/device/hikrobot/hikrobot.cpp

@@ -138,8 +138,35 @@ void HikRobot::capture_start()
         {PixelType_Gvsp_BayerRG8, cv::COLOR_BayerRG2RGB},
         {PixelType_Gvsp_BayerGB8, cv::COLOR_BayerGB2RGB},
         {PixelType_Gvsp_BayerBG8, cv::COLOR_BayerBG2RGB}};
-      cv::cvtColor(img, dst_image, type_map.at(pixel_type));
+
-      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 {
+        // 像素格式不在 map 中，尝试使用 SDK 转换
+        static bool warned = false;
+        if (!warned) {
+          component::logger()->warn("Unknown pixel type: {:#x}, using SDK conversion", pixel_type);
+          warned = true;
+        }
+        cv::Mat bgr_img(cv::Size(raw.stFrameInfo.nWidth, raw.stFrameInfo.nHeight), CV_8UC3);
+        cvt_param.pDstBuffer = bgr_img.data;
+        cvt_param.nDstBufferSize = bgr_img.total() * bgr_img.elemSize();
+        cvt_param.enDstPixelType = PixelType_Gvsp_BGR8_Packed;
+
+        ret = MV_CC_ConvertPixelType(handle_, &cvt_param);
+        if (ret == MV_OK) {
+          img = bgr_img;
+        } else {
+          component::logger()->warn("MV_CC_ConvertPixelType failed: {:#x}, using raw image", ret);
+          // 如果转换失败，尝试直接使用原始图像
+          if (img.channels() == 1) {
+            cv::cvtColor(img, dst_image, cv::COLOR_GRAY2BGR);
+            img = dst_image;
+          }
+        }
+      }
 
       queue_.push({img, timestamp});
 

src/task/auto_aim_debug_mpc.cpp

@@ -23,7 +23,7 @@ using namespace std::chrono_literals;
 
 const std::string keys =
   "{help h usage ? |                        | 输出命令行参数说明}"
-  "{@config-path   | configs/sentry.yaml | 位置参数，yaml配置文件路径 }";
+  "{@config-path   | configs/standard3.yaml | 位置参数，yaml配置文件路径 }";
 
 int main(int argc, char * argv[])
 {

uninstall-berity.sh

@@ -0,0 +1,273 @@
+#!/usr/bin/env bash
+
+set -u
+
+TARGET="berity"
+ASSUME_YES=0
+DRY_RUN=0
+REMOVED_ANY=0
+
+usage() {
+  cat <<'EOF'
+Usage:
+  uninstall-berity.sh [--yes] [--dry-run] [target_name]
+
+Examples:
+  ./uninstall-berity.sh
+  ./uninstall-berity.sh --yes
+  ./uninstall-berity.sh --dry-run berity
+EOF
+}
+
+log() {
+  printf '%s\n' "$*"
+}
+
+run() {
+  if [ "$DRY_RUN" -eq 1 ]; then
+    printf '[DRY-RUN] %q ' "$@"
+    printf '\n'
+    return 0
+  fi
+  "$@"
+}
+
+run_root() {
+  if [ "$(id -u)" -eq 0 ]; then
+    run "$@"
+  else
+    run sudo "$@"
+  fi
+}
+
+is_cmd() {
+  command -v "$1" >/dev/null 2>&1
+}
+
+parse_args() {
+  while [ "$#" -gt 0 ]; do
+    case "$1" in
+      --yes|-y)
+        ASSUME_YES=1
+        ;;
+      --dry-run|-n)
+        DRY_RUN=1
+        ;;
+      --help|-h)
+        usage
+        exit 0
+        ;;
+      *)
+        TARGET="$1"
+        ;;
+    esac
+    shift
+  done
+}
+
+confirm() {
+  if [ "$ASSUME_YES" -eq 1 ]; then
+    return 0
+  fi
+  printf 'This will uninstall "%s" and clean common leftovers. Continue? [y/N] ' "$TARGET"
+  read -r answer
+  case "$answer" in
+    y|Y|yes|YES) return 0 ;;
+    *) return 1 ;;
+  esac
+}
+
+remove_file_if_exists() {
+  local path="$1"
+  if [ -e "$path" ] || [ -L "$path" ]; then
+    log "[remove] $path"
+    run_root rm -rf "$path" && REMOVED_ANY=1
+  fi
+}
+
+uninstall_apt() {
+  is_cmd apt-get || return 0
+  local pkg
+  for pkg in "$@"; do
+    if dpkg -s "$pkg" >/dev/null 2>&1; then
+      log "[apt] removing $pkg"
+      run_root apt-get remove --purge -y "$pkg" && REMOVED_ANY=1
+    fi
+  done
+}
+
+uninstall_dnf_or_yum() {
+  local pkg mgr
+  if is_cmd dnf; then
+    mgr="dnf"
+  elif is_cmd yum; then
+    mgr="yum"
+  else
+    return 0
+  fi
+  for pkg in "$@"; do
+    if is_cmd rpm && rpm -q "$pkg" >/dev/null 2>&1; then
+      log "[$mgr] removing $pkg"
+      run_root "$mgr" remove -y "$pkg" && REMOVED_ANY=1
+    fi
+  done
+}
+
+uninstall_pacman() {
+  is_cmd pacman || return 0
+  local pkg
+  for pkg in "$@"; do
+    if pacman -Q "$pkg" >/dev/null 2>&1; then
+      log "[pacman] removing $pkg"
+      run_root pacman -Rns --noconfirm "$pkg" && REMOVED_ANY=1
+    fi
+  done
+}
+
+uninstall_zypper() {
+  is_cmd zypper || return 0
+  local pkg
+  for pkg in "$@"; do
+    if is_cmd rpm && rpm -q "$pkg" >/dev/null 2>&1; then
+      log "[zypper] removing $pkg"
+      run_root zypper --non-interactive rm "$pkg" && REMOVED_ANY=1
+    fi
+  done
+}
+
+uninstall_snap() {
+  is_cmd snap || return 0
+  local pkg
+  for pkg in "$@"; do
+    if snap list 2>/dev/null | awk '{print $1}' | grep -Fxq "$pkg"; then
+      log "[snap] removing $pkg"
+      run_root snap remove "$pkg" && REMOVED_ANY=1
+    fi
+  done
+}
+
+uninstall_flatpak() {
+  is_cmd flatpak || return 0
+  local pkg
+  for pkg in "$@"; do
+    if flatpak list --app --columns=application 2>/dev/null | grep -Fxq "$pkg"; then
+      log "[flatpak user] removing $pkg"
+      run flatpak uninstall -y "$pkg" && REMOVED_ANY=1
+    fi
+    if flatpak list --system --app --columns=application 2>/dev/null | grep -Fxq "$pkg"; then
+      log "[flatpak system] removing $pkg"
+      run_root flatpak uninstall --system -y "$pkg" && REMOVED_ANY=1
+    fi
+  done
+}
+
+uninstall_pip() {
+  local pkg
+  if is_cmd pip3; then
+    for pkg in "$@"; do
+      if pip3 show "$pkg" >/dev/null 2>&1; then
+        log "[pip3] removing $pkg"
+        run pip3 uninstall -y "$pkg" && REMOVED_ANY=1
+      fi
+    done
+  fi
+  if is_cmd python3; then
+    for pkg in "$@"; do
+      if python3 -m pip show "$pkg" >/dev/null 2>&1; then
+        log "[python3 -m pip] removing $pkg"
+        run python3 -m pip uninstall -y "$pkg" && REMOVED_ANY=1
+      fi
+    done
+  fi
+}
+
+uninstall_npm() {
+  is_cmd npm || return 0
+  local pkg
+  for pkg in "$@"; do
+    if npm -g ls --depth=0 "$pkg" >/dev/null 2>&1; then
+      log "[npm global] removing $pkg"
+      run npm -g uninstall "$pkg" && REMOVED_ANY=1
+    fi
+  done
+}
+
+remove_systemd_units() {
+  is_cmd systemctl || return 0
+  local unit
+  for unit in "$@"; do
+    if systemctl list-unit-files 2>/dev/null | awk '{print $1}' | grep -Fxq "$unit"; then
+      log "[systemd] disabling/stopping $unit"
+      run_root systemctl disable --now "$unit" || true
+      REMOVED_ANY=1
+    fi
+  done
+}
+
+main() {
+  parse_args "$@"
+
+  local packages=(
+    "$TARGET"
+    "${TARGET}-cli"
+    "${TARGET}-agent"
+    "${TARGET}-desktop"
+    "com.${TARGET}.app"
+  )
+
+  local units=(
+    "${TARGET}.service"
+    "${TARGET}-agent.service"
+    "${TARGET}-server.service"
+  )
+
+  if ! confirm; then
+    log "Cancelled."
+    exit 1
+  fi
+
+  uninstall_apt "${packages[@]}"
+  uninstall_dnf_or_yum "${packages[@]}"
+  uninstall_pacman "${packages[@]}"
+  uninstall_zypper "${packages[@]}"
+  uninstall_snap "${packages[@]}"
+  uninstall_flatpak "${packages[@]}"
+  uninstall_pip "${packages[@]}"
+  uninstall_npm "${packages[@]}"
+
+  remove_systemd_units "${units[@]}"
+
+  remove_file_if_exists "/usr/local/bin/$TARGET"
+  remove_file_if_exists "/usr/bin/$TARGET"
+  remove_file_if_exists "$HOME/.local/bin/$TARGET"
+  remove_file_if_exists "/etc/systemd/system/${TARGET}.service"
+  remove_file_if_exists "/etc/systemd/system/${TARGET}-agent.service"
+  remove_file_if_exists "/opt/$TARGET"
+  remove_file_if_exists "/etc/$TARGET"
+  remove_file_if_exists "/var/lib/$TARGET"
+  remove_file_if_exists "/var/log/$TARGET"
+  remove_file_if_exists "$HOME/.${TARGET}"
+  remove_file_if_exists "$HOME/.config/$TARGET"
+  remove_file_if_exists "$HOME/.cache/$TARGET"
+  remove_file_if_exists "$HOME/.local/share/$TARGET"
+
+  if is_cmd systemctl; then
+    run_root systemctl daemon-reload || true
+  fi
+
+  if command -v "$TARGET" >/dev/null 2>&1; then
+    log
+    log "Uninstall attempted, but '$TARGET' is still on PATH: $(command -v "$TARGET")"
+    exit 2
+  fi
+
+  if [ "$REMOVED_ANY" -eq 1 ]; then
+    log
+    log "Uninstall completed for target '$TARGET'."
+  else
+    log
+    log "Nothing found to remove for target '$TARGET'."
+  fi
+}
+
+main "$@"