Copter: throw uses motor spooling instead of interlock

ArduCopter/AP_State.cpp

@@ -122,7 +122,7 @@ void Copter::update_using_interlock()
 #else
     // check if we are using motor interlock control on an aux switch or are in throw mode
     // which uses the interlock to stop motors while the copter is being thrown
-    ap.using_interlock = check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) || (control_mode == THROW);
+    ap.using_interlock = check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK);
 #endif
 }
 

ArduCopter/Copter.h

@@ -411,8 +411,6 @@ private:
     Vector3f flip_orig_attitude;         // original copter attitude before flip
 
     // Throw
-    bool throw_early_exit_interlock = true; // value of the throttle interlock that must be restored when exiting throw mode early
-    bool throw_flight_commenced = false;    // true when the throw has been detected and the motors and control loops are running
     uint32_t throw_free_fall_start_ms = 0;  // system time free fall was detected
     float throw_free_fall_start_velz = 0.0f;// vertical velocity when free fall was detected
 
@@ -851,7 +849,6 @@ private:
 
     // Throw to launch functionality
     bool throw_init(bool ignore_checks);
-    void throw_exit();
     void throw_run();
     bool throw_detected();
     bool throw_attitude_good();

ArduCopter/arming_checks.cpp

@@ -51,7 +51,8 @@ bool Copter::pre_arm_checks(bool display_failure)
     // if it is, switch needs to be in disabled position to arm
     // otherwise exit immediately.  This check to be repeated,
     // as state can change at any time.
-    if (ap.using_interlock && motors.get_interlock()){
+    // skip check in Throw mode which takes control of the motor interlock
+    if (ap.using_interlock && motors.get_interlock()) {
         if (display_failure) {
             gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled");
         }
@@ -610,7 +611,8 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
     }
 
     // if we are using motor interlock switch and it's enabled, fail to arm
-    if (ap.using_interlock && motors.get_interlock()){
+    // skip check in Throw mode which takes control of the motor interlock
+    if (ap.using_interlock && motors.get_interlock()) {
         gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Interlock Enabled");
         return false;
     }

ArduCopter/control_throw.cpp

@@ -22,15 +22,6 @@ bool Copter::throw_init(bool ignore_checks)
     return true;
 }
 
-// clean up when exiting throw mode
-void Copter::throw_exit()
-{
-    // If exiting throw mode before commencing flight, restore the throttle interlock to the value last set by the switch
-    if (!throw_flight_commenced) {
-        motors.set_interlock(throw_early_exit_interlock);
-    }
-}
-
 // runs the throw to start controller
 // should be called at 100hz or more
 void Copter::throw_run()
@@ -48,31 +39,10 @@ void Copter::throw_run()
         // state machine entry is always from a disarmed state
         throw_state.stage = Throw_Disarmed;
 
-        // remember the current value of the motor interlock so that this condition can be restored if we exit the throw mode before starting motors
-        throw_early_exit_interlock = true;
-
-        // prevent motors from rotating before the throw is detected unless enabled by the user
-        if (g.throw_motor_start == 1) {
-            motors.set_interlock(true);
-        } else {
-            motors.set_interlock(false);
-        }
-
-        // status to let system know flight control has not started which means the interlock setting needs to restored if we exit to another flight mode
-        // this is necessary because throw mode uses the interlock to achieve a post arm motor start.
-        throw_flight_commenced = false;
-
     } else if (throw_state.stage == Throw_Disarmed && motors.armed()) {
         gcs_send_text(MAV_SEVERITY_INFO,"waiting for throw");
         throw_state.stage = Throw_Detecting;
 
-        // prevent motors from rotating before the throw is detected unless enabled by the user
-        if (g.throw_motor_start == 1) {
-            motors.set_interlock(true);
-        } else {
-            motors.set_interlock(false);
-        }
-
     } else if (throw_state.stage == Throw_Detecting && throw_detected()){
         gcs_send_text(MAV_SEVERITY_INFO,"throw detected - uprighting");
         throw_state.stage = Throw_Uprighting;
@@ -80,12 +50,6 @@ void Copter::throw_run()
         // Cancel the waiting for throw tone sequence
         AP_Notify::flags.waiting_for_throw = false;
 
-        // reset the interlock
-        motors.set_interlock(true);
-
-        // status to let system know flight control has started which means the entry interlock setting will not restored if we exit to another flight mode
-        throw_flight_commenced = true;
-
     } else if (throw_state.stage == Throw_Uprighting && throw_attitude_good()) {
         gcs_send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
         throw_state.stage = Throw_HgtStabilise;
@@ -142,12 +106,26 @@ void Copter::throw_run()
 
     case Throw_Disarmed:
 
+        // prevent motors from rotating before the throw is detected unless enabled by the user
+        if (g.throw_motor_start == 1) {
+            motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
+        } else {
+            motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
+        }
+
         // demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
         attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
         break;
 
     case Throw_Detecting:
 
+        // prevent motors from rotating before the throw is detected unless enabled by the user
+        if (g.throw_motor_start == 1) {
+            motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
+        } else {
+            motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
+        }
+
         // Hold throttle at zero during the throw and continually reset the attitude controller
         attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
 
@@ -158,6 +136,9 @@ void Copter::throw_run()
 
     case Throw_Uprighting:
 
+        // set motors to full range
+        motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+
         // demand a level roll/pitch attitude with zero yaw rate
         attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
 
@@ -168,6 +149,9 @@ void Copter::throw_run()
 
     case Throw_HgtStabilise:
 
+        // set motors to full range
+        motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+
         // call attitude controller
         attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
 
@@ -179,6 +163,9 @@ void Copter::throw_run()
 
     case Throw_PosHold:
 
+        // set motors to full range
+        motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+
         // run loiter controller
         wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
 

ArduCopter/flight_mode.cpp

@@ -266,10 +266,6 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr
 #endif  // MOUNT == ENABLED
     }
 
-    if (old_control_mode == THROW) {
-        throw_exit();
-    }
-
     // smooth throttle transition when switching from manual to automatic flight modes
     if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
         // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle

ArduCopter/switches.cpp

@@ -567,9 +567,6 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
             // control signal in tradheli
             motors.set_interlock(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
 
-            // remember the current value of the motor interlock so that this condition can be restored if we exit the throw mode early
-            throw_early_exit_interlock = motors.get_interlock();
-
             // Log new status
             if (motors.get_interlock()){
                 Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);