Copter: Helicopters to force descent when motor is shut off

ArduCopter/control_althold.cpp

@@ -9,6 +9,13 @@
 // althold_init - initialise althold controller
 bool Copter::althold_init(bool ignore_checks)
 {
+#if FRAME_CONFIG == HELI_FRAME
+    // do not allow helis to enter Alt Hold if the Rotor Runup is not complete
+    if (!ignore_checks && !motors.rotor_runup_complete()){
+        return false;
+    }
+#endif
+
     // initialize vertical speeds and leash lengths
     pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
     pos_control.set_accel_z(g.pilot_accel_z);
@@ -52,8 +59,10 @@ void Copter::althold_run()
 #endif
 
     // Alt Hold State Machine Determination
-    if(!ap.auto_armed || !motors.get_interlock()) {
+    if(!ap.auto_armed) {
         althold_state = AltHold_Disarmed;
+    } else if (!motors.get_interlock()){
+        althold_state = AltHold_MotorStop;
     } else if (takeoff_state.running || takeoff_triggered){
         althold_state = AltHold_Takeoff;
     } else if (ap.land_complete){
@@ -78,6 +87,22 @@ void Copter::althold_run()
         pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
         break;
 
+    case AltHold_MotorStop:
+
+#if FRAME_CONFIG == HELI_FRAME    
+        // helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
+        // call attitude controller
+        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_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   // Multicopter do not stabilize roll/pitch/yaw when motor 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->control_in)-throttle_average);
+#endif  // HELI_FRAME
+        break;
+
     case AltHold_Takeoff:
 
         // initiate take-off

ArduCopter/control_loiter.cpp

@@ -9,6 +9,13 @@
 // loiter_init - initialise loiter controller
 bool Copter::loiter_init(bool ignore_checks)
 {
+#if FRAME_CONFIG == HELI_FRAME
+    // do not allow helis to enter Loiter if the Rotor Runup is not complete
+    if (!ignore_checks && !motors.rotor_runup_complete()){
+        return false;
+    }
+#endif
+
     if (position_ok() || ignore_checks) {
 
         // set target to current position
@@ -62,8 +69,10 @@ void Copter::loiter_run()
     }
 
     // Loiter State Machine Determination
-    if(!ap.auto_armed || !motors.get_interlock()) {
+    if(!ap.auto_armed) {
         loiter_state = Loiter_Disarmed;
+    } else if (!motors.get_interlock()){
+        loiter_state = Loiter_MotorStop;
     } 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){
@@ -88,6 +97,27 @@ void Copter::loiter_run()
         pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
         break;
 
+    case Loiter_MotorStop:
+
+#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.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
+
+        // 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->control_in)-throttle_average);
+#endif  // HELI_FRAME
+        break;
+
     case Loiter_Takeoff:
 
         if (!takeoff_state.running) {

ArduCopter/defines.h

@@ -199,6 +199,7 @@ enum RTLState {
 // Alt_Hold states
 enum AltHoldModeState {
     AltHold_Disarmed,
+    AltHold_MotorStop,
     AltHold_Takeoff,
     AltHold_Flying,
     AltHold_Landed
@@ -207,6 +208,7 @@ enum AltHoldModeState {
 // Loiter states
 enum LoiterModeState {
     Loiter_Disarmed,
+    Loiter_MotorStop,
     Loiter_Takeoff,
     Loiter_Flying,
     Loiter_Landed