Copter: loiter spin-up to throttle min (and remove a state)

ArduCopter/control_loiter.cpp

@@ -72,14 +72,19 @@ void Copter::loiter_run()
         wp_nav.loiter_soften_for_landing();
     }
 
+#if FRAME_CONFIG == HELI_FRAME
+    // helicopters are held on the ground until rotor speed runup has finished
+    bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors.rotor_runup_complete());
+#else
+    bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);
+#endif
+
     // Loiter State Machine Determination
     if (!motors.armed() || !motors.get_interlock()) {
         loiter_state = Loiter_MotorStopped;
-    } else if (!ap.auto_armed) {
-        loiter_state = Loiter_NotAutoArmed;
-    } else if (takeoff_state.running || (ap.land_complete && (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()))){
+    } else if (takeoff_state.running || takeoff_triggered) {
         loiter_state = Loiter_Takeoff;
-    } else if (ap.land_complete){
+    } else if (!ap.auto_armed || ap.land_complete) {
         loiter_state = Loiter_Landed;
     } else {
         loiter_state = Loiter_Flying;
@@ -92,41 +97,24 @@ void Copter::loiter_run()
 
         motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
 #if FRAME_CONFIG == HELI_FRAME
-        // helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
-        // run loiter controller
-        wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
-
-        // call attitude controller
-        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
-
         // force descent rate and call position controller
         pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
-        pos_control.update_z_controller();
 #else
         wp_nav.init_loiter_target();
-        // multicopters do not stabilize roll/pitch/yaw when motors are stopped
-        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
-        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
-#endif
-        break;
-
-    case Loiter_NotAutoArmed:
-
-        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
-        wp_nav.init_loiter_target();
-#if FRAME_CONFIG == HELI_FRAME
-        // Helicopters always stabilize roll/pitch/yaw
-        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
-        attitude_control.set_throttle_out(0,false,g.throttle_filt);
-#else
-        // Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
-        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
+        attitude_control.reset_rate_controller_I_terms();
+        attitude_control.set_yaw_target_to_current_heading();
+        pos_control.relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
 #endif
-        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
+        wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
+        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
+        pos_control.update_z_controller();
         break;
 
     case Loiter_Takeoff:
+        // set motors to full range
+        motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
+        // initiate take-off
         if (!takeoff_state.running) {
             takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
             // indicate we are taking off
@@ -138,9 +126,6 @@ void Copter::loiter_run()
         // get takeoff adjusted pilot and takeoff climb rates
         takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
 
-        // set motors to full range
-        motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
-
         // run loiter controller
         wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
 
@@ -154,19 +139,18 @@ void Copter::loiter_run()
         break;
 
     case Loiter_Landed:
-
-        wp_nav.init_loiter_target();
-        // call attitude controller
-        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
-        // move throttle to between minimum and non-takeoff-throttle to keep us on the ground
-        attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
-        // if throttle zero reset attitude and exit immediately
-        if (ap.throttle_zero) {
+        // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
+        if (target_climb_rate < 0.0f) {
             motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
         } else {
             motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
         }
-        pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-motors.get_throttle_hover());
+        wp_nav.init_loiter_target();
+        attitude_control.reset_rate_controller_I_terms();
+        attitude_control.set_yaw_target_to_current_heading();
+        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
+        pos_control.relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
+        pos_control.update_z_controller();
         break;
 
     case Loiter_Flying:

ArduCopter/defines.h

@@ -233,7 +233,6 @@ enum AltHoldModeState {
 // Loiter states
 enum LoiterModeState {
     Loiter_MotorStopped,
-    Loiter_NotAutoArmed,
     Loiter_Takeoff,
     Loiter_Flying,
     Loiter_Landed