返航绕8，只有第一次绕，自动模式第一个点fast wp设置false

第一个点绕圈增加期望值和超时判断，返航是否绕圈判断调整

ArduCopter/mode.h

@@ -1090,6 +1090,7 @@ private:
 
     bool terrain_following_allowed;
     uint32_t last_send_time;
+    uint8_t run_once_circle;
 };
 
 

ArduCopter/mode_auto.cpp

@@ -817,6 +817,7 @@ void ModeAuto::wp_run()
     if(g.zr_8_circle == 2 && wp_nav_index == 0){
         const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
         static uint32_t last_100ms;
+        static uint8_t timeout_cnt;
         uint32_t tnow = AP_HAL::millis();
         if (tnow - last_100ms > 100) {
             Vector3f stopping_point;
@@ -834,6 +835,9 @@ void ModeAuto::wp_run()
                                     stopping_point.z );
         }
         if(reached_wp_dest){
+            if(timeout_cnt == 0){
+                timeout_cnt = 1;
+            }
             static autopilot_yaw_mode last_yaw_mode;
             if(circle_init_yaw == false){   // 如果需要转向到下一个航点
                 if (abs(wrap_180_cd(ahrs.yaw_sensor-wp_yaw_cd)) <= 200) {   //转到角度
@@ -842,7 +846,9 @@ void ModeAuto::wp_run()
                     copter.circle_nav->init();
                     gcs().send_text(MAV_SEVERITY_INFO,"Auto run circle");
                 }else{
-                        // wp_nav->set_wp_destination(last_loc);// 设置第一个WayPoint点为期望值
+                        if(timeout_cnt < 10){
+                            wp_nav->set_wp_destination(last_loc);// 设置第一个WayPoint点为期望值
+                        }
                         if (auto_yaw.mode() != AUTO_YAW_FIXED) {    // 需要转角度，yaw mode使用AUTO_YAW_FIXED
                             // wp_nav->set_wp_destination(last_loc);// 设置第一个WayPoint点为期望值
                             last_yaw_mode = auto_yaw.mode();
@@ -852,9 +858,32 @@ void ModeAuto::wp_run()
                 }
                 return;
             }
+        }else{
+                // get current location
+            static uint32_t last_1s;
+            if (timeout_cnt > 0 &&AP_HAL::millis() - last_1s > 1000) {
+                const Vector3f &curr_pos = copter.inertial_nav.get_position();
+                static Vector3f last_pos = curr_pos;
+                Vector3f dist_to_dest = (curr_pos - last_pos);
+                // gcs().send_text(MAV_SEVERITY_INFO,"Circle turn move:%.2f,%d", dist_to_dest.length(),timeout_cnt);
+                if( dist_to_dest.length() <= 100 ) {
+                    timeout_cnt+=1;
+                }else{
+                    timeout_cnt = 1;
+                }
+                last_1s = AP_HAL::millis();
+                last_pos = curr_pos;
+            }
+            if (timeout_cnt == 10)
+            {
+                gcs().send_text(MAV_SEVERITY_INFO,"Circle turn yaw timeout");
+                // circle_init_yaw = true;
+            }
+            
         }
         if(circle_init_yaw)
         {   // 如果需要转向到下一个航点
+            timeout_cnt = 0;
             copter.circle_nav->turn_2_circle();
             pos_control->update_z_controller();
             // Yaw 由 circle决定
@@ -1341,7 +1370,9 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
                 // for unsupported commands it is safer to stop
                 break;
         }
-
+        if(wp_nav_index == 0){
+            fast_waypoint = false;
+        }
         // fast_waypoint = copter.wp_nav->get_fast_waypoint();
         copter.wp_nav->set_fast_waypoint(fast_waypoint);
     }

ArduCopter/mode_rtl.cpp

@@ -22,6 +22,7 @@ bool ModeRTL::init(bool ignore_checks)
     _state = RTL_Starting;
     _state_complete = true; // see run() method below
     terrain_following_allowed = !copter.failsafe.terrain;
+    run_once_circle += 1;
     return true;
 }
 
@@ -44,22 +45,30 @@ void ModeRTL::run(bool disarm_on_land)
     if (!motors->armed()) {
         return;
     }
-if(g.zr_8_circle == 2 && copter.mission_nav_index > 1 && !copter.alt_too_low){
-        static uint8_t flag = 0;
-        if(_state == RTL_ReturnHome){
-            // uint8_t flag = copter.circle_nav->run_rtl_8_circle(_state != RTL_LoiterAtHome);
-            copter.circle_nav->run_rtl_8_circle(&flag);
-            if(flag == 2){
-                auto_yaw.set_mode(AUTO_YAW_HOLD);
-                attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true);
-                return; //等待
-            }else if(flag == 3){
-                auto_yaw.set_mode(AUTO_YAW_LOOK_AT_NEXT_WP);
-                gcs().send_text(MAV_SEVERITY_INFO, "RTL circle finish");
-                flag = 4;
+    if(g.zr_8_circle == 2){
+        static uint16_t last_index;
+        if(last_index != copter.mission_nav_index){
+            run_once_circle = 1;
+            last_index = copter.mission_nav_index;
+        }
+        if(copter.mission_nav_index > 1 && !copter.alt_too_low && run_once_circle < 2)
+        {
+            static uint8_t flag = 0;
+            if(_state == RTL_ReturnHome){
+                // uint8_t flag = copter.circle_nav->run_rtl_8_circle(_state != RTL_LoiterAtHome);
+                copter.circle_nav->run_rtl_8_circle(&flag);
+                if(flag == 2){
+                    auto_yaw.set_mode(AUTO_YAW_HOLD);
+                    attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true);
+                    return; //等待
+                }else if(flag == 3){
+                    auto_yaw.set_mode(AUTO_YAW_LOOK_AT_NEXT_WP);
+                    gcs().send_text(MAV_SEVERITY_INFO, "RTL circle finish");
+                    flag = 4;
+                }
+            }else{
+                flag = 0;
             }
-        }else{
-            flag = 0;
         }
     }
 

ArduCopter/version.h

@@ -6,7 +6,7 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZRUAV v4.0.17-RC6"  //"ArduCopter V4.0.0"
+#define THISFIRMWARE "ZRUAV v4.0.17-RC7"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 4,0,17,FIRMWARE_VERSION_TYPE_OFFICIAL