Copter: Create Loiter state machine.

ArduCopter/control_loiter.cpp

@@ -31,19 +31,12 @@ bool Copter::loiter_init(bool ignore_checks)
 // should be called at 100hz or more
 void Copter::loiter_run()
 {
+    LoiterModeState loiter_state;
     float target_yaw_rate = 0;
     float target_climb_rate = 0;
     float takeoff_climb_rate = 0.0f;
 
-    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
-    if(!ap.auto_armed || !motors.get_interlock()) {
-        wp_nav.init_loiter_target();
-        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
-        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
-        return;
-    }
-
-    // process pilot inputs
+    // process pilot inputs unless we are in radio failsafe
     if (!failsafe.radio) {
         // apply SIMPLE mode transform to pilot inputs
         update_simple_mode();
@@ -60,18 +53,6 @@ void Copter::loiter_run()
 
         // get takeoff adjusted pilot and takeoff climb rates
         takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
-
-        // check for take-off
-        if (ap.land_complete && (takeoff_state.running || channel_throttle->control_in > get_takeoff_trigger_throttle())) {
-            if (!takeoff_state.running) {
-                takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
-            }
-
-            // indicate we are taking off
-            set_land_complete(false);
-            // clear i term when we're taking off
-            set_throttle_takeoff();
-        }
     } else {
         // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
         wp_nav.clear_pilot_desired_acceleration();
@@ -82,13 +63,75 @@ void Copter::loiter_run()
         wp_nav.loiter_soften_for_landing();
     }
 
-    // when landed reset targets and output zero throttle
-    if (ap.land_complete) {
+    // Loiter State Machine Determination
+    if(!ap.auto_armed || !motors.get_interlock()) {
+        loiter_state = Loiter_Disarmed;
+    } else if (takeoff_state.running || (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()))){
+        loiter_state = Loiter_Takeoff;
+    } else if (ap.land_complete){
+        loiter_state = Loiter_Landed;
+    } else {
+        loiter_state = Loiter_Flying;
+    }
+
+    // Loiter State Machine
+    switch (loiter_state) {
+
+    case Loiter_Disarmed:
+
         wp_nav.init_loiter_target();
+#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
+        // call attitude controller
+        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
+        attitude_control.set_throttle_out(0,false,g.throttle_filt);
+#else   // Multirotors do not stabilize roll/pitch/yaw when disarmed
+        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+#endif  // HELI_FRAME
+        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
+        break;
+
+    case Loiter_Takeoff:
+
+        if (!takeoff_state.running) {
+            takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
+            // indicate we are taking off
+            set_land_complete(false);
+            // clear i term when we're taking off
+            set_throttle_takeoff();
+        }
+
+        // run loiter controller
+        wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+
+        // call attitude controller
+        attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
+
+        // body-frame rate controller is run directly from 100hz loop
+
+        // update altitude target and call position controller
+        pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
+        pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
+        pos_control.update_z_controller();
+
+        break;
+
+    case Loiter_Landed:
+
+        wp_nav.init_loiter_target();
+#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
+        // call attitude controller
+        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
+        attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);
+#else   // Multirotors do not stabilize roll/pitch/yaw when disarmed
         // move throttle to between minimum and non-takeoff-throttle to keep us on the ground
         attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);
+#endif
         pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
-    }else{
+
+        break;
+
+    case Loiter_Flying:
+
         // run loiter controller
         wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
 
@@ -107,5 +150,7 @@ void Copter::loiter_run()
         pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
         pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         pos_control.update_z_controller();
+
+        break;
     }
-}
+}

ArduCopter/defines.h

@@ -204,6 +204,14 @@ enum AltHoldModeState {
     AltHold_Landed
 };
 
+// Loiter states
+enum LoiterModeState {
+    Loiter_Disarmed,
+    Loiter_Takeoff,
+    Loiter_Flying,
+    Loiter_Landed
+};
+
 // Flip states
 enum FlipState {
     Flip_Start,