Plane: Re-order logic in soaring.cpp.

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