From ce408ca58bee438ef1c000375b8b16d3c0cc3e5d Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 2 Jul 2021 16:37:03 +1000
Subject: [PATCH] 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         |  2 ++
 ArduCopter/mode_throw.cpp | 30 ++++++++++++++++++++++++++++--
 2 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 74817f5bef..9bea00e4c1 100644
--- a/ArduCopter/mode.h
+++ b/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
diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp
index c039af7acd..6a67665cc1 100644
--- a/ArduCopter/mode_throw.cpp
+++ b/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