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

Plane: Re-order logic in soaring.cpp.

This commit is contained in:
Samuel Tabor 2020-12-30 20:26:08 +00:00 committed by Andrew Tridgell
parent a8549f5e5f
commit d97a81bfea
1 changed files with 25 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ArduPlane/soaring.cpp
View File

@ -19,36 +19,41 @@ void Plane::update_soaring() {
g2.soaring_controller.update_vario();
// Check for throttle suppression change.
if (control_mode->does_automatic_thermal_switch()) {
g2.soaring_controller.suppress_throttle();
}
else if (control_mode == &mode_thermal) {
if (control_mode == &mode_thermal) {
// We are currently thermalling; suppress throttle and update
// the thermal mode.
// Never use throttle in THERMAL with soaring active.
g2.soaring_controller.set_throttle_suppressed(true);
}
else {
// In any other mode allow throttle.
g2.soaring_controller.set_throttle_suppressed(false);
}
// Nothing to do if we are in powered flight
if (!g2.soaring_controller.get_throttle_suppressed() && aparm.throttle_max > 0) {
// Update THERMAL mode soaring logic.
mode_thermal.update_soaring();
return;
}
if (control_mode->does_automatic_thermal_switch()) {
// Test for switch into thermalling mode
g2.soaring_controller.update_cruising();
// We might decide to start thermalling; if we're not under
// powered flight then check for thermals and potentially
// switch modes.
if (g2.soaring_controller.check_thermal_criteria()) {
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering %s", mode_thermal.name());
set_mode(mode_thermal, ModeReason::SOARING_THERMAL_DETECTED);
// Check for throttle suppression change.
if (g2.soaring_controller.suppress_throttle()) {
// Throttle is suppressed, perform cruising modes update and check for mode switch.
// Cruising modes update.
g2.soaring_controller.update_cruising();
// Test for switch into THERMAL mode
if (g2.soaring_controller.check_thermal_criteria()) {
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering %s", mode_thermal.name());
set_mode(mode_thermal, ModeReason::SOARING_THERMAL_DETECTED);
}
}
} else if (control_mode == &mode_thermal) {
// Update thermal mode soaring logic.
mode_thermal.update_soaring();
return;
}
// We are not thermalling and won't start to from this mode. Allow throttle.
g2.soaring_controller.set_throttle_suppressed(false);
}
#endif // SOARING_ENABLED