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; LoiterStatus result = LoiterStatus::GOOD_TO_KEEP_LOITERING;
const float alt = _vario.alt; 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; result = LoiterStatus::ALT_TOO_HIGH;
if (result != _cruise_criteria_msg_last) { if (result != _cruise_criteria_msg_last) {
gcs().send_text(MAV_SEVERITY_ALERT, "Reached upper alt = %dm", (int16_t)alt); 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_x_filter.reset(_ekf.X[2]);
_position_y_filter.reset(_ekf.X[3]); _position_y_filter.reset(_ekf.X[3]);
_exit_commanded = false;
} }
void SoaringController::init_cruising() void SoaringController::init_cruising()
@ -393,10 +397,18 @@ void SoaringController::update_active_state()
// It's enabled, but wasn't on the last loop. // It's enabled, but wasn't on the last loop.
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Enabled, manual mode changes."); gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Enabled, manual mode changes.");
set_throttle_suppressed(true); 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; break;
case ActiveStatus::AUTO_MODE_CHANGE: case ActiveStatus::AUTO_MODE_CHANGE:
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Enabled, automatic mode changes."); gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Enabled, automatic mode changes.");
set_throttle_suppressed(true); 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; break;
} }
} }

5
libraries/AP_Soaring/AP_Soaring.h
View File

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