anti top v2.1

armor/include/armor_finder/armor_finder.h

@@ -115,6 +115,7 @@ private:
     int tracking_cnt;                                   // 记录追踪帧数，用于定时退出追踪
     Serial &serial;                                     // 串口对象，引用外部变量，用于和能量机关共享同一个变量
     systime last_front_time;                            // 上次陀螺正对时间
+    int anti_top_cnt;
     RoundQueue<double, 4> top_periodms;                 // 陀螺周期循环队列
     vector<systime> time_seq;                           // 一个周期内的时间采样点
     vector<float> angle_seq;                            // 一个周期内的角度采样点

armor/src/armor_finder/anti_top/anti_top.cpp

@@ -32,7 +32,6 @@ static systime getFrontTime(const vector<systime> time_seq, const vector<float>
 
 void ArmorFinder::antiTop() {
     if (target_box.rect == cv::Rect2d()) return;
-
     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);
@@ -40,13 +39,20 @@ void ArmorFinder::antiTop() {
 //            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);
         uint16_t shoot_delay = front_time + periodms * 2 - curr_time;
+        if (anti_top_cnt < 4) {
+            sendBoxPosition(0);
+        } else if (abs(once_periodms - top_periodms[-1]) > 50) {
+            sendBoxPosition(0);
+        } else {
             sendBoxPosition(shoot_delay);
+        }
         time_seq.clear();
         angle_seq.clear();
         last_front_time = front_time;
@@ -57,5 +63,6 @@ void ArmorFinder::antiTop() {
         angle_seq.emplace_back(yaw);
         sendBoxPosition(0);
     }
+    anti_top_cnt++;
 }
 

armor/src/armor_finder/armor_finder.cpp

@@ -93,6 +93,7 @@ end:
     if(is_anti_top) {
         antiTop();
     }else if(target_box.rect != cv::Rect2d()) {
+        anti_top_cnt = 0;
         time_seq.clear();
         angle_seq.clear();
         sendBoxPosition(0);

main.cpp

@@ -32,7 +32,7 @@ McuData mcu_data = {    // 单片机端回传结构体
         0,              // 当前云台pitch角
         ARMOR_STATE,    // 当前状态，自瞄-大符-小符
         0,              // 云台角度标记位
-        1,              // 是否启用数字识别
+        0,              // 是否为反陀螺模式
         ENEMY_RED,      // 敌方颜色
         0,              // 能量机关x轴补偿量
         0,              // 能量机关y轴补偿量
@@ -66,8 +66,8 @@ int main(int argc, char *argv[]) {
             video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/);
             video_chassis = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "energy"*/);
         } else {
-            video_gimbal = new VideoWrapper(PROJECT_DIR"/test_video/blue_big.avi");
+            video_gimbal = new VideoWrapper(PROJECT_DIR"/8-7-NO7.avi");
-            video_chassis = new VideoWrapper(PROJECT_DIR"/test_video/blue_big.avi");
+            video_chassis = new VideoWrapper(PROJECT_DIR"/8-7-NO7.avi");
         }
         if (video_gimbal->init()) {
             LOGM("video_gimbal source initialization successfully.");