From 814c52fbb730e6e1ba22e9056f9ea8ea155e977b Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Sun, 5 Dec 2021 18:57:05 -0500 Subject: [PATCH] AP_Motors: add spool down complete flag --- libraries/AP_Motors/AP_MotorsHeli.cpp | 2 +- libraries/AP_Motors/AP_MotorsHeli.h | 1 + libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 2 ++ libraries/AP_Motors/AP_MotorsHeli_Quad.cpp | 2 ++ libraries/AP_Motors/AP_MotorsHeli_RSC.cpp | 2 ++ libraries/AP_Motors/AP_MotorsHeli_RSC.h | 4 ++++ libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 3 +++ 7 files changed, 15 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 6d08d0ffc0..9a096bf3f5 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -414,7 +414,7 @@ void AP_MotorsHeli::output_logic() _spool_state = SpoolState::SPOOLING_UP; break; } - if (!rotor_speed_above_critical()){ + if (_heliflags.rotor_spooldown_complete){ _spool_state = SpoolState::GROUND_IDLE; } break; diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index e1377c31e3..47eb3b0693 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -247,6 +247,7 @@ protected: uint8_t land_complete : 1; // true if aircraft is landed uint8_t takeoff_collective : 1; // true if collective is above 30% between H_COL_MID and H_COL_MAX uint8_t below_mid_collective : 1; // true if collective is below H_COL_MID + uint8_t rotor_spooldown_complete : 1; // true if the rotors have spooled down completely } _heliflags; // parameters diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 54754cfc60..955fb9635f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -514,6 +514,8 @@ void AP_MotorsHeli_Dual::update_motor_control(RotorControlState state) // Check if rotors are run-up _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); + // Check if rotors are spooled down + _heliflags.rotor_spooldown_complete = _main_rotor.is_spooldown_complete(); } // diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index 21a38aa7cb..8ace8c33c3 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -210,6 +210,8 @@ void AP_MotorsHeli_Quad::update_motor_control(RotorControlState state) // Check if rotors are run-up _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); + // Check if rotors are spooled down + _heliflags.rotor_spooldown_complete = _main_rotor.is_spooldown_complete(); } // diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index adcd64ebb0..a5832debe0 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -382,6 +382,8 @@ void AP_MotorsHeli_RSC::update_rotor_runup(float dt) if (_runup_complete && (get_rotor_speed() <= get_critical_speed())) { _runup_complete = false; } + // if rotor estimated speed is zero, then spooldown has been completed + if (get_rotor_speed() <= 0.0f) { _spooldown_complete = true; } else { _spooldown_complete = false; } } // get_rotor_speed - gets rotor speed either as an estimate, or (ToDO) a measured value diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 72bf9f5dfb..a40b91b5c4 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -105,6 +105,9 @@ public: // is_runup_complete bool is_runup_complete() const { return _runup_complete; } + // is_spooldown_complete + bool is_spooldown_complete() const { return _spooldown_complete; } + // set_ramp_time void set_ramp_time(int8_t ramp_time) { _ramp_time = ramp_time; } @@ -163,6 +166,7 @@ private: bool _use_bailout_ramp; // true if allowing RSC to quickly ramp up engine bool _in_autorotation; // true if vehicle is currently in an autorotation int16_t _rsc_arot_bailout_pct; // the throttle percentage sent to the external governor to signal that autorotation bailout ramp should be used + bool _spooldown_complete; // flag for determining if spooldown is complete // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output void update_rotor_ramp(float rotor_ramp_input, float dt); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 9821dfe99c..8bb2bafda0 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -393,6 +393,9 @@ void AP_MotorsHeli_Single::update_motor_control(RotorControlState state) // Check if both rotors are run-up, tail rotor controller always returns true if not enabled _heliflags.rotor_runup_complete = ( _main_rotor.is_runup_complete() && _tail_rotor.is_runup_complete() ); + + // Check if both rotors are spooled down, tail rotor controller always returns true if not enabled + _heliflags.rotor_spooldown_complete = ( _main_rotor.is_spooldown_complete() ); } //