diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index 00b46fa84b..38e9c65034 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -183,9 +183,7 @@ bool SoaringController::suppress_throttle() bool SoaringController::check_thermal_criteria() { - ActiveStatus status = active_state(); - - return (status == ActiveStatus::AUTO_MODE_CHANGE + return (_last_update_status == ActiveStatus::AUTO_MODE_CHANGE && ((AP_HAL::micros64() - _cruise_start_time_us) > ((unsigned)min_cruise_s * 1e6)) && (_vario.get_trigger_value() - _vario.get_exp_thermalling_sink()) > thermal_vspeed && _vario.alt < alt_max @@ -199,9 +197,7 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria(Vector2 // heading before some of these conditions will actually trigger. // The GCS messages are emitted in mode_thermal.cpp. Update these if the logic here is changed. - ActiveStatus status = active_state(); - - if (status == ActiveStatus::SOARING_DISABLED) { + if (_last_update_status == ActiveStatus::SOARING_DISABLED) { return LoiterStatus::DISABLED; } @@ -281,7 +277,7 @@ void SoaringController::init_thermalling() void SoaringController::init_cruising() { - if (active_state()>=ActiveStatus::MANUAL_MODE_CHANGE) { + if (_last_update_status >= ActiveStatus::MANUAL_MODE_CHANGE) { _cruise_start_time_us = AP_HAL::micros64(); // Start glide. Will be updated on the next loop. set_throttle_suppressed(true); @@ -366,18 +362,18 @@ float SoaringController::McCready(float alt) return thermal_vspeed; } -SoaringController::ActiveStatus SoaringController::active_state() const +SoaringController::ActiveStatus SoaringController::active_state(bool override_disable) const { - if (!soar_active) { + if (override_disable || !soar_active) { return ActiveStatus::SOARING_DISABLED; } return _pilot_desired_state; } -void SoaringController::update_active_state() +void SoaringController::update_active_state(bool override_disable) { - ActiveStatus status = active_state(); + ActiveStatus status = active_state(override_disable); bool state_changed = !(status == _last_update_status); if (state_changed) { diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h index 16b67f369f..9751c876c3 100644 --- a/libraries/AP_Soaring/AP_Soaring.h +++ b/libraries/AP_Soaring/AP_Soaring.h @@ -125,7 +125,7 @@ public: bool check_drift(Vector2f prev_wp, Vector2f next_wp); - void update_active_state(); + void update_active_state(bool override_disable); bool is_active() const {return _last_update_status>=SoaringController::ActiveStatus::MANUAL_MODE_CHANGE;}; @@ -143,7 +143,7 @@ private: ActiveStatus _pilot_desired_state = ActiveStatus::AUTO_MODE_CHANGE; - ActiveStatus active_state() const; + ActiveStatus active_state(bool override_disable) const; bool _exit_commanded; };