添加shoot_delay

armor/include/armor_finder/armor_finder.h

@@ -120,10 +120,9 @@ private:
 
     systime last_front_time;                            // 上次陀螺正对时间
     int anti_top_cnt;
-    double auto_omega;                                  // 角速度缓存
+    RoundQueue<double, 4> top_periodms;                 // 陀螺周期循环队列
-    double last_phase;                                  // 上次相位角
+    vector<systime> time_seq;                           // 一个周期内的时间采样点
-    systime last_phase_time;                            // 上次相位角时间
+    vector<float> angle_seq;                            // 一个周期内的角度采样点
-
 
     float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
     float last_yaw, last_pitch;//PID中微分项
@@ -141,7 +140,6 @@ 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,136 +7,81 @@
 #include <log.h>
 #include <show_images/ballistic_predicition.h>
 #include <config/setconfig.h>
-#include <opencv2/core.hpp>
 
-static bool fitCircle(const std::vector<ArmorFinder::HistoryItem>& history, cv::Point3f& center, double& radius) {
+template<int length>
-    int n = history.size();
+static double mean(RoundQueue<double, length> &vec) {
-    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);
     }
-    cv::Mat X;
+    return sum / length;
-    if (!cv::solve(A, B, X, cv::DECOMP_SVD)) return false;
+}
-    
+
-    center.x = X.at<double>(0, 0);
+static systime getFrontTime(const vector<systime> time_seq, const vector<float> angle_seq) {
-    center.z = X.at<double>(1, 0);
+    double A = 0, B = 0, C = 0, D = 0;
-    center.y = sum_y / n; // Average height
+    int len = time_seq.size();
-    double c = X.at<double>(2, 0);
+    for (int i = 0; i < len; i++) {
-    radius = sqrt(center.x * center.x + center.z * center.z - c);
+        A += angle_seq[i] * angle_seq[i];
-    return true;
+        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;
 }
 
 void ArmorFinder::antiTop(double dist_m, double pitch_imu_deg) {
-    if (target_box.rect == cv::Rect2d() || history.empty()) return;
+    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);
 
-    // 1. Fit Circle to find center
+        // 飞行时间补偿（ms）
-    cv::Point3f center;
+        double fly_time_ms = BallisticSolver::get_flight_time(
-    double radius;
+            dist_m, pitch_imu_deg, MUZZLE_VELOCITY, BALLISTIC_K) * 1000.0;
-    bool has_center = fitCircle(history, center, radius);
 
-    double curr_time = frame_time / 1000.0; // time in seconds
+        // 修正公式：子弹命中时刻 = 发令时刻 + 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);
 
-    if (!has_center) {
+        // 若错过当前窗口（delay_raw < 0），顺延一个周期
-        // Not enough data to fit circle. Aim at current target.
+        uint16_t shoot_delay = (delay_raw > 0)
-        double yaw = atan2(target_xyz.x, target_xyz.z) * 180 / PI;
+            ? static_cast<uint16_t>(delay_raw)
-        sendAntiTopTarget(yaw, 0, false);
+            : static_cast<uint16_t>(delay_raw + static_cast<int32_t>(periodms));
-        return;
+        if (anti_top_cnt < 4) {
-    }
+            sendBoxPosition(0);
-
+        } else if (abs(once_periodms - top_periodms[-1]) > 50) {
-    // 2. Lock Gimbal Yaw to the Center
+            sendBoxPosition(0);
-    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 {
-            auto_omega = 0.8 * auto_omega + 0.2 * current_omega; // Low pass filter
+            sendBoxPosition(shoot_delay);
         }
-    }
+        time_seq.clear();
-    last_phase = current_phase;
+        angle_seq.clear();
-    last_phase_time = curr_time;
+        last_front_time = front_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 {
-        // 延迟太高，必定完美错过前面的板子。
+        time_seq.emplace_back(frame_time);
-        // 通过 ceil（向上取整）推导出“子弹飞到时，我们至少要迎击第 n 块板”：
+        double dx = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
-        int n_plates_missed = ceil(time_shortfall / T_face);
+        double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
-        
+        angle_seq.emplace_back(yaw);
-        // 我们真实需要等待的时机 = 命中第 n 块板的绝对时间 - 子弹与系统的总延迟
+        sendBoxPosition(0);
-        wait_time_s = (min_t_hit + n_plates_missed * T_face) - total_delay_s;
     }
-
+    anti_top_cnt++;
-    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,9 +100,8 @@ end:
         antiTop(dist_m, pitch_imu_deg);
     }else if(target_box.rect != cv::Rect2d()) {
         anti_top_cnt = 0;
-        auto_omega = 0;
+        time_seq.clear();
-        last_phase = 0;
+        angle_seq.clear();
-        last_phase_time = 0;
         sendBoxPosition(0);
     }
 
@@ -131,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() > 60) { // 保留足够帧来拟合圆心
+        if (history.size() > 10) { // 仅保留最近10帧
             history.erase(history.begin());
         }
     } else {

armor/src/armor_finder/send_target/send_target.cpp

@@ -9,10 +9,9 @@
 #include <log.h>
 
 
-static bool sendTarget(Serial &serial, double yaw, double pitch, double roll, uint16_t shoot_delay, bool fire) {
+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[10];
+    uint8_t buff[6];//10
-
 
 #ifdef WITH_COUNT_FPS
     static time_t last_time = time(nullptr);
@@ -20,7 +19,7 @@ static bool sendTarget(Serial &serial, double yaw, double pitch, double roll, ui
     time_t t = time(nullptr);
     if (last_time != t) {
         last_time = t;
-        cout << "Armor: fps:" << fps << ", yaw: " << yaw <<",pitch:"<<pitch<<",roll:"<<roll<< " delay: " << shoot_delay << endl;
+        cout << "Armor: fps:" << fps << ", (" << x  << ","<<shoot_delay << ")"<<endl;//<< y << "," << z << ")" << endl;
         fps = 0;
     }
     fps += 1;
@@ -28,37 +27,25 @@ static bool sendTarget(Serial &serial, double yaw, double pitch, double roll, ui
 
 #define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
 
-    short yaw_tmp   = static_cast<short>(yaw   * (32768 - 1) / 100);
+    x_tmp = static_cast<short>(x * (32768 - 1) / 100);
-    short pitch_tmp  = static_cast<short>(pitch * (32768 - 1) / 100);
+    //y_tmp = static_cast<short>(y * (32768 - 1) / 100);
-    short roll_tmp   = static_cast<short>(roll  * (32768 - 1) / 100);
+    //z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
 
     buff[0] = 's';
-    buff[1] = static_cast<char>((yaw_tmp   >> 8) & 0xFF);
+    buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
-    buff[2] = static_cast<char>((yaw_tmp   >> 0) & 0xFF);
+    buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
-    buff[3] = static_cast<char>((pitch_tmp >> 8) & 0xFF);
+    //buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
-    buff[4] = static_cast<char>((pitch_tmp >> 0) & 0xFF);
+    //buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
-    buff[5] = static_cast<char>((roll_tmp  >> 8) & 0xFF);
+    //buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
-    buff[6] = static_cast<char>((roll_tmp  >> 0) & 0xFF);
+    //buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
-    buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
+    buff[3] = static_cast<char>((shoot_delay >> 8) & 0xFF);
-    buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
+    buff[4] = static_cast<char>((shoot_delay >> 0) & 0xFF);
-    // buff[9] = fire ? 1 : 0;  // fire flag — no spare byte in current 10-byte protocol
+    buff[5] = 'e';
-    buff[9] = 'e';  // end marker
+//    if(buff[7]<<8 | buff[8])
+//        cout << (buff[7]<<8 | buff[8]) << endl;
     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) {
@@ -85,7 +72,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 坐标
@@ -94,8 +81,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, dx, pitch_comp, 0.5);
+    can_fire = AutoTrigger::should_fire(*this, MUZZLE_VELOCITY, yaw, pitch_comp, 1.5);
 
-    return sendTarget(serial, last_yaw, pitch_comp, 0.0, shoot_delay, can_fire);
+    return sendTarget(serial, yaw, shoot_delay);// pitch_comp, dist * 100.0,
 }
 

others/include/additions.h

@@ -35,8 +35,6 @@ void extract(cv::Mat &src);
 
 double getPointLength(const cv::Point2f &p);
 
-#include <mutex>
-
 // 循环队列
 template<class type, int length>
 class RoundQueue {
@@ -44,51 +42,34 @@ private:
     type data[length];
     int head;
     int tail;
-    int count;
-    mutable std::mutex mtx;
 public:
-    RoundQueue<type, length>() : head(0), tail(0), count(0) {};
+    RoundQueue<type, length>() : head(0), tail(0) {};
 
-    constexpr int capacity() const {
+    constexpr int size() const {
         return length;
     };
 
-    int size() const {
-        std::lock_guard<std::mutex> lock(mtx);
-        return count;
-    };
-
     bool empty() const {
-        std::lock_guard<std::mutex> lock(mtx);
+        return head == tail;
-        return count == 0;
     };
 
     void push(const type &obj) {
-        std::lock_guard<std::mutex> lock(mtx);
         data[head] = obj;
         head = (head + 1) % length;
-        if (count < length) {
+        if (head == tail) {
-            count++;
-        } else {
             tail = (tail + 1) % length;
         }
     };
 
     bool pop(type &obj) {
-        std::lock_guard<std::mutex> lock(mtx);
+        if (empty()) return false;
-        if (count == 0) return false;
         obj = data[tail];
         tail = (tail + 1) % length;
-        count--;
         return true;
     };
 
-    type operator[](int idx) const {
+    type &operator[](int idx) {
-        std::lock_guard<std::mutex> lock(mtx);
+        while (tail + idx < 0) idx += length;
-        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 (150.0)
+    #define SYSTEM_DELAY (50.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/ttyUSB* --color=never", "r");
+    FILE *ls = popen("ls /dev/ttyCH341USB* --color=never", "r");
     char name[20] = {0};
     fscanf(ls, "%s", name);
     return name;