Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Soaring: Add an override-disable flag to update_active_state to support e.g. mission landing sequence.

This commit is contained in:
Samuel Tabor 2021-08-03 12:08:14 +01:00 committed by Andrew Tridgell
parent 5dea87ae41
commit 4564992b47
2 changed files with 9 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -183,9 +183,7 @@ bool SoaringController::suppress_throttle()
bool SoaringController::check_thermal_criteria() bool SoaringController::check_thermal_criteria()
{ {
ActiveStatus status = active_state(); return (_last_update_status == ActiveStatus::AUTO_MODE_CHANGE
return (status == ActiveStatus::AUTO_MODE_CHANGE
&& ((AP_HAL::micros64() - _cruise_start_time_us) > ((unsigned)min_cruise_s * 1e6)) && ((AP_HAL::micros64() - _cruise_start_time_us) > ((unsigned)min_cruise_s * 1e6))
&& (_vario.get_trigger_value() - _vario.get_exp_thermalling_sink()) > thermal_vspeed && (_vario.get_trigger_value() - _vario.get_exp_thermalling_sink()) > thermal_vspeed
&& _vario.alt < alt_max && _vario.alt < alt_max
@ -199,9 +197,7 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria(Vector2
// heading before some of these conditions will actually trigger. // 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. // The GCS messages are emitted in mode_thermal.cpp. Update these if the logic here is changed.
ActiveStatus status = active_state(); if (_last_update_status == ActiveStatus::SOARING_DISABLED) {
if (status == ActiveStatus::SOARING_DISABLED) {
return LoiterStatus::DISABLED; return LoiterStatus::DISABLED;
} }
@ -281,7 +277,7 @@ void SoaringController::init_thermalling()
void SoaringController::init_cruising() 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(); _cruise_start_time_us = AP_HAL::micros64();
// Start glide. Will be updated on the next loop. // Start glide. Will be updated on the next loop.
set_throttle_suppressed(true); set_throttle_suppressed(true);
@ -366,18 +362,18 @@ float SoaringController::McCready(float alt)
return thermal_vspeed; 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 ActiveStatus::SOARING_DISABLED;
} }
return _pilot_desired_state; 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); bool state_changed = !(status == _last_update_status);
if (state_changed) { if (state_changed) {

4
libraries/AP_Soaring/AP_Soaring.h
View File

@ -125,7 +125,7 @@ public:
bool check_drift(Vector2f prev_wp, Vector2f next_wp); 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;}; 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 _pilot_desired_state = ActiveStatus::AUTO_MODE_CHANGE;
ActiveStatus active_state() const; ActiveStatus active_state(bool override_disable) const;
bool _exit_commanded; bool _exit_commanded;
}; };