ArduPlane: enable soaring in LOITER and GUIDED modes. Maintain altitude on exit.

Co-authored-by: Julian <53869112+zyromskij@users.noreply.github.com>
Reviewed-by: Peter Barker <pbarker@barker.dropbear.id.au>

Author: Maarrk

ArduPlane/Plane.h

@@ -1198,6 +1198,11 @@ class Plane : public AP_Vehicle {
     // soaring.cpp
 #if HAL_SOARING_ENABLED
     void update_soaring();
+
+    // Handles the altitude target, returns true *once* after soaring is deactivated,
+    // the calling mode can use that to return to its normal operation after thermaling
+    bool set_soaring_altitude();
+    bool soaring_was_active = false;
 #endif
 
     // RC_Channel.cpp

ArduPlane/mode.h

@@ -119,16 +119,16 @@ class Mode
     // true if mode can have terrain following disabled by switch
     virtual bool allows_terrain_disable() const { return false; }
 
-    // true if automatic switch to thermal mode is supported.
-    virtual bool does_automatic_thermal_switch() const {return false; }
-
     // subclasses override this if they require navigation.
     virtual void navigate() { return; }
 
     // this allows certain flight modes to mix RC input with throttle
     // depending on airspeed_nudge_cm
     virtual bool allows_throttle_nudging() const { return false; }
 
+    // true if automatic switch to thermal mode is supported.
+    virtual bool does_automatic_thermal_switch() const { return false; }
+
     // true if the mode sets the vehicle destination, which controls
     // whether control input is ignored with STICK_MIXING=0
     virtual bool does_auto_navigation() const { return false; }
@@ -261,15 +261,15 @@ class ModeAuto : public Mode
     const char *name() const override { return "Auto"; }
     const char *name4() const override { return "AUTO"; }
 
-    bool does_automatic_thermal_switch() const override { return true; }
-
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
     void navigate() override;
 
     bool allows_throttle_nudging() const override { return true; }
 
+    bool does_automatic_thermal_switch() const override { return true; }
+
     bool does_auto_navigation() const override;
 
     bool does_auto_throttle() const override;
@@ -368,6 +368,8 @@ class ModeGuided : public Mode
 
     bool allows_throttle_nudging() const override { return true; }
 
+    bool does_automatic_thermal_switch() const override { return true; }
+
     bool does_auto_navigation() const override { return true; }
 
     bool does_auto_throttle() const override { return true; }
@@ -445,6 +447,8 @@ class ModeLoiter : public Mode
 
     bool allows_throttle_nudging() const override { return true; }
 
+    bool does_automatic_thermal_switch() const override { return true; }
+
     bool does_auto_navigation() const override { return true; }
 
     bool does_auto_throttle() const override { return true; }
@@ -646,11 +650,11 @@ class ModeFBWB : public Mode
 
     bool allows_terrain_disable() const override { return true; }
 
-    bool does_automatic_thermal_switch() const override { return true; }
-
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
+    bool does_automatic_thermal_switch() const override { return true; }
+
     bool does_auto_throttle() const override { return true; }
 
     bool mode_allows_autotuning() const override { return true; }
@@ -672,15 +676,15 @@ class ModeCruise : public Mode
 
     bool allows_terrain_disable() const override { return true; }
 
-    bool does_automatic_thermal_switch() const override { return true; }
-
     // methods that affect movement of the vehicle in this mode
     void update() override;
 
     void navigate() override;
 
     bool get_target_heading_cd(int32_t &target_heading) const;
 
+    bool does_automatic_thermal_switch() const override { return true; }
+
     bool does_auto_throttle() const override { return true; }
 
     void update_target_altitude() override {};

ArduPlane/mode_guided.cpp

@@ -3,6 +3,10 @@
 
 bool ModeGuided::_enter()
 {
+#if HAL_SOARING_ENABLED
+    plane.g2.soaring_controller.init_cruising();
+#endif // HAL_SOARING_ENABLED
+
     plane.guided_throttle_passthru = false;
     /*
       when entering guided mode we set the target as the current
@@ -94,6 +98,17 @@ void ModeGuided::update()
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle);
 
     } else {
+#if HAL_SOARING_ENABLED
+        if (plane.set_soaring_altitude()) {
+            // Just stopped soaring in this mode
+
+            // Return to guided target point at current altitude
+            Location loc = plane.next_WP_loc;
+            loc.copy_alt_from(plane.current_loc);
+            plane.set_guided_WP(loc);
+        }
+#endif // HAL_SOARING_ENABLED
+
         // TECS control
         plane.calc_throttle();
 
@@ -197,6 +212,9 @@ void ModeGuided::update_target_altitude()
     } else 
 #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
         {
+#if HAL_SOARING_ENABLED
+        plane.set_soaring_altitude();
+#endif // HAL_SOARING_ENABLED
         Mode::update_target_altitude();
     }
 }

ArduPlane/mode_loiter.cpp

@@ -3,6 +3,10 @@
 
 bool ModeLoiter::_enter()
 {
+#if HAL_SOARING_ENABLED
+    plane.g2.soaring_controller.init_cruising();
+#endif // HAL_SOARING_ENABLED
+
     plane.do_loiter_at_location();
     plane.setup_terrain_target_alt(plane.next_WP_loc);
 
@@ -25,6 +29,15 @@ void ModeLoiter::update()
         plane.update_fbwb_speed_height();
     } else {
         plane.calc_nav_pitch();
+#if HAL_SOARING_ENABLED
+        if (plane.set_soaring_altitude()) {
+            // Just stopped soaring in this mode
+
+            // Return to loitering at target point with current altitude
+            plane.set_target_altitude_current();
+            plane.next_WP_loc.set_alt_cm(plane.target_altitude.amsl_cm, Location::AltFrame::ABSOLUTE);
+        }
+#endif // HAL_SOARING_ENABLED
         plane.calc_throttle();
     }
 

ArduPlane/navigation.cpp

@@ -430,15 +430,11 @@ void Plane::update_fbwb_speed_height(void)
         change_target_altitude(alt_change_cm);
 
 #if HAL_SOARING_ENABLED
-        if (g2.soaring_controller.is_active()) {
-            if (g2.soaring_controller.get_throttle_suppressed()) {
-                // we're in soaring mode with throttle suppressed
-                set_target_altitude_current();
-            } else {
-                // we're in soaring mode climbing back to altitude. Set target to SOAR_ALT_CUTOFF plus 10m to ensure we positively climb
-                // through SOAR_ALT_CUTOFF, thus triggering throttle suppression and return to glide.
-                target_altitude.amsl_cm = 100*plane.g2.soaring_controller.get_alt_cutoff() + 1000 + AP::ahrs().get_home().alt;
-            }
+        if (plane.set_soaring_altitude()) {
+            // Just stopped soaring in this mode
+
+            // Continue flying with current altitude
+            plane.set_target_altitude_current();
         }
 #endif
 

ArduPlane/soaring.cpp

@@ -58,4 +58,25 @@ void Plane::update_soaring() {
     g2.soaring_controller.set_throttle_suppressed(false);
 }
 
+bool Plane::set_soaring_altitude()
+{
+    if (!g2.soaring_controller.is_active()) {
+        // Notify the calling mode that we're exiting soaring and it needs to reset altitude
+        bool previous = soaring_was_active;
+        soaring_was_active = false;
+        return previous;
+    }
+
+    if (g2.soaring_controller.get_throttle_suppressed()) {
+        // we're in soaring mode with throttle suppressed
+        set_target_altitude_current();
+    } else {
+        // we're in soaring mode climbing back to altitude. Set target to SOAR_ALT_CUTOFF plus 10m to ensure we positively climb
+        // through SOAR_ALT_CUTOFF, thus triggering throttle suppression and return to glide.
+        target_altitude.amsl_cm = 100 * (plane.g2.soaring_controller.get_alt_cutoff() + 10) + AP::ahrs().get_home().alt;
+    }
+    soaring_was_active = true;
+    return false;
+}
+
 #endif // SOARING_ENABLED