绕8，移除1的模式

ArduCopter/mode_auto.cpp

@@ -47,7 +47,6 @@ bool ModeAuto::init(bool ignore_checks)
         mission.start_or_resume();
         wp_nav_index = 0;   // nav 重新开始计数
         updown_state = 0;   // 起飞悬停状态清0
-        first_wp_dec = false;
         // gcs().send_text(MAV_SEVERITY_INFO, "auto init:up:%d,wp:%d",updown_state,wp_nav_index);
         return true;
     } else {
@@ -808,14 +807,6 @@ void ModeAuto::takeoff_run()
             }
         }
     }
-    if(g.zr_8_circle == 1){
-        const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
-        if (copter.circle_nav->run_8_circle(reached_wp_dest))
-        {
-            circle_run();
-            return;
-        }
-    }
     auto_takeoff_run();
 
 }
@@ -825,7 +816,7 @@ void ModeAuto::takeoff_run()
 void ModeAuto::wp_run()
 {
     // 到航点绕圈
-    if(g.zr_8_circle == 2 && !first_wp_dec){
+    if(g.zr_8_circle == 2 && wp_nav_index == 0){
         const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
         if(!reached_wp_dest){
             circle_init_yaw = false;
@@ -1294,7 +1285,7 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
                             // delt_cd = (delt_cd > 0)? delt_cd:360+delt_cd;
                             fast_waypoint = (delt_cd > 6000)?false:true;
                             // gcs().send_text(MAV_SEVERITY_CRITICAL, "%d,a:%d,d:%d,f:%d,p:%.2f",cmd.index,wp_yaw_cd,delt_cd,fast_waypoint,wp_distance);
-                            if(wp_distance - last_dist > 0  && first_wp_dec){   // P2.5转弯校准用 //TODO P2.5需要修改这里 
+                            if(wp_distance - last_dist > 0  && wp_nav_index > 0){   // P2.5转弯校准用 //TODO P2.5需要修改这里 
                                 // gcs().send_text(MAV_SEVERITY_CRITICAL, "---- First Turn, Do Sth ----");
                             }
                             last_dist = wp_distance;
@@ -1683,24 +1674,23 @@ bool ModeAuto::verify_takeoff()
     if (reached_wp_dest) {
         copter.landinggear.set_position(AP_LandingGear::LandingGear_Retract);
     }
-    if(g.zr_8_circle == 1){	//如果启用绕8字，并且是起飞到头顶绕8字，增加判断
-        return (reached_wp_dest && copter.circle_nav->get_circle_8_finish());
-    }else{	// 常规起飞判断，增加设置绕圈相关初始设置
+    if(g.zr_8_circle > 0){	//如果启用绕8字，做一些初始化
         copter.circle_nav->set_circle_8_finish(false);
-        first_wp_dec = false;
-        if(g.zr_tk_delay > 0 && g.zr_tk_req_alt > 0 ){  // 启用到高度悬停
+        wp_nav_index = 0;
+    }
+    if(g.zr_tk_delay > 0 && g.zr_tk_req_alt > 0 ){  // 启用到高度悬停
 
-            if(reached_wp_dest && updown_state <2){     // 原到达航点判断，到达起飞悬停位置
-                updown_state = 2;
-            }else if(updown_state >= 4){    // 悬停倒计时结束，或者地面站点继续
-                return reached_wp_dest;
-            }
-            return false;
-        }else{
+        if(reached_wp_dest && updown_state <2){     // 原到达航点判断，到达起飞悬停位置
+            updown_state = 2;
+        }else if(updown_state >= 4){    // 悬停倒计时结束，或者地面站点继续
             return reached_wp_dest;
         }
-        
+        return false;
+    }else{
+        return reached_wp_dest;
     }
+        
+    
 }
 
 // verify_land - returns true if landing has been completed
@@ -2006,11 +1996,10 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 			AP_Notify::events.waypoint_complete = 1;
 			}
         
-        if(g.zr_8_circle == 2 && !first_wp_dec){
+        if(g.zr_8_circle == 2 && wp_nav_index == 0){
             bool cyc_ret =  copter.circle_nav->get_circle_8_finish();
             if(cyc_ret){
-                gcs().send_text(MAV_SEVERITY_INFO, "circle ok,Reached command #%i,nav:%d",cmd.index,wp_nav_index);
-                first_wp_dec = true;  
+                gcs().send_text(MAV_SEVERITY_INFO, "circle ok,Reached command #%i,nav:%d",cmd.index,wp_nav_index); 
                 wp_nav_index += 1;   // nav 计数  
                 copter.mission_nav_index = wp_nav_index;
             }