Copter: wait for motors to spool up before trying to upright vehicle

This stops us from progressing through the whole throw mode if the
vehicle just happens to be in the right state - which is can be for a
drop.

ArduCopter/mode.h

@@ -1468,11 +1468,13 @@ private:
     bool throw_position_good() const;
     bool throw_height_good() const;
     bool throw_attitude_good() const;
+    bool throttle_is_unlimited() const;
 
     // Throw stages
     enum ThrowModeStage {
         Throw_Disarmed,
         Throw_Detecting,
+        Throw_Wait_Throttle_Unlimited,
         Throw_Uprighting,
         Throw_HgtStabilise,
         Throw_PosHold

ArduCopter/mode_throw.cpp

@@ -51,12 +51,15 @@ void ModeThrow::run()
         stage = Throw_Detecting;
 
     } else if (stage == Throw_Detecting && throw_detected()){
-        gcs().send_text(MAV_SEVERITY_INFO,"throw detected - uprighting");
-        stage = Throw_Uprighting;
+        gcs().send_text(MAV_SEVERITY_INFO,"throw detected - spooling motors");
+        stage = Throw_Wait_Throttle_Unlimited;
 
         // Cancel the waiting for throw tone sequence
         AP_Notify::flags.waiting_for_throw = false;
 
+    } else if (stage == Throw_Wait_Throttle_Unlimited && throttle_is_unlimited()) {
+        gcs().send_text(MAV_SEVERITY_INFO,"throttle is unlimited - uprighting");
+        stage = Throw_Uprighting;
     } else if (stage == Throw_Uprighting && throw_attitude_good()) {
         gcs().send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
         stage = Throw_HgtStabilise;
@@ -140,6 +143,13 @@ void ModeThrow::run()
 
         break;
 
+    case Throw_Wait_Throttle_Unlimited:
+
+        // set motors to full range
+        motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+
+        break;
+
     case Throw_Uprighting:
 
         // set motors to full range
@@ -294,4 +304,20 @@ bool ModeThrow::throw_position_good() const
     // check that our horizontal position error is within 50cm
     return (pos_control->get_pos_error_xy_cm() < 50.0f);
 }
+
+bool ModeThrow::throttle_is_unlimited() const
+{
+    switch (motors->get_spool_state()) {
+    case AP_Motors::SpoolState::SHUT_DOWN:
+    case AP_Motors::SpoolState::GROUND_IDLE:
+    case AP_Motors::SpoolState::SPOOLING_UP:
+        return false;
+    case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
+        return true;
+    case AP_Motors::SpoolState::SPOOLING_DOWN:
+        return false;
+    }
+    // compiler ensures we never get here
+    return true;
+}
 #endif