From c0b424861941b4305d9a5d02a99298ee1bbbe391 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marek=20S=2E=20=C5=81ukasiewicz?= Date: Mon, 23 Feb 2026 09:24:26 +0100 Subject: [PATCH 1/2] 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 --- ArduPlane/Plane.h | 5 +++++ ArduPlane/mode.h | 22 +++++++++++++--------- ArduPlane/mode_guided.cpp | 18 ++++++++++++++++++ ArduPlane/mode_loiter.cpp | 13 +++++++++++++ ArduPlane/navigation.cpp | 14 +++++--------- ArduPlane/soaring.cpp | 21 +++++++++++++++++++++ 6 files changed, 75 insertions(+), 18 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1889fdee160521..adba5f88b00213 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1201,6 +1201,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 diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 9bae00e73a9f08..4f67a4000070ec 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -119,9 +119,6 @@ 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; } @@ -129,6 +126,9 @@ class Mode // 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,8 +261,6 @@ 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; @@ -270,6 +268,8 @@ class ModeAuto : 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; 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,8 +676,6 @@ 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; @@ -681,6 +683,8 @@ class ModeCruise : public Mode 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 {}; diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index cabcb1e8cd74ec..c27c2e80ba80d7 100644 --- a/ArduPlane/mode_guided.cpp +++ b/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(); } } diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index b2e3437bf58ebf..2267fd5fbc69e7 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/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(); } diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 6d8ca8e869a804..c24427582d9955 100644 --- a/ArduPlane/navigation.cpp +++ b/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 diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp index 6e8f6a12f19387..2f0db043ff3a81 100644 --- a/ArduPlane/soaring.cpp +++ b/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 From a77258e5492765454ae4040177c919f2cff8c95e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marek=20S=2E=20=C5=81ukasiewicz?= Date: Mon, 23 Feb 2026 09:24:26 +0100 Subject: [PATCH 2/2] Tools: 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 --- Tools/autotest/arduplane.py | 56 +++++++++++++++++++++++++++++++++++-- 1 file changed, 54 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 2d6fde18c644f4..b07da7c981446a 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -2552,6 +2552,7 @@ def Soaring(self): self.set_parameters({ "SOAR_VSPEED": 0.55, "SOAR_MIN_THML_S": 25, + "SOAR_ALT_MIN": 100, # Stay above nearby terrain }) self.set_current_waypoint(1) @@ -2594,9 +2595,15 @@ def Soaring(self): self.change_mode('FBWB') self.delay_sim_time(5) + # Descent to some in-between altitude + alt_mid = (alt_min + alt_ctf) / 2 + self.wait_altitude(alt_mid-10, alt_mid, timeout=600, relative=True) + # Now disable soaring (should hold altitude) self.set_parameter("SOAR_ENABLE", 0) - self.delay_sim_time(10) + self.delay_sim_time(20) + # Ensure the altitude is maintained (immediate check around upper threshold) + self.wait_altitude(alt_mid-10, alt_mid+10, timeout=1, relative=True) # And re-enable. This should force throttle-down self.set_parameter("SOAR_ENABLE", 1) @@ -2605,7 +2612,52 @@ def Soaring(self): # Now wait for descent and check throttle up self.wait_altitude(alt_min-10, alt_min, timeout=600, relative=True) - self.progress("Waiting for climb") + self.progress("Waiting for climb in FBWB") + self.wait_altitude(alt_ctf-10, alt_ctf, timeout=600, relative=True) + + # Now set GUIDED mode and go to a nearby location + self.change_mode("GUIDED") + loc = self.mav.location() + self.location_offset_ne(loc, 350, 0) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_REPOSITION, + p5=int(loc.lat * 1e7), + p6=int(loc.lng * 1e7), + p7=alt_ctf, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + ) + + # GUIDED should glide en route to target + self.wait_servo_channel_value(3, 1200, timeout=5, comparator=operator.lt) + self.wait_altitude(alt_mid-10, alt_mid, timeout=600, relative=True) + + # Maintain current altitude on soaring disable + self.set_parameter("SOAR_ENABLE", 0) + self.delay_sim_time(20) + self.wait_altitude(alt_mid-10, alt_mid+10, timeout=1, relative=True) + self.set_parameter("SOAR_ENABLE", 1) + + self.progress("Waiting for climb in GUIDED") + self.wait_altitude(alt_ctf-10, alt_ctf, timeout=600, relative=True) + + # Now set LOITER mode + self.change_mode("LOITER") + self.delay_sim_time(5) + + # Loiter should also glide around target + self.wait_servo_channel_value(3, 1200, timeout=5, comparator=operator.lt) + self.wait_altitude(alt_min-10, alt_min, timeout=600, relative=True) + + # Check altitude hold also in climb + self.wait_altitude(alt_mid-10, alt_mid, timeout=600, relative=True) + # Maintain current altitude on soaring disable + self.set_parameter("SOAR_ENABLE", 0) + self.delay_sim_time(20) + # We were climbing, so check around lower threshold + self.wait_altitude(alt_mid-20, alt_mid, timeout=1, relative=True) + self.set_parameter("SOAR_ENABLE", 1) + + self.progress("Waiting for climb in LOITER") self.wait_altitude(alt_ctf-10, alt_ctf, timeout=600, relative=True) # Back to auto