修改开火延迟一直为0的错误

armor/src/armor_finder/anti_top/anti_top.cpp

@@ -10,11 +10,12 @@
 
 template<int length>
 static double mean(RoundQueue<double, length> &vec) {
+    if (vec.size() == 0) return 0;
     double sum = 0;
     for (int i = 0; i < vec.size(); i++) {
         sum += vec[i];
     }
-    return sum / length;
+    return sum / vec.size();
 }
 
 static systime getFrontTime(const vector<systime> time_seq, const vector<float> angle_seq) {
@@ -47,6 +48,15 @@ void ArmorFinder::antiTop(double dist_m, double pitch_imu_deg) {
 //            return;
 //        }
         LOGM(STR_CTR(WORD_GREEN, "Top period: %.1lf"), once_periodms);
+        
+        bool is_period_stable = true;
+        if (!top_periodms.empty()) {
+            double last_period = top_periodms[-1];
+            if (abs(once_periodms - last_period) > 50) {
+                is_period_stable = false;
+            }
+        }
+        
         top_periodms.push(once_periodms);
         auto periodms = mean(top_periodms);
         systime curr_time;
@@ -67,7 +77,7 @@ void ArmorFinder::antiTop(double dist_m, double pitch_imu_deg) {
             : 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) {
+        } else if (!is_period_stable) {
             sendBoxPosition(0);
         } else {
             sendBoxPosition(shoot_delay);

others/include/additions.h

@@ -42,21 +42,28 @@ private:
     type data[length];
     int head;
     int tail;
+    int count;
 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 {
+        return count;
+    };
+
     bool empty() const {
-        return head == tail;
+        return count == 0;
     };
 
     void push(const type &obj) {
         data[head] = obj;
         head = (head + 1) % length;
-        if (head == tail) {
+        if (count < length) {
+            count++;
+        } else {
             tail = (tail + 1) % length;
         }
     };
@@ -65,11 +72,15 @@ public:
         if (empty()) return false;
         obj = data[tail];
         tail = (tail + 1) % length;
+        count--;
         return true;
     };
 
     type &operator[](int idx) {
-        while (tail + idx < 0) idx += length;
+        if (count == 0) return data[0];
+        if (idx < 0) idx += count;
+        if (idx < 0) idx = 0;
+        if (idx >= count) idx = count - 1;
         return data[(tail + idx) % length];
     };
 };