Support gliding setpoints for autonomous soaring #14643

Jaeyoung-Lim · 2020-04-11T09:32:45Z

Describe problem solved by this pull request
There is currently no way in px4 to enable the vehicle to "glide" along the setpoint.

Describe your solution
This PR enables a soaring behavior while following a position setpoint in offboard mode. A soaring setpoint is sent through a setpoint_type mask, which configures TECS to prioritize kinetic energy and thus enabling the vehicle to soar along it's glide slope, while loitering around its setpoint.

This is needed for autonomous soaring applications, where the fixed wing vehicle needs to soar around a setpoint without activating any throttle.

Test data / coverage
A test code can be found in https://github.com/Jaeyoung-Lim/mav_soaring, which is a ROS based implementation(WIP) of Ardusoar

Video:

Log: https://review.px4.io/plot_app?log=35a0647d-f69f-4a19-bfe1-ea66d099a9a1

dagar · 2020-04-11T14:54:45Z

Just thinking out loud for a minute, could we get this behavior for "free" (no special position setpoint type) by respecting the cruising_throttle field and have either 0 or a special value cause TECS to enable speed weight 2?

I'm also wondering about a manual soaring mode. Maybe instead of a separate mode it could be a side mechanism that allows cutting throttle in altitude mode and again switching the speed weight.

From playing around in the past I recall switching the speed weight in flight is quite abrupt, but that could be addressed separately.

Jaeyoung-Lim · 2020-04-11T19:10:18Z

@dagar That would indeed be better, since we can have use all waypoint types with / without gliding. However, if the setpoints are coming from offboard, what would be a good way to include that information in the local / global position messages?

dagar · 2020-04-12T16:01:05Z

However, if the setpoints are coming from offboard, what would be a good way to include that information in the local / global position messages?

I'm not sure what the cleanest (or least bad) way to fit it into SET_POSITION_TARGET_LOCAL_NED is, but within PX4 from the mavlink receiver on it could then just be a regular position setpoint with special throttle settings.

Wouldn't "soaring" be SET_POSITION_TARGET_LOCAL_NED with a combination of a body coordinate_frame with at least POSITION_TARGET_TYPEMASK_Z_IGNORE, POSITION_TARGET_TYPEMASK_VZ_IGNORE? More or less trying to encode the equivalent of speed weight 2 (always prioritize airspeed) with an interface that doesn't even mention airspeed.

sfuhrer · 2020-04-12T21:35:36Z

I also think we should find a solution that works well for all modes. Let's quickly think it through what a nice behavior for the user would be in all possible flight modes: (I'll just add whatever comes to my mind now, to have some points for a discussion)

full manual/rate control/stabilized: nothing special required, as one directly sets the throttle
altitude control and position control modes: it should be easy for the user to switch into and out of the soaring/gliding mode. In this mode, the user should still be able to control the airspeed (within min-max region). With that he indirectly also controls altitude to some extent, as it is coupled without having throttle). Nothing changes for lateral control.
go to/loiter: like when flying in altitude/position mode, the user should be able to switch in and out of the soaring/gliding mode, and not much else has to change
mission: that's a though one...if we don't want to introduce new mission item types we could for a start keep the soaring handling like with previous mentioned modes separate - the user switches in and out of it manually. Otherwise we would need to introduce a mission mode that automatically switches in and out of if, depending on the waypoint type and the current altitude error.
offboard: it has to be possible to pass along "soaring mode on/off" within the position setpoint

How to do it:

alt/pos/go to/loiter: what makes most sense for me atm is to have a separate switch for soaring on/off. Maybe it's also enough to have a parameter "allow_soaring" and then look at the throttle stick, and enter the mode if the stick is at 0.
mission: don't have a good idea right now that doesn't involve adding a special mission waypoint that would automatically climb when needed, and otherwise glide
offboard: I think @dagar's idea of using the POSITION_TARGET_TYPEMASK_Z_IGNORE for it could work (without having any offboard experience though). And then either just set z and vz of the postition setpoint to NAN, use cruising_throttle = 0 or add another field like do_soaring.

And then we ofc need to handle it properly inside fw_position controller and TECS, but let's first discuss the interface to it.

What do you think?

Jaeyoung-Lim · 2020-05-10T23:30:17Z

@dagar @sfuhrer I have changed the approach to enable soaring by setting the following flags true: POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_Z_IGNORE, POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_VZ_IGNORE, POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_AZ_IGNORE

This sets the cruising_throttle = -1.0 in the position setpoint, which triggers the TECS mode to be set as 2. I think cruising_throttle can act as a nice mechanism to enable soaring (when set to -1.0) since it also makes sense tecs to be configured to prioritize airspeed if the cruising throttle is small or -1.0

This only includes changes to enable offboard, but would be easy to extend this to other flight modes. although for including gliding in the mission would require a setpoint type that includes such information.

Would this be something more preferable than the previous approach?

sfuhrer

Yes for me setting cruising_throttle to -1 to indicate to the FW position controller that if should fly in gliding mode atm makes sense. Let's just make sure that we before did not set it to a negative value.

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

Jaeyoung-Lim · 2020-05-12T07:56:27Z

@dagar Is there anything I can improve on?

hamishwillee · 2020-06-21T22:45:14Z

@Jaeyoung-Lim Is there any doc impact to this?

Jaeyoung-Lim · 2020-06-22T02:42:22Z

@hamishwillee There is! However, this PR has been going through quite some changes, so I will create a doc once it is stable :)

Jaeyoung-Lim · 2020-06-26T15:15:11Z

@sfuhrer @dagar Is there anything else missing?

sfuhrer

It's good for me.

Adds a capability to make the vehicle glide when given position setpoints. Gliding setpoints are enabled by setting POSITION_TARGET_TYPEMASK_Z_IGNORE, POSITION_TARGET_TYPEMASK_VZ_IGNORE, POSITION_TARGET_TYPEMASK_AZ_IGNORE on the typemask of position setpoints. Include zero cruising throttle Invalid setpoints with nan

Jaeyoung-Lim · 2020-09-08T00:09:40Z

src/modules/navigator/mission.cpp

@@ -1912,6 +1912,7 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit
 		(p1->acceleration_is_force == p2->acceleration_is_force) &&
 		(fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) &&
 		(fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) &&
-		(fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON));
+		((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle)
+				&& !PX4_ISFINITE(p2->cruising_throttle)));


@dagar I couldn't think of any better way to handle the nans in this context. Any ideas?

Yes I think that makes sense, although maintaining this entire thing is pretty gross. We should look at generating comparison operators for uORB later. https://github.com/PX4/Firmware/blob/df38e3798c25541d1462d8011fbf0d58904ce4f7/msg/templates/uorb/msg.cpp.em#L85-L99

@dagar Right, makes sense

Jaeyoung-Lim · 2020-09-08T15:13:53Z

@dagar @sfuhrer If nothing else is missing, could we try and get this in?

dagar · 2020-09-14T00:04:41Z

Looks good to me.
@sfuhrer any concerns from your perspective?

sfuhrer

No, no objections from my side

Jaeyoung-Lim · 2020-09-14T08:56:25Z

@dagar @sfuhrer Thanks!

hamishwillee · 2020-09-14T23:16:21Z

@Jaeyoung-Lim Well done :-). Docs reminder.

Jaeyoung-Lim requested review from MaEtUgR and sfuhrer April 11, 2020 09:32

Jaeyoung-Lim mentioned this pull request Apr 11, 2020

Support gliding setpoints for autonomous soaring Jaeyoung-Lim/Firmware#3

Closed

Jaeyoung-Lim force-pushed the pr-soaring-setpoint branch from ce44fb4 to e368c35 Compare April 11, 2020 09:37

dagar added Admin: Enhancement (improvement) 💡 Fixed Wing 🛩️ labels Apr 11, 2020

Jaeyoung-Lim mentioned this pull request Apr 14, 2020

offboard: How to specify type of setpoint PX4/PX4-user_guide#588

Merged

weekly-digest bot mentioned this pull request Apr 19, 2020

Weekly Digest (12 April, 2020 - 19 April, 2020) #14700

Closed

Jaeyoung-Lim mentioned this pull request Apr 22, 2020

[ThermalSoaring] TODO Work Packages Jaeyoung-Lim/mav-soaring#8

Closed

7 tasks

Jaeyoung-Lim force-pushed the pr-soaring-setpoint branch from e368c35 to 9cbf24e Compare May 9, 2020 21:49

Jaeyoung-Lim force-pushed the pr-soaring-setpoint branch 2 times, most recently from 6a56029 to d16fd60 Compare May 10, 2020 23:36

sfuhrer reviewed May 11, 2020

View reviewed changes

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp Show resolved Hide resolved

Jaeyoung-Lim force-pushed the pr-soaring-setpoint branch 6 times, most recently from 62c3d9d to b70f6b7 Compare May 23, 2020 23:11

Jaeyoung-Lim force-pushed the pr-soaring-setpoint branch 4 times, most recently from 29540d8 to 074ad55 Compare June 21, 2020 21:04

Jaeyoung-Lim requested a review from dagar June 22, 2020 14:00

sfuhrer previously approved these changes Jun 29, 2020

View reviewed changes

weekly-digest bot mentioned this pull request Jul 5, 2020

Weekly Digest (28 June, 2020 - 5 July, 2020) #15258

Closed

Jaeyoung-Lim dismissed sfuhrer’s stale review via f2f6252 September 7, 2020 23:45

Jaeyoung-Lim force-pushed the pr-soaring-setpoint branch from 074ad55 to f2f6252 Compare September 7, 2020 23:45

Jaeyoung-Lim commented Sep 8, 2020

View reviewed changes

Handle nans for curise_throttle

a949a67

Jaeyoung-Lim force-pushed the pr-soaring-setpoint branch from 6562fec to a949a67 Compare September 8, 2020 04:25

dagar approved these changes Sep 14, 2020

View reviewed changes

sfuhrer approved these changes Sep 14, 2020

View reviewed changes

Jaeyoung-Lim merged commit 0592f6b into PX4:master Sep 14, 2020

Jaeyoung-Lim deleted the pr-soaring-setpoint branch September 14, 2020 08:57

Jaeyoung-Lim mentioned this pull request Sep 15, 2020

Add instructions for gliding setpoint PX4/PX4-user_guide#813

Merged

Jaeyoung-Lim added the Mode: Offboard Offboard mode control label Mar 1, 2021

Jaeyoung-Lim mentioned this pull request Jul 7, 2022

Improve fixed wing trim throttle compensation & geneal refactoring #19623

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Support gliding setpoints for autonomous soaring #14643

Support gliding setpoints for autonomous soaring #14643

Jaeyoung-Lim commented Apr 11, 2020 •

edited

Loading

dagar commented Apr 11, 2020

Jaeyoung-Lim commented Apr 11, 2020 •

edited

Loading

dagar commented Apr 12, 2020

sfuhrer commented Apr 12, 2020

Jaeyoung-Lim commented May 10, 2020 •

edited

Loading

sfuhrer left a comment

Jaeyoung-Lim commented May 12, 2020

hamishwillee commented Jun 21, 2020

Jaeyoung-Lim commented Jun 22, 2020

Jaeyoung-Lim commented Jun 26, 2020

sfuhrer left a comment

Jaeyoung-Lim Sep 8, 2020 •

edited

Loading

dagar Sep 8, 2020

Jaeyoung-Lim Sep 8, 2020

Jaeyoung-Lim commented Sep 8, 2020

dagar commented Sep 14, 2020

sfuhrer left a comment

Jaeyoung-Lim commented Sep 14, 2020

hamishwillee commented Sep 14, 2020

Support gliding setpoints for autonomous soaring #14643

Support gliding setpoints for autonomous soaring #14643

Conversation

Jaeyoung-Lim commented Apr 11, 2020 • edited Loading

dagar commented Apr 11, 2020

Jaeyoung-Lim commented Apr 11, 2020 • edited Loading

dagar commented Apr 12, 2020

sfuhrer commented Apr 12, 2020

Jaeyoung-Lim commented May 10, 2020 • edited Loading

sfuhrer left a comment

Choose a reason for hiding this comment

Jaeyoung-Lim commented May 12, 2020

hamishwillee commented Jun 21, 2020

Jaeyoung-Lim commented Jun 22, 2020

Jaeyoung-Lim commented Jun 26, 2020

sfuhrer left a comment

Choose a reason for hiding this comment

Jaeyoung-Lim Sep 8, 2020 • edited Loading

Choose a reason for hiding this comment

dagar Sep 8, 2020

Choose a reason for hiding this comment

Jaeyoung-Lim Sep 8, 2020

Choose a reason for hiding this comment

Jaeyoung-Lim commented Sep 8, 2020

dagar commented Sep 14, 2020

sfuhrer left a comment

Choose a reason for hiding this comment

Jaeyoung-Lim commented Sep 14, 2020

hamishwillee commented Sep 14, 2020

Jaeyoung-Lim commented Apr 11, 2020 •

edited

Loading

Jaeyoung-Lim commented Apr 11, 2020 •

edited

Loading

Jaeyoung-Lim commented May 10, 2020 •

edited

Loading

Jaeyoung-Lim Sep 8, 2020 •

edited

Loading