diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp index 592dfaa811..4647091d79 100644 --- a/ArduPlane/soaring.cpp +++ b/ArduPlane/soaring.cpp @@ -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(); - - 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); + // We might decide to start thermalling; if we're not under + // powered flight then check for thermals and potentially + // switch modes. + + // 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