yaw中心轴

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;                 // 陀螺周期循环队列
+    double auto_omega;                                  // 角速度缓存
-    vector<systime> time_seq;                           // 一个周期内的时间采样点
+    double last_phase;                                  // 上次相位角
-    vector<float> angle_seq;                            // 一个周期内的角度采样点
+    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,111 +7,119 @@
 #include <log.h>
 #include <show_images/ballistic_predicition.h>
 #include <config/setconfig.h>
+#include <opencv2/core.hpp>
 
-template<int length>
+static bool fitCircle(const std::vector<ArmorFinder::HistoryItem>& history, cv::Point3f& center, double& radius) {
-static double mean(RoundQueue<double, length> &vec) {
+    int n = history.size();
-    if (vec.size() == 0) return 0;
+    if (n < 15) return false;
-    double sum = 0;
+    cv::Mat A(n, 3, CV_64F);
-    for (int i = 0; i < vec.size(); i++) {
+    cv::Mat B(n, 1, CV_64F);
-        sum += vec[i];
+    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 / vec.size();
+    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) {
+    center.x = X.at<double>(0, 0);
-    double A = 0, B = 0, C = 0, D = 0;
+    center.z = X.at<double>(1, 0);
-    int len = time_seq.size();
+    center.y = sum_y / n; // Average height
-    for (int i = 0; i < len; i++) {
+    double c = X.at<double>(2, 0);
-        A += angle_seq[i] * angle_seq[i];
+    radius = sqrt(center.x * center.x + center.z * center.z - c);
-        B += angle_seq[i];
+    return true;
-        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;
 }
 
 void ArmorFinder::antiTop(double dist_m, double pitch_imu_deg) {
-    if (target_box.rect == cv::Rect2d()) return;
+    if (target_box.rect == cv::Rect2d() || history.empty()) return;
-    // 判断是否发生装甲目标切换。
 
-    // 如果是首帧追踪，直接记录不进行推算
+    // 1. Fit Circle to find center
-    if (last_box.rect == cv::Rect2d()) {
+    cv::Point3f center;
-        time_seq.emplace_back(frame_time);
+    double radius;
-        double dx = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
+    bool has_center = fitCircle(history, center, radius);
-        double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
+
-        angle_seq.emplace_back(yaw);
+    double curr_time = frame_time / 1000.0; // time in seconds
-        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
-    // 通过线性拟合计算出角度为0时对应的时间点
+    double center_yaw = atan2(center.x, center.z) * 180 / PI;
-    // 通过两次装甲角度为零的时间差计算陀螺旋转周期
+
-    // 根据旋转周期计算下一次装甲出现在角度为零的时间点
+    // 3. Phase Analysis and Omega Calculation
-    if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) {
+    double dx = target_xyz.x - center.x;
-        if (time_seq.size() < 2) {
+    double dz = target_xyz.z - center.z;
-            // 采点不足以拟合直线
+    double current_phase = atan2(dz, dx); // radians, [-pi, pi]
-            time_seq.clear();
+
-            angle_seq.clear();
+    double dt = curr_time - (last_phase_time == 0 ? curr_time : last_phase_time);
-            last_front_time = frame_time;
+    if (dt > 0.005 && dt < 0.1) {
-            sendBoxPosition(0);
+        double d_phase = current_phase - last_phase;
-            anti_top_cnt++;
+        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 {
+            auto_omega = 0.8 * auto_omega + 0.2 * current_omega; // Low pass filter
+        }
+    }
+    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;
     }
-        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);
 
-        bool is_period_stable = true;
+    // The vector from center to camera is (-center.x, -center.z).
-        if (!top_periodms.empty()) {
+    // The plate points at the camera when it's exactly between center and camera.
-            double last_period = top_periodms[-1];
+    double camera_phase = atan2(-center.z, -center.x); 
-            if (abs(once_periodms - last_period) > 50) {
+    
-                is_period_stable = false;
+    // 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;
         }
     }
 
-        top_periodms.push(once_periodms);
+    // 5. Check if it's time to fire
-        auto periodms = mean(top_periodms);
+    double fly_time_s = BallisticSolver::get_flight_time(dist_m, pitch_imu_deg, MUZZLE_VELOCITY, BALLISTIC_K);
-        systime curr_time;
+    double total_delay_s = fly_time_s + SYSTEM_DELAY / 1000.0;
-        getsystime(curr_time);
     
-        // 飞行时间补偿（ms）
+    double wait_time_s = min_t_hit - total_delay_s;
-        double fly_time_ms = BallisticSolver::get_flight_time(
+    if (wait_time_s < 0) {
-            dist_m, pitch_imu_deg, MUZZLE_VELOCITY, BALLISTIC_K) * 1000.0;
+        wait_time_s += abs((PI / 2.0) / auto_omega); // Look at the next plate
-
-        // 修正公式：子弹命中时刻 = 发令时刻 + 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);
-
-        // 若错过当前窗口（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 (!is_period_stable) {
-            sendBoxPosition(0);
-        } else {
-            sendBoxPosition(shoot_delay);
     }
-        time_seq.clear();
+
-        angle_seq.clear();
+    bool fire = false;
-        last_front_time = front_time;
+    // If the time falls within a 20ms prediction window, trigger immediately.
-    } else {
+    if (wait_time_s < 0.020 && wait_time_s >= -0.010) {
-        time_seq.emplace_back(frame_time);
+        fire = true;
-        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++;
+
+    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

@@ -130,7 +130,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,8 @@
 #include <log.h>
 
 
-static bool sendTarget(Serial &serial, double x, uint16_t shoot_delay) {// double y, double z
+static bool sendTarget(Serial &serial, double yaw, uint16_t shoot_delay, bool fire) {
-    static short x_tmp; //y_tmp, z_tmp;
+    uint8_t buff[7];
-    uint8_t buff[6];//10
 
 #ifdef WITH_COUNT_FPS
     static time_t last_time = time(nullptr);
@@ -27,25 +26,22 @@ 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);
+    short yaw_tmp = static_cast<short>(yaw * (32768 - 1) / 100);
-    //y_tmp = static_cast<short>(y * (32768 - 1) / 100);
-    //z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
 
     buff[0] = 's';
-    buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
+    buff[1] = static_cast<char>((yaw_tmp >> 8) & 0xFF);
-    buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
+    buff[2] = static_cast<char>((yaw_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';
+    buff[5] = fire ? 1 : 0;
-//    if(buff[7]<<8 | buff[8])
+    buff[6] = 'e';
-//        cout << (buff[7]<<8 | buff[8]) << endl;
     return serial.WriteData(buff, sizeof(buff));
 }
 
+bool ArmorFinder::sendAntiTopTarget(double yaw, uint16_t shoot_delay, bool fire) {
+    return sendTarget(serial, yaw, shoot_delay, fire);
+}
+
 bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     if (target_box.rect == cv::Rect2d()) return false;
     if (shoot_delay) {
@@ -83,6 +79,6 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     // 计算是否满足开火条件 (例如残差小于 1.5 度)
     can_fire = AutoTrigger::should_fire(*this, MUZZLE_VELOCITY, yaw, pitch_comp, 1.5);
 
-    return sendTarget(serial, yaw, shoot_delay);// pitch_comp, dist * 100.0,
+    return sendTarget(serial, yaw, shoot_delay, can_fire);
 }