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

AP_Soaring: Add EXIT_COMMANDED status used when pilot changes switch position when thermalling.

This commit is contained in:
Samuel Tabor 2020-09-07 12:59:47 +01:00 committed by Tom Pittenger
parent 666314216d
commit 861b57cb09
2 changed files with 17 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -198,7 +198,9 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria(Vector2
LoiterStatus result = LoiterStatus::GOOD_TO_KEEP_LOITERING;
const float alt = _vario.alt;
if (alt > alt_max) {
if (_exit_commanded) {
result = LoiterStatus::EXIT_COMMANDED;
} else if (alt > alt_max) {
result = LoiterStatus::ALT_TOO_HIGH;
if (result != _cruise_criteria_msg_last) {
gcs().send_text(MAV_SEVERITY_ALERT, "Reached upper alt = %dm", (int16_t)alt);
@ -279,6 +281,8 @@ void SoaringController::init_thermalling()
_position_x_filter.reset(_ekf.X[2]);
_position_y_filter.reset(_ekf.X[3]);
_exit_commanded = false;
}
void SoaringController::init_cruising()
@ -393,10 +397,18 @@ void SoaringController::update_active_state()
// It's enabled, but wasn't on the last loop.
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Enabled, manual mode changes.");
set_throttle_suppressed(true);
// We changed mode - if we're in LOITER this means we should exit gracefully.
// This has no effect if we're cruising as it is reset on thermal entry.
_exit_commanded = true;
break;
case ActiveStatus::AUTO_MODE_CHANGE:
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Enabled, automatic mode changes.");
set_throttle_suppressed(true);
// We changed mode - if we're in LOITER this means we should exit gracefully.
// This has no effect if we're cruising as it is reset on thermal entry.
_exit_commanded = true;
break;
}
}

5
libraries/AP_Soaring/AP_Soaring.h
View File

@ -81,7 +81,8 @@ public:
THERMAL_WEAK,
ALT_LOST,
DRIFT_EXCEEDED,
GOOD_TO_KEEP_LOITERING
GOOD_TO_KEEP_LOITERING,
EXIT_COMMANDED,
};
enum class ActiveStatus {
@ -133,4 +134,6 @@ private:
ActiveStatus _pilot_desired_state = ActiveStatus::AUTO_MODE_CHANGE;
ActiveStatus active_state() const;
bool _exit_commanded;
};