backup test

ArduCopter/mode_auto.cpp

@@ -819,20 +819,6 @@ void ModeAuto::takeoff_run()
 //      called by auto_run at 100hz or more
 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();
-        if(!reached_wp_dest){
-            circle_init_yaw = false;
-        }else{
-            if (copter.circle_nav->run_8_circle(reached_wp_dest)){
-                circle_run();
-            }
-            return;
-        }
-        
-    }
-
     // process pilot's yaw input
     float target_yaw_rate = 0;
     if (!copter.failsafe.radio) {
@@ -859,6 +845,39 @@ void ModeAuto::wp_run()
     // call z-axis position controller (wpnav should have already updated it's alt target)
     pos_control->update_z_controller();
 
+
+    // 到航点绕圈
+    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){
+            static autopilot_yaw_mode last_yaw_mode;
+            if(circle_init_yaw == false){   // 如果需要转向到下一个航点
+                if (abs(wrap_180_cd(ahrs.yaw_sensor-wp_yaw_cd)) <= 200) {   //转到角度
+                    circle_init_yaw = true;
+                    auto_yaw.set_mode(last_yaw_mode);
+                    copter.circle_nav->init();
+                    gcs().send_text(MAV_SEVERITY_INFO,"Auto run circle");
+                    // gcs().send_text(MAV_SEVERITY_INFO, "circ init: y: %d -- %d ",wp_yaw_cd,ahrs.yaw_sensor);
+                }else{
+                    if (auto_yaw.mode() != AUTO_YAW_FIXED) {    // 需要转角度，yaw mode使用AUTO_YAW_FIXED
+                        last_yaw_mode = auto_yaw.mode();
+                        // gcs().send_text(MAV_SEVERITY_INFO, "auto_yaw: y: %d ",auto_yaw.mode());
+                        auto_yaw.set_mode(AUTO_YAW_FIXED);
+                    }
+                    attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), wp_yaw_cd,true);
+                }
+                return;
+            }else{   // 如果需要转向到下一个航点
+                copter.circle_nav->turn_2_circle();
+                pos_control->update_z_controller();
+                // Yaw 由 circle决定
+                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;
+            }
+        }
+    }
+
     // call attitude controller
     if (auto_yaw.mode() == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
@@ -1693,8 +1712,7 @@ bool ModeAuto::verify_takeoff()
     }else{
         return reached_wp_dest;
     }
-        
-    
+
 }
 
 // verify_land - returns true if landing has been completed