Plane: Separate soaring update in mode_thermal to avoid loop rate issues.

ArduPlane/mode.h

@@ -548,6 +548,9 @@ public:
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
+    // Update thermal tracking and exiting logic.
+    void update_soaring();
+
     void navigate() override;
 
 protected:

ArduPlane/mode_thermal.cpp

@@ -26,6 +26,13 @@ void ModeThermal::update()
     plane.calc_nav_roll();
     plane.calc_nav_pitch();
     plane.calc_throttle();
+}
+
+void ModeThermal::update_soaring()
+{
+    // Update the thermal estimation and switching logic.
+    // This is called from soaring.cpp at fixed 50Hz to avoid
+    // potential issues with the main loop rate setting.
 
     // Update thermal estimate and check for switch back to AUTO
     plane.g2.soaring_controller.update_thermalling();  // Update estimate

ArduPlane/soaring.cpp

@@ -55,7 +55,11 @@ void Plane::update_soaring() {
             gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering %s", mode_thermal.name());
             set_mode(mode_thermal, ModeReason::SOARING_THERMAL_DETECTED);
         }
-
+        break;
+    case Mode::Number::THERMAL:
+        // Update thermal mode soaring logic.
+        mode_thermal.update_soaring();
+        break;
     } // switch control_mode
 }