diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 3bb6bf95cd..bc9d5c05ae 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -17,6 +17,7 @@ #include #include "AP_MotorsHeli.h" #include +#include extern const AP_HAL::HAL& hal; @@ -598,3 +599,16 @@ AP_MotorsHeli_RSC::RotorControlState AP_MotorsHeli::get_rotor_control_state() co // Should be unreachable, but needed to keep the compiler happy return AP_MotorsHeli_RSC::RotorControlState::STOP; } + +// Update _heliflags.rotor_runup_complete value writing log event on state change +void AP_MotorsHeli::set_rotor_runup_complete(bool new_value) +{ +#if HAL_LOGGING_ENABLED + if (!_heliflags.rotor_runup_complete && new_value) { + LOGGER_WRITE_EVENT(LogEvent::ROTOR_RUNUP_COMPLETE); + } else if (_heliflags.rotor_runup_complete && !new_value && !_heliflags.in_autorotation) { + LOGGER_WRITE_EVENT(LogEvent::ROTOR_SPEED_BELOW_CRITICAL); + } +#endif + _heliflags.rotor_runup_complete = new_value; +} diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index c3c5cfc0c1..172fd39c61 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -233,6 +233,9 @@ protected: // update turbine start flag void update_turbine_start(); + // Update _heliflags.rotor_runup_complete value writing log event on state change + void set_rotor_runup_complete(bool new_value); + // enum values for HOVER_LEARN parameter enum HoverLearn { HOVER_LEARN_DISABLED = 0, diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 2849b3c85c..c9b645056d 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -374,7 +374,8 @@ void AP_MotorsHeli_Dual::update_motor_control(AP_MotorsHeli_RSC::RotorControlSta } // Check if rotors are run-up - _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); + set_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 af66de81db..817e89f1ac 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -159,7 +159,8 @@ void AP_MotorsHeli_Quad::update_motor_control(AP_MotorsHeli_RSC::RotorControlSta } // Check if rotors are run-up - _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); + set_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_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 65213c60bf..a996fb7d04 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -362,7 +362,7 @@ void AP_MotorsHeli_Single::update_motor_control(AP_MotorsHeli_RSC::RotorControlS } // 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() ); + set_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() );