From f3dc80597839cc363e8f1190c2aaaf6a94012526 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 6 Sep 2022 15:00:39 +0100 Subject: [PATCH] AP_Motors: Block Spoolup: stop advance from ground idle --- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 5 ++++- libraries/AP_Motors/AP_Motors_Class.cpp | 5 ----- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index d69b62b352..ebbfff5e28 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -612,7 +612,10 @@ void AP_MotorsMulticopter::output_logic() // constrain ramp value and update mode if (_spin_up_ratio >= 1.0f) { _spin_up_ratio = 1.0f; - _spool_state = SpoolState::SPOOLING_UP; + if (!get_spoolup_block()) { + // Only advance from ground idle if spoolup checks have passed + _spool_state = SpoolState::SPOOLING_UP; + } } break; diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 36f7117cb3..78932e8b7c 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -77,11 +77,6 @@ void AP_Motors::armed(bool arm) void AP_Motors::set_desired_spool_state(DesiredSpoolState spool) { - // check if spoolup has been blocked - if (_spoolup_block && (spool == DesiredSpoolState::THROTTLE_UNLIMITED)) { - return; - } - if (_armed || (spool == DesiredSpoolState::SHUT_DOWN)) { _spool_desired = spool; }