diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index b4c428e72e..fd933831b6 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