修改

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/src/armor_finder/anti_top/anti_top.cpp

@@ -7,81 +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];
+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);
     }
-    return sum / length;
-}
+    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;
+    // 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 - SYSTEM_DELAY - 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);
-        } else {
-            sendBoxPosition(shoot_delay);
+    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;
     }
-        time_seq.clear();
-        angle_seq.clear();
-        last_front_time = front_time;
+
+    // 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 {
-        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);
+            auto_omega = 0.8 * auto_omega + 0.2 * current_omega; // Low pass filter
         }
-    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/send_target/send_target.cpp

@@ -9,9 +9,10 @@
 #include <log.h>
 
 
-static bool sendTarget(Serial &serial, double x, uint16_t shoot_delay) {// double y, double z
-    static short x_tmp; //y_tmp, z_tmp;
-    uint8_t buff[6];//10
+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);
@@ -19,7 +20,7 @@ static bool sendTarget(Serial &serial, double x, uint16_t shoot_delay) {// doubl
     time_t t = time(nullptr);
     if (last_time != t) {
         last_time = t;
-        cout << "Armor: fps:" << fps << ", (" << x  <<  endl;//<< 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, uint16_t shoot_delay) {// doubl
 
 #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[3] = static_cast<char>((shoot_delay >> 8) & 0xFF);
-    buff[4] = static_cast<char>((shoot_delay >> 0) & 0xFF);
-    buff[5] = 'e';
-//    if(buff[7]<<8 | buff[8])
-//        cout << (buff[7]<<8 | buff[8]) << endl;
+    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] = 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, shoot_delay);// pitch_comp, dist * 100.0,
+    return sendTarget(serial, last_yaw, pitch_comp, 0.0, shoot_delay, can_fire);
 }
 

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;