修改

armor/include/armor_finder/armor_finder.h

@@ -120,9 +120,10 @@ private:
 
     systime last_front_time;                            // 上次陀螺正对时间
     int anti_top_cnt;
-    RoundQueue<double, 4> top_periodms;                 // 陀螺周期循环队列
-    vector<systime> time_seq;                           // 一个周期内的时间采样点
-    vector<float> angle_seq;                            // 一个周期内的角度采样点
+    double auto_omega;                                  // 角速度缓存
+    double last_phase;                                  // 上次相位角
+    systime last_phase_time;                            // 上次相位角时间
+
 
     float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
     float last_yaw, last_pitch;//PID中微分项
@@ -140,6 +141,7 @@ private:
     cv::Point3f getTarget3D(const ArmorBox &box);      // 获取目标的3D坐标 (相对于相机)
 
     bool sendBoxPosition(uint16_t shoot_delay);           // 发送装甲板位置
+    bool sendAntiTopTarget(double yaw, uint16_t shoot_delay, bool fire); // 发送反陀螺射击目标
     bool shouldFire() const { return can_fire; }        // 获取开火建议
 public:
     void run(cv::Mat &src);                             // 自瞄主函数

armor/include/show_images/ballistic_predicition.h

@@ -56,42 +56,6 @@ public:
      * @return 弹着点相对枪口的垂直高度 (m，高于枪口为正)
      */
     static double get_impact_y(double d, double pitch, double v0, double k = 0.1);
-
-    /**
-     * [TEST] 用于验证弹道算法正确性：将弹着点反解算到相机坐标系，
-     * 再通过 project_to_pixel 投影到像素并用红点显示。
-     * 如果实验结果正确，可直接注释调用处，不影响其他结构。
-     *
-     * 坐标变换链：
-     *   xyz_camera = R_c2g^T * (R_g2w^T * xyz_world - t_c2g)
-     *
-     * @param xyz_in_world      目标在世界坐标系中的位置 (m，来自 PnP Solver)
-     * @param pitch_imu         当前 IMU pitch 角 (度，仰为正)
-     * @param v0                枪口初速度 (m/s)
-     * @param R_camera2gimbal   相机到云台的旋转矩阵
-     * @param R_gimbal2world    云台到世界的旋转矩阵
-     * @param t_camera2gimbal   相机原点在云台系中的平移向量 (m)
-     * @param k                 空气阻力系数
-     * @return ImpactPoint      { 弹着点相机坐标(m), 飞行时间(s) }
-     */
-    static ImpactPoint get_impact_in_camera(
-        const Eigen::Vector3d& xyz_in_world,
-        double pitch_imu,
-        double v0,
-        const Eigen::Matrix3d& R_camera2gimbal,
-        const Eigen::Matrix3d& R_gimbal2world,
-        const Eigen::Vector3d& t_camera2gimbal,
-        double k = 0.1);
-
-    /**
-     * [TEST] 将相机坐标系3D点简化投影到像素坐标（针孔模型，无畲变）。
-     * @param xyz_in_camera  弹着点在相机坐标系中的位置 (m)
-     * @param camera_matrix  OpenCV 相机内参矩阵 (3x3 CV_64F)
-     * @return 像素坐标
-     */
-    static cv::Point2f project_to_pixel(
-        const Eigen::Vector3d& xyz_in_camera,
-        const cv::Mat& camera_matrix);
 };
 
 #endif // BALLISTIC_PREDICITION_H

armor/include/show_images/show_images.h

@@ -15,9 +15,4 @@ void showLightBlobs(std::string windows_name, const cv::Mat &src, const LightBlo
 void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes &boxes);
 void showTrackSearchingPos(std::string window_names, const cv::Mat &src, const cv::Rect2d pos);
 
-// [TEST] 在图像上标注弹着点（红色小圆点），用于验证弹道算法正确性。
-// 如果实验结果正确，直接注释调用处即可，不影响其他结构。
-// impact_pixel: 由 BallisticSolver::project_to_pixel() 计算得到的像素坐标。
-void showImpactPoint(const std::string& window_name, const cv::Mat& src, cv::Point2f impact_pixel);
-
 #endif /* _SHOW_IMAGES_H_ */

armor/src/armor_finder/anti_top/anti_top.cpp

@@ -7,82 +7,136 @@
 #include <log.h>
 #include <show_images/ballistic_predicition.h>
 #include <config/setconfig.h>
+#include <opencv2/core.hpp>
 
-template<int length>
-static double mean(RoundQueue<double, length> &vec) {
-    double sum = 0;
-    for (int i = 0; i < vec.size(); i++) {
-        sum += vec[i];
-    }
-    return sum / length;
+static bool fitCircle(const std::vector<ArmorFinder::HistoryItem>& history, cv::Point3f& center, double& radius) {
+    int n = history.size();
+    if (n < 15) return false;
+    cv::Mat A(n, 3, CV_64F);
+    cv::Mat B(n, 1, CV_64F);
+    double sum_y = 0;
+    for (int i = 0; i < n; i++) {
+        double x = history[i].pos.x;
+        double z = history[i].pos.z;
+        sum_y += history[i].pos.y;
+        A.at<double>(i, 0) = -2.0 * x;
+        A.at<double>(i, 1) = -2.0 * z;
+        A.at<double>(i, 2) = 1.0;
+        B.at<double>(i, 0) = -(x * x + z * z);
     }
+    cv::Mat X;
+    if (!cv::solve(A, B, X, cv::DECOMP_SVD)) return false;
     
-static systime getFrontTime(const vector<systime> time_seq, const vector<float> angle_seq) {
-    double A = 0, B = 0, C = 0, D = 0;
-    int len = time_seq.size();
-    for (int i = 0; i < len; i++) {
-        A += angle_seq[i] * angle_seq[i];
-        B += angle_seq[i];
-        C += angle_seq[i] * time_seq[i];
-        D += time_seq[i];
-        cout << "(" << angle_seq[i] << ", " << time_seq[i] << ") ";
-    }
-    double b = (A * D - B * C) / (len * A - B * B);
-    cout << b << endl;
-    return b;
+    center.x = X.at<double>(0, 0);
+    center.z = X.at<double>(1, 0);
+    center.y = sum_y / n; // Average height
+    double c = X.at<double>(2, 0);
+    radius = sqrt(center.x * center.x + center.z * center.z - c);
+    return true;
 }
 
 void ArmorFinder::antiTop(double dist_m, double pitch_imu_deg) {
-    if (target_box.rect == cv::Rect2d()) return;
-    // 判断是否发生装甲目标切换。
-    // 记录切换前一段时间目标装甲的角度和时间
-    // 通过线性拟合计算出角度为0时对应的时间点
-    // 通过两次装甲角度为零的时间差计算陀螺旋转周期
-    // 根据旋转周期计算下一次装甲出现在角度为零的时间点
-    if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) {
-        auto front_time = getFrontTime(time_seq, angle_seq);
-        auto once_periodms = getTimeIntervalms(front_time, last_front_time);
-//        if (abs(once_periodms - top_periodms[-1]) > 50) {
-//            sendBoxPosition(0);
-//            return;
-//        }
-        LOGM(STR_CTR(WORD_GREEN, "Top period: %.1lf"), once_periodms);
-        top_periodms.push(once_periodms);
-        auto periodms = mean(top_periodms);
-        systime curr_time;
-        getsystime(curr_time);
+    if (target_box.rect == cv::Rect2d() || history.empty()) return;
 
-        // 飞行时间补偿（ms）
-        double fly_time_ms = BallisticSolver::get_flight_time(
-            dist_m, pitch_imu_deg, MUZZLE_VELOCITY, BALLISTIC_K) * 1000.0;
-        constexpr double sys_delay_ms = SYSTEM_DELAY; // setconfig.h 默认 50 ms
+    // 1. Fit Circle to find center
+    cv::Point3f center;
+    double radius;
+    bool has_center = fitCircle(history, center, radius);
 
-        // 修正公式：子弹命中时刻 = 发令时刻 + shoot_delay + sys_delay + fly_time
-        // 令子弹命中时刻 = front_time + periodms×2
-        int32_t delay_raw = static_cast<int32_t>(
-            front_time + periodms * 2 - curr_time - sys_delay_ms - fly_time_ms);
+    double curr_time = frame_time / 1000.0; // time in seconds
 
-        // 若错过当前窗口（delay_raw < 0），顺延一个周期
-        uint16_t shoot_delay = (delay_raw > 0)
-            ? static_cast<uint16_t>(delay_raw)
-            : static_cast<uint16_t>(delay_raw + static_cast<int32_t>(periodms));
-        if (anti_top_cnt < 4) {
-            sendBoxPosition(0);
-        } else if (abs(once_periodms - top_periodms[-1]) > 50) {
-            sendBoxPosition(0);
+    if (!has_center) {
+        // Not enough data to fit circle. Aim at current target.
+        double yaw = atan2(target_xyz.x, target_xyz.z) * 180 / PI;
+        sendAntiTopTarget(yaw, 0, false);
+        return;
+    }
+
+    // 2. Lock Gimbal Yaw to the Center
+    double center_yaw = atan2(center.x, center.z) * 180 / PI;
+
+    // 3. Phase Analysis and Omega Calculation
+    double dx = target_xyz.x - center.x;
+    double dz = target_xyz.z - center.z;
+    double current_phase = atan2(dz, dx); // radians, [-pi, pi]
+
+    double dt = curr_time - (last_phase_time == 0 ? curr_time : last_phase_time);
+    if (dt > 0.005 && dt < 0.1) {
+        double d_phase = current_phase - last_phase;
+        while (d_phase > PI) d_phase -= 2*PI;
+        while (d_phase < -PI) d_phase += 2*PI;
+        
+        double current_omega = d_phase / dt;
+        if (auto_omega == 0) {
+            auto_omega = current_omega;
         } else {
-            sendBoxPosition(shoot_delay);
+            auto_omega = 0.8 * auto_omega + 0.2 * current_omega; // Low pass filter
         }
-        time_seq.clear();
-        angle_seq.clear();
-        last_front_time = front_time;
-    } else {
-        time_seq.emplace_back(frame_time);
-        double dx = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
-        double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
-        angle_seq.emplace_back(yaw);
-        sendBoxPosition(0);
     }
-    anti_top_cnt++;
+    last_phase = current_phase;
+    last_phase_time = curr_time;
+
+    // 4. Calculate prediction
+    if (abs(auto_omega) < 1.0) { // Top is not spinning fast enough or noise
+        sendAntiTopTarget(center_yaw, 0, false);
+        return;
+    }
+
+    // The vector from center to camera is (-center.x, -center.z).
+    // The plate points at the camera when it's exactly between center and camera.
+    double camera_phase = atan2(-center.z, -center.x); 
+    
+    // Find min time to hit ANY of the 4 plates
+    double min_t_hit = 1e9;
+    for (int k = 0; k < 4; k++) {
+        double p = current_phase + k * PI / 2.0;
+        double diff = camera_phase - p;
+        
+        while (diff > PI) diff -= 2*PI;
+        while (diff < -PI) diff += 2*PI;
+        
+        if (auto_omega > 0 && diff < 0) diff += 2*PI;
+        if (auto_omega < 0 && diff > 0) diff -= 2*PI;
+        
+        double t_hit = diff / auto_omega; // will be positive
+        if (t_hit < min_t_hit && t_hit > 0) {
+            min_t_hit = t_hit;
+        }
+    }
+
+    // 5. Check if it's time to fire
+    double fly_time_s = BallisticSolver::get_flight_time(dist_m, pitch_imu_deg, MUZZLE_VELOCITY, BALLISTIC_K);
+    double total_delay_s = fly_time_s + SYSTEM_DELAY / 1000.0;
+
+    // 核心优化：O(1) 绝对相位耦合算法
+    // 原理：子弹必须在装甲板精确到达 Yaw 中心的那一刻撞击
+    double T_face = abs((PI / 2.0) / auto_omega); // 陀螺转过一个面的理论耗时
+    
+    // 如果我们现在立刻开火，子弹到达的时间点超越了下一块板的到来时间，
+    // 意味着我们必然“脱靶”（子弹飞在半空时，最近的板子已经溜过去了）。
+    // 在这个差值 time_shortfall 内，我们究竟错过了几块板子？
+    double time_shortfall = total_delay_s - min_t_hit; 
+    
+    double wait_time_s = 0.0;
+    if (time_shortfall <= 0) {
+        // 第一块板子的时间还没到，完全赶得及！我们只需要原地等差值补齐就行。
+        wait_time_s = -time_shortfall; 
+    } else {
+        // 延迟太高，必定完美错过前面的板子。
+        // 通过 ceil（向上取整）推导出“子弹飞到时，我们至少要迎击第 n 块板”：
+        int n_plates_missed = ceil(time_shortfall / T_face);
+        
+        // 我们真实需要等待的时机 = 命中第 n 块板的绝对时间 - 子弹与系统的总延迟
+        wait_time_s = (min_t_hit + n_plates_missed * T_face) - total_delay_s;
+    }
+
+    bool fire = false;
+    // If the time falls within a 20ms prediction window, trigger immediately.
+    if (wait_time_s < 0.020 && wait_time_s >= -0.010) {
+        fire = true;
+    }
+
+    uint16_t shoot_delay = static_cast<uint16_t>(wait_time_s * 1000.0);
+    sendAntiTopTarget(center_yaw, shoot_delay, fire);
 }
 

armor/src/armor_finder/armor_finder.cpp

@@ -100,8 +100,9 @@ end:
         antiTop(dist_m, pitch_imu_deg);
     }else if(target_box.rect != cv::Rect2d()) {
         anti_top_cnt = 0;
-        time_seq.clear();
-        angle_seq.clear();
+        auto_omega = 0;
+        last_phase = 0;
+        last_phase_time = 0;
         sendBoxPosition(0);
     }
 
@@ -130,7 +131,7 @@ end:
         cv::Point3f current_pos = target_xyz;
         double current_time = frame_time / 1000.0;
         history.push_back({current_pos, current_time});
-        if (history.size() > 10) { // 仅保留最近10帧
+        if (history.size() > 60) { // 保留足够帧来拟合圆心
             history.erase(history.begin());
         }
     } else {

armor/src/armor_finder/classifier/solver.cpp

@@ -71,14 +71,14 @@ Solver::Solver(const std::string & config_path)
 {
   auto yaml = YAML::LoadFile(config_path);
 
-  /*
+  
   auto R_gimbal2imubody_data = yaml["R_gimbal2imubody"].as<std::vector<double>>();
   auto R_camera2gimbal_data = yaml["R_camera2gimbal"].as<std::vector<double>>();
   auto t_camera2gimbal_data = yaml["t_camera2gimbal"].as<std::vector<double>>();
   R_gimbal2imubody_ = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(R_gimbal2imubody_data.data());
   R_camera2gimbal_ = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(R_camera2gimbal_data.data());
   t_camera2gimbal_ = Eigen::Matrix<double, 3, 1>(t_camera2gimbal_data.data());
-  */
+  
  
   auto camera_matrix_data = yaml["camera_matrix"].as<std::vector<double>>();
   auto distort_coeffs_data = yaml["distort_coeffs"].as<std::vector<double>>();
@@ -146,13 +146,13 @@ void Solver::solve(Armor & armor) const
 
   optimize_yaw(armor);
 
-  LOGM(
+  /*LOGM(
     "Armor: %s, pnp_xyz: (%.3f, %.3f, %.3f), world_xyz: (%.3f, %.3f, %.3f), ypd: (%.1f, %.1f, "
     "%.3f)",
     id2name[static_cast<int>(armor.name)].c_str(), armor.xyz_in_gimbal.x(), armor.xyz_in_gimbal.y(),
     armor.xyz_in_gimbal.z(), armor.xyz_in_world.x(), armor.xyz_in_world.y(), armor.xyz_in_world.z(),
     armor.ypd_in_world.x() * 180.0 / CV_PI, armor.ypd_in_world.y() * 180.0 / CV_PI,
-    armor.ypd_in_world.z());
+    armor.ypd_in_world.z());*/
 }
 
 std::vector<cv::Point2f> Solver::reproject_armor(

armor/src/armor_finder/find/find_light_blobs.cpp

@@ -79,7 +79,7 @@ bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
 
     int light_threshold;
     if(enemy_color == ENEMY_BLUE){
-        light_threshold = 225;
+        light_threshold = 240;//225
     }else{
         light_threshold = 200;
     }

armor/src/armor_finder/send_target/send_target.cpp

@@ -9,17 +9,18 @@
 #include <log.h>
 
 
-static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) {
-    static short x_tmp, y_tmp, z_tmp;
+static bool sendTarget(Serial &serial, double yaw, double pitch, double roll, uint16_t shoot_delay, bool fire) {
+    
     uint8_t buff[10];
 
+
 #ifdef WITH_COUNT_FPS
     static time_t last_time = time(nullptr);
     static int fps;
     time_t t = time(nullptr);
     if (last_time != t) {
         last_time = t;
-        cout << "Armor: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
+        cout << "Armor: fps:" << fps << ", yaw: " << yaw <<",pitch:"<<pitch<<",roll:"<<roll<< " delay: " << shoot_delay << endl;
         fps = 0;
     }
     fps += 1;
@@ -27,25 +28,37 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
 
 #define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
 
-    x_tmp = static_cast<short>(x * (32768 - 1) / 100);
-    y_tmp = static_cast<short>(y * (32768 - 1) / 100);
-    z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
+    short yaw_tmp   = static_cast<short>(yaw   * (32768 - 1) / 100);
+    short pitch_tmp  = static_cast<short>(pitch * (32768 - 1) / 100);
+    short roll_tmp   = static_cast<short>(roll  * (32768 - 1) / 100);
 
     buff[0] = 's';
-    buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
-    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[1] = static_cast<char>((yaw_tmp   >> 8) & 0xFF);
+    buff[2] = static_cast<char>((yaw_tmp   >> 0) & 0xFF);
+    buff[3] = static_cast<char>((pitch_tmp >> 8) & 0xFF);
+    buff[4] = static_cast<char>((pitch_tmp >> 0) & 0xFF);
+    buff[5] = static_cast<char>((roll_tmp  >> 8) & 0xFF);
+    buff[6] = static_cast<char>((roll_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;
+    // buff[9] = fire ? 1 : 0;  // fire flag — no spare byte in current 10-byte protocol
+    buff[9] = 'e';  // end marker
     return serial.WriteData(buff, sizeof(buff));
+    
+
+    // Vofa串口验证 不用可以注释掉
+    /*
+    char buff[128];
+    int len = sprintf(buff,"channels: %f, %u, %d\n", yaw, (unsigned int)shoot_delay, fire);
+    return serial.WriteData((unsigned char *)buff, len);
+    */
 }
 
+bool ArmorFinder::sendAntiTopTarget(double yaw, uint16_t shoot_delay, bool fire) {
+    return sendTarget(serial, yaw, 0.0, 0.0, shoot_delay, fire);
+}
+
+
 bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     if (target_box.rect == cv::Rect2d()) return false;
     if (shoot_delay) {
@@ -72,7 +85,7 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     last_pitch = tmp_pitch;
     //
 
-    double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
+    // double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
     double dist = sqrt(target_xyz.x * target_xyz.x + target_xyz.y * target_xyz.y + target_xyz.z * target_xyz.z) / 100.0; // 米
     
     // 弹道补偿使用 PnP 提供的 3D 坐标
@@ -81,8 +94,8 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     double pitch_comp = BallisticSolver::get_pitch(x_target, y_target, MUZZLE_VELOCITY, BALLISTIC_K);
 
     // 计算是否满足开火条件 (例如残差小于 1.5 度)
-    can_fire = AutoTrigger::should_fire(*this, MUZZLE_VELOCITY, yaw, pitch_comp, 1.5);
+    can_fire = AutoTrigger::should_fire(*this, MUZZLE_VELOCITY, dx, pitch_comp, 0.5);
 
-    return sendTarget(serial, yaw, pitch_comp, dist * 100.0, shoot_delay);
+    return sendTarget(serial, last_yaw, pitch_comp, 0.0, shoot_delay, can_fire);
 }
 

armor/src/show_images/ballistic_predicition.cpp

@@ -49,54 +49,3 @@ double BallisticSolver::get_impact_y(double d, double pitch, double v0, double k
     double t = get_flight_time(d, pitch, v0, k);
     return (1.0 / k) * (v0 * sin(pitch_rad) + g / k) * (1.0 - exp(-k * t)) - (g / k) * t;
 }
-
-// ──────────────────── [TEST] 以下两个函数用于可视化验证弹道算法正确性 ────────────────────
-// 如果测试通过，直接注释掉 armor_finder.cpp 里的调用，不影响其他逻辑。
-
-/**
- * [TEST] 将弹着点从世界坐标系反解算到相机坐标系。
- * 变换链：xyz_camera = R_c2g^T * (R_g2w^T * xyz_world - t_c2g)
- */
-ImpactPoint BallisticSolver::get_impact_in_camera(
-    const Eigen::Vector3d& xyz_in_world,
-    double pitch_imu,
-    double v0,
-    const Eigen::Matrix3d& R_camera2gimbal,
-    const Eigen::Matrix3d& R_gimbal2world,
-    const Eigen::Vector3d& t_camera2gimbal,
-    double k)
-{
-    // 水平距离：世界系 x=前, y=左
-    double d = std::sqrt(xyz_in_world.x() * xyz_in_world.x() +
-                         xyz_in_world.y() * xyz_in_world.y());
-
-    double impact_z = get_impact_y(d, pitch_imu, v0, k);
-    double fly_time = get_flight_time(d, pitch_imu, v0, k);
-
-    // 弹着点世界坐标：水平 (x,y) 与目标相同，z 换为弹道高度
-    Eigen::Vector3d impact_world(xyz_in_world.x(), xyz_in_world.y(), impact_z);
-
-    // 世界 → 云台 → 相机
-    Eigen::Vector3d impact_gimbal = R_gimbal2world.transpose() * impact_world;
-    Eigen::Vector3d impact_camera = R_camera2gimbal.transpose() * (impact_gimbal - t_camera2gimbal);
-
-    return ImpactPoint{ impact_camera, fly_time };
-}
-
-/**
- * [TEST] 针孔投影：相机坐标系 3D 点 → 像素坐标（不含畸变，仅用于可视化）。
- */
-cv::Point2f BallisticSolver::project_to_pixel(
-    const Eigen::Vector3d& xyz_in_camera,
-    const cv::Mat& camera_matrix)
-{
-    if (xyz_in_camera.z() <= 0) return cv::Point2f(-1, -1); // 在相机背后，无效
-    double fx = camera_matrix.at<double>(0, 0);
-    double fy = camera_matrix.at<double>(1, 1);
-    double cx = camera_matrix.at<double>(0, 2);
-    double cy = camera_matrix.at<double>(1, 2);
-    float u = static_cast<float>(fx * xyz_in_camera.x() / xyz_in_camera.z() + cx);
-    float v = static_cast<float>(fy * xyz_in_camera.y() / xyz_in_camera.z() + cy);
-    return cv::Point2f(u, v);
-}
-// ──────────────────────────────── [TEST END] ─────────────────────────────────────────────

armor/src/show_images/show_images.cpp

@@ -155,28 +155,3 @@ void showTrackSearchingPos(std::string window_names, const cv::Mat &src, const c
     rectangle(image2show, pos, Scalar(0, 255, 0), 1);
     imshow(window_names, image2show);
 }
-
-// ─────────────── [TEST] 弹着点可视化 ───────────────────────────────────────────────────────
-// 在图像上画一个红色小圆点标注弹道落点位置，用于验证弹道算法正确性。
-// 验证通过后直接注释调用处即可，函数本身不影响任何发包逻辑。
-void showImpactPoint(const std::string& window_name, const cv::Mat& src, cv::Point2f impact_pixel) {
-    static Mat image2show;
-    if (src.type() == CV_8UC1) {
-        cvtColor(src, image2show, COLOR_GRAY2RGB);
-    } else if (src.type() == CV_8UC3) {
-        image2show = src.clone();
-    } else {
-        return;
-    }
-
-    // 越界检查
-    int u = static_cast<int>(impact_pixel.x);
-    int v = static_cast<int>(impact_pixel.y);
-    if (u >= 0 && u < image2show.cols && v >= 0 && v < image2show.rows) {
-        // 红色实心圆，半径 3px
-        cv::circle(image2show, cv::Point(u, v), 3, Scalar(0, 0, 255), -1);
-    }
-
-    imshow(window_name, image2show);
-}
-// ─────────────── [TEST END] ────────────────────────────────────────────────────────────────

main.cpp

@@ -43,7 +43,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;
+uint8_t forced_anti_top = 1;//反陀螺1为开
+ArmorFinder armor_finder(enemy_color, serial, PROJECT_DIR"/tools/para/", forced_anti_top);
 
 int main(int argc, char *argv[]) {
     processOptions(argc, argv);             // 处理命令行参数

others/include/additions.h

@@ -35,6 +35,8 @@ void extract(cv::Mat &src);
 
 double getPointLength(const cv::Point2f &p);
 
+#include <mutex>
+
 // 循环队列
 template<class type, int length>
 class RoundQueue {
@@ -42,34 +44,51 @@ private:
     type data[length];
     int head;
     int tail;
+    int count;
+    mutable std::mutex mtx;
 public:
-    RoundQueue<type, length>() : head(0), tail(0) {};
+    RoundQueue<type, length>() : head(0), tail(0), count(0) {};
 
-    constexpr int size() const {
+    constexpr int capacity() const {
         return length;
     };
 
+    int size() const {
+        std::lock_guard<std::mutex> lock(mtx);
+        return count;
+    };
+
     bool empty() const {
-        return head == tail;
+        std::lock_guard<std::mutex> lock(mtx);
+        return count == 0;
     };
 
     void push(const type &obj) {
+        std::lock_guard<std::mutex> lock(mtx);
         data[head] = obj;
         head = (head + 1) % length;
-        if (head == tail) {
+        if (count < length) {
+            count++;
+        } else {
             tail = (tail + 1) % length;
         }
     };
 
     bool pop(type &obj) {
-        if (empty()) return false;
+        std::lock_guard<std::mutex> lock(mtx);
+        if (count == 0) return false;
         obj = data[tail];
         tail = (tail + 1) % length;
+        count--;
         return true;
     };
 
-    type &operator[](int idx) {
-        while (tail + idx < 0) idx += length;
+    type operator[](int idx) const {
+        std::lock_guard<std::mutex> lock(mtx);
+        if (count == 0) return data[0]; // will return garbage but safe from crash
+        if (idx < 0) idx += count;
+        if (idx < 0) idx = 0;
+        if (idx >= count) idx = count - 1;
         return data[(tail + idx) % length];
     };
 };

others/include/config/setconfig.h

@@ -90,7 +90,7 @@
 #endif
 
 #ifndef SYSTEM_DELAY
-    #define SYSTEM_DELAY (50.0)
+    #define SYSTEM_DELAY (150.0)
 #endif
 
 // 到此为止

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/ttyCH341USB* --color=never", "r");
+    FILE *ls = popen("ls /dev/ttyUSB* --color=never", "r");
     char name[20] = {0};
     fscanf(ls, "%s", name);
     return name;