From 652283a5706eb4d73e29fba618dfbd7e1fe71522 Mon Sep 17 00:00:00 2001
From: Robert Lefebvre <robert.lefebvre@gmail.com>
Date: Wed, 11 Nov 2015 22:19:15 -0500
Subject: [PATCH] Copter: Helicopters to force descent when motor is shut off

---
 ArduCopter/control_althold.cpp | 27 ++++++++++++++++++++++++++-
 ArduCopter/control_loiter.cpp  | 32 +++++++++++++++++++++++++++++++-
 ArduCopter/defines.h           |  2 ++
 3 files changed, 59 insertions(+), 2 deletions(-)

diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp
index c531e893f3..3c51d3f1df 100644
--- a/ArduCopter/control_althold.cpp
+++ b/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);
@@ -56,8 +63,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){
@@ -82,6 +91,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
diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp
index 5ed36ef7d1..fbef7ccf16 100644
--- a/ArduCopter/control_loiter.cpp
+++ b/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
@@ -66,8 +73,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){
@@ -92,6 +101,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) {
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 59846aa573..2f6deb03f8 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -200,6 +200,7 @@ enum RTLState {
 // Alt_Hold states
 enum AltHoldModeState {
     AltHold_Disarmed,
+    AltHold_MotorStop,
     AltHold_Takeoff,
     AltHold_Flying,
     AltHold_Landed
@@ -208,6 +209,7 @@ enum AltHoldModeState {
 // Loiter states
 enum LoiterModeState {
     Loiter_Disarmed,
+    Loiter_MotorStop,
     Loiter_Takeoff,
     Loiter_Flying,
     Loiter_Landed