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(); g2.soaring_controller.update_vario();
// Check for throttle suppression change. if (control_mode == &mode_thermal) {
if (control_mode->does_automatic_thermal_switch()) { // We are currently thermalling; suppress throttle and update
g2.soaring_controller.suppress_throttle(); // the thermal mode.
}
else if (control_mode == &mode_thermal) {
// Never use throttle in THERMAL with soaring active. // Never use throttle in THERMAL with soaring active.
g2.soaring_controller.set_throttle_suppressed(true); 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 // Update THERMAL mode soaring logic.
if (!g2.soaring_controller.get_throttle_suppressed() && aparm.throttle_max > 0) { mode_thermal.update_soaring();
return; return;
} }
if (control_mode->does_automatic_thermal_switch()) { if (control_mode->does_automatic_thermal_switch()) {
// Test for switch into thermalling mode // We might decide to start thermalling; if we're not under
g2.soaring_controller.update_cruising(); // powered flight then check for thermals and potentially
// switch modes.
if (g2.soaring_controller.check_thermal_criteria()) { // Check for throttle suppression change.
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering %s", mode_thermal.name()); if (g2.soaring_controller.suppress_throttle()) {
set_mode(mode_thermal, ModeReason::SOARING_THERMAL_DETECTED); // 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) { return;
// Update thermal mode soaring logic.
mode_thermal.update_soaring();
} }
// We are not thermalling and won't start to from this mode. Allow throttle.
g2.soaring_controller.set_throttle_suppressed(false);
} }
#endif // SOARING_ENABLED #endif // SOARING_ENABLED