修改开火延迟一直为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);