Skip to content

ArduPlane: enable soaring in LOITER and GUIDED modes #32034

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Maarrk wants to merge 2 commits into
base: master
Choose a base branch
from
Open

ArduPlane: enable soaring in LOITER and GUIDED modes #32034

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c0b4248
ArduPlane: enable soaring in LOITER and GUIDED modes. Maintain altitu…
Maarrk Feb 23, 2026
a77258e
Tools: enable soaring in LOITER and GUIDED modes. Maintain altitude o…
Maarrk Feb 23, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions ArduPlane/Plane.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
22 changes: 13 additions & 9 deletions ArduPlane/mode.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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; }
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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; }
Expand Down Expand Up @@ -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; }
Expand Down Expand Up @@ -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; }
Expand All @@ -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 {};
Expand Down
18 changes: 18 additions & 0 deletions ArduPlane/mode_guided.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -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();
Copy link
Copy Markdown
Contributor

@Georacer Georacer Feb 18, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm a bit hesitant with this. In FBWB and CRUISE the pilot files VLOS or at least VLOS.
But with LOITER and GUIDED, terrain becomes an issue. How will this play nice with a terrain where ALT_CUTOFF+10+<home_alt> is still lower than the terrain elevation?

Copy link
Copy Markdown
Contributor

@Georacer Georacer Feb 18, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Perhaps an answer to my question would be "the same way this is treated in AUTO".

Copy link
Copy Markdown
Contributor Author

@Maarrk Maarrk Feb 19, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

All altitudes SOAR_ALT_MIN, SOAR_ALT_CUTOFF and SOAR_ALT_MAX are relative to home. Currently there no exceptions to that, including AUTO mode.

If we were to support a terrain height check, I think we should make it follow the present Terrain Following feature. Add another bit to TERRAIN_FOLLOW for Soaring and then check SOAR_ALT_MIN against terrain instead of home if that bit is set. But I think that both regarding code and user experience that is a separate feature that we can discuss in a separate Issue/PR if you think it's a requirement for GUIDED

#endif // HAL_SOARING_ENABLED
Mode::update_target_altitude();
}
}
13 changes: 13 additions & 0 deletions ArduPlane/mode_loiter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -25,6 +29,15 @@ void ModeLoiter::update()
plane.update_fbwb_speed_height();
} else {
plane.calc_nav_pitch();
#if HAL_SOARING_ENABLED
Copy link
Copy Markdown
Contributor

@peterbarker peterbarker Feb 21, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copy/paste problem - make a method in the base class

Copy link
Copy Markdown
Contributor Author

@Maarrk Maarrk Feb 23, 2026 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I moved as much logic as I can to the set_soaring_altitude function of Plane, but the modes manage their internal state a bit differently, I don't think I can factor it out anymore.

Now the function has considerably more complicated return value, but it's documented at declaration and all call sites.

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);
Comment on lines +37 to +38
Copy link
Copy Markdown
Member

@IamPete1 IamPete1 May 10, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think this will work correctly if terrain following is enabled? I see we follow the same pattern for nav-scripting, but that might well be wrong too.

To test make sure that terrain following works before and after soaring kicks in.

Copy link
Copy Markdown
Contributor Author

@Maarrk Maarrk May 11, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I haven't done any work with terrain following, I will familiarise myself with this feature and test/autotest it, thanks for the hint

}
#endif // HAL_SOARING_ENABLED
plane.calc_throttle();
}

Expand Down
14 changes: 5 additions & 9 deletions ArduPlane/navigation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Copy link
Copy Markdown
Contributor

@Georacer Georacer Mar 17, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Let me see if I get this right.

Before:

  • If soaring was active
    • and throttle was suppressed, we would maintain altitude.
    • otherwise we would climb to min soaring alt.
  • Otherwise do nothing.

Now:

  • (entering set_soaring_altitude()) If soaring is not active, then return True the first time.
    • Set the altitude target to the current altitude.
    • The second time over, nothing will happen. Alt. control goes as usual.
  • If soaring is active
    • and throttle is suppressed, then let altitude glide down.
    • otherwise climb to min soaring alt.
    • FINALLY return false (returned to update_fbwb_speed_height() and do nothing more.

It's a bit complicated, but right now I don't have an alternative at hand.

Copy link
Copy Markdown
Contributor Author

@Maarrk Maarrk Mar 18, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Before:

If soaring was active
and throttle was suppressed, we would maintain altitude.

We would actually let the aircraft glide down, since throttle is suppressed.

otherwise we would climb to min soaring alt.

This code was moved to set_soaring_altitude because previously the same logic was in a bunch of places.

I know this call site got a bit more involved, but on the request of @peterbarker now the switching logic is provided from only one place. The return value of the function is there to allow the mode calling it to know "we just ended soaring". I couldn't do the "maintain current altitude" feature from soaring itself, because it's a bit different for each supported flight mode. Most importantly, the behaviour is not complicated from end user's perspective:

In supported modes (not AUTO) the vehicle will maintain current altitude when soaring is disabled.
quoted from ArduPilot/ardupilot_wiki#7457)

I feel that this is easy to understand, but also makes sense that we don't lose all the energy we saved on powered climb to what FBWB happened to be flying before a thermal was detected.

BUT it really needs the corresponding wiki changes.

Completely agree, and I think it would be fair to introduce "Merge on wiki completion" policy for user-facing features. But that's a discussion for another day :)

}
#endif

Expand Down
21 changes: 21 additions & 0 deletions ArduPlane/soaring.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Copy link
Copy Markdown
Member

@IamPete1 IamPete1 May 10, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This flag is only kept upto date in the new modes, it needs to be cleared when changing to a mode that does not support soaring. Maybe clear it in the init_cruising function?

return false;
}

#endif // SOARING_ENABLED
56 changes: 54 additions & 2 deletions Tools/autotest/arduplane.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand All @@ -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
Expand Down
Loading