integrate optional NPFG library for wind-aware fixed-wing guidance

[tstastny]

Describe problem solved by this pull request
High winds can degrade position tracking performance of slow-flying fixed-wing vehicles (or VTOL platforms in transitional states). Current mitigation logic in the fixed-wing position controller is based on an arbitrary threshold leading to jumpy behavior when winds rise close to the vehicle's nominal airspeed, see

PX4-Autopilot/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

Lines 2113 to 2116 in 495f1c9

	// if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
	if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
	nav_speed_2d = air_speed_2d;
	}

and inefficient (w.r.t. energy demands.. see similar discussion on ground speed undershoot / air-ground angle in #10219), see

PX4-Autopilot/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

Lines 384 to 395 in 495f1c9

	if (!_l1_control.circle_mode()) {
	/*
	* This error value ensures that a plane (as long as its throttle capability is
	* not exceeded) travels towards a waypoint (and is not pushed more and more away
	* by wind). Not countering this would lead to a fly-away.
	*/
	const float ground_speed_body = _body_velocity(0);
	if (ground_speed_body < _param_fw_gnd_spd_min.get()) {
	airspeed_demand += max(_param_fw_gnd_spd_min.get() - ground_speed_body, 0.0f);
	}
	}

Further, wind is currently estimated onboard, but not used to aid any higher level control loops.

Describe your solution
This PR adds an optional new library with an implementation of a more generalized Nonlinear Path Following Guidance (NPFG) logic with excess wind handing derived for minimum energy usage (endurance optimal behavior). If enabled (via parameter), the logic replaces L1 and the current ground speed undershoot logic.

NOTE: this PR does not replace any legacy functionality by default. The intention here is to make the logic available for users comfortable with a more advanced technique, get flight feedback, suggestions, end-user experiences, and hopefully encourage the restructuring of fixed-wing guidance handling to be more modular (see later notes on repeat code!). Attempted to make a minimally invasive addition.

Primary references:

• T. Stastny and R. Siegwart. "On Flying Backwards: Preventing Run-away of Small, Low-speed, Fixed-wing UAVs in Strong Winds". IEEE International Conference on Intelligent Robots and Systems (IROS). 2019. https://arxiv.org/pdf/1908.01381.pdf
• T. Stastny. "Low-Altitude Control and Local Re-Planning Strategies for Small Fixed-Wing UAVs". Doctoral Thesis, ETH Zürich. 2020. https://tstastny.github.io/pdf/tstastny_phd_thesis_wcover.pdf (Paper III)
• TODO: still wrapping up documentation for current implementation + stability analysis (done, not prettified)

Available wind handling modes:

1. Wind excess mitigation: reduce rate of run-away from track at the trim airspeed.
2. Wind excess regulation: adjust the airspeed setpoint to match the wind speed if it exceeds the airspeed.
3. Minimum forward ground speed maintenance: adjust the airspeed setpoint to maintain a minimum forward ground speed along the current bearing vector.
4. Track keeping: adjust the airspeed setpoint to return to the path until resting on track at zero ground speed until wind subsides.

The higher modes fall back into the lower ones when the required airspeed setpoint is not available (max airspeed exceeded). See the following video documentation and animation of the various modes and performance in flight tests for a general / layman's recounting of the logic (more details in the above paper references)

Other features:

• period and damping can be set just as in the L1 controller, so no change in tuning is necessary from legacy setups
• optional (advanced) period lower bounds (based on knowledge of the roll time constant) will automatically adapt the period and guard against possibly unstable tunings
• optional (advanced) upper period bounds, if enabled, will adapt the period to better track the given path

Gotchas:

• this controller will NOT work properly without sufficiently dynamic (and good) wind estimates. Flight experience has shown that estimating 1-2 second length gusts should be sufficient for good performance. With EKF2 - this translates to tuning up the EKF2_WIND_NOISE parameter to 1m/s/s.
• the controller will get disabled if the wind estimate is invalid -- however, current "invalid" is only defined here as having a finite value. E.g. covariance considerations would likely be better.
• the wind estimate will not converge immediately on take-off (initialized at 0m/s), so performance will lag a bit until the estimator catches up.
• I had to disable the "load factor" airspeed check in high winds.. otherwise it invalidates the airspeed for some reason.

Possible future strategies for the above points:

• use an instance of the stand alone wind estimator, tuned specifically for this guidance law.
• consider an optionally set wind speed and direction initialization before takeoff (e.g. measured on ground with anemometer)

Why should one use this logic?

• deterministic behavior in inclement conditions is desirable for an aircraft. Aside from the logic's endurance optimal properties, removing thresholding mitigation strategies means we have continuous acceleration commands while following a given path, no jumpy behavior, and the path following behavior is repeatable.
• as the logic is based on a generalized path following form - following future paths other than circles or lines require no additional augmentation to the guidance logic (assuming one has the closest point, and tangent vector available), something not easily doable with L1.
• no need for arbitrary minimum speed constraints, progressions through zero airspeed and zero ground speed are mapped to output controls continuously in the logic.
• no need for switching logic on circle tracking. stability bounds remain for any curvature, and there is a smooth bearing transition when transiting from far off the track until following on track.

DISCLAIMER: the controller will behave almost identically to L1 in the vast majority of cases. There isn't really a need to improve nominal performance. The gains come from the wind handling logic, and the future possibility to generalize to different path shapes.

Future recommendations:
There is a lot of copied over interfacing code from the L1 controller to keep the same functionalities available while using either guidance law (e.g. the roll slew limits). Also there is a mess of if conditions throughout the already somewhat hard to read fixed-wing position controller module. Would be good to perhaps design a guidance class, where the standard desired output methods can be available in any derived class. Further.. we should separate and encapsulate navigation logic, from guidance logic, and command output filtering. For example.. everything below the this line in both the header and cpp should ideally not be part of this library..

PX4-Autopilot/src/lib/npfg/npfg.cpp

Lines 517 to 519 in ea7a562

	/*******************************************************************************
	* PX4 NAVIGATION INTERFACE FUNCTIONS (provide similar functionality to ECL_L1_Pos_Controller)
	*/

Test data / coverage

A few simulations to more easily reproduce specific cases --

Comparison L1 + current wind mitigation logic vs wind-aware NPFG:
L1 14 m/s log: https://review.px4.io/plot_app?log=e752d5ff-4250-4d33-a0fa-48259ca71966
NPFG 14 m/s log: https://review.px4.io/plot_app?log=da1455d4-4358-4540-9845-9a848d426952

Same tuning - period = 10s, damping = 0.7, same conditions - airspeed trim = 15 m/s, wind speed = 14 m/s north.



Note the jumpy roll commands about the threshold of the air-ground angle and ground speed, and drifting behavior once the threshold is undershot. Further the objective 5m/s minimum ground speed setting is not maintained. NPFG keeps the circle, maintaining the 5m/s forward, in this case, minimum ground speed, and roll commands are continuous.

Example in SITL with wind speed (17m/s) exceeding the vehicle's trim airspeed:
L1 17 m/s log: https://review.px4.io/plot_app?log=c90a1c7c-34d3-4691-8e67-71729cc354e1
NPFG 17 m/s log: https://review.px4.io/plot_app?log=ac88c8f2-844b-4519-9031-f6d262abdce3

NPFG is shown in this log using the minimum forward ground speed mode. Keeping forward ground speed of 5m/s when the max airspeed allows, and reducing airspeed to the trim value when no additional airspeed is needed (e.g. down wind legs). Further, the end of the log has a track keeping mode demonstration (with min. gsp turned off) - where it ends up returning to the track.. and sitting at very near zero ground speed. Current L1 + ground speed undershoot gets stuck in place never reaching the first loiter (but this behavior will vary depending on the side from which it approached the loiter)

Flight testing --

The logic of the controller has been flight tested quite extensively in its various iterations (see some examples in https://arxiv.org/pdf/1908.01381.pdf and https://youtu.be/oM690LO29kM of flight in very strong winds). However, this new firmware (cleaned up, rebased, etc) still should need some new flight testing validation. Also checking behavior in other modes.. hold, landing, etc.

Happy for comments, concerns, and windy flight results :) @sfuhrer @Jaeyoung-Lim @acfloria @CarlOlsson @sverling (and anyone else!)

[sfuhrer]

Thanks a lot for this big contribution! Started testing it in SITL, it's like day and night when the wind speed is approaching the trim airspeed, and so far I couldn't get the new logic to break down:)
I so far only have some small feedback about the changes outside of the npfg library, and are now making my way through the algorithm. Looking forward to start testflying it on real vehicles after Christmas!

Concerning code structure/ library organization: Assuming we don't find any shortcomings compared to the legacy L1 guidance, do you even see the need of keeping both options on the long run?

[dagar]

@tstastny @Jaeyoung-Lim @sfuhrer maybe we could do a quick pass to get this into a safe state to merge (disabled by default)?

[Jaeyoung-Lim]

@dagar @sfuhrer We have been flying internally with this for a while now, and given that it has no influence when it is disabled, I would say we can still merge it.

@tstastny There is a small merge conflict that we need to resolve 😄

I think we should still get ethz-asl#54 included since it handles a corner case of the waypoint logic. I am more curious how we want to have L1 and NPFG co-exist. Is the current way of having two controller libraries built in and selected with a parameter good enough?

[tstastny]

I think we should still get ethz-asl#54 included since it handles a corner case of the waypoint logic.

@Jaeyoung-Lim - I made a change in e3b4e2a which should address this corner case.

see before (https://review.px4.io/plot_app?log=6f9f1b13-db52-457b-83e0-304c04dff891)

and after (https://review.px4.io/plot_app?log=baa005b0-be9f-4200-b527-1cd90ff4bc2c)

However, @sfuhrer I'm interested in your perspective on the other changes I made in that commit. I moved this handling to the mission block, as that seems like the more appropriate place to consider waypoint switches to me. In general.. this is part of a larger wish I have of separating out the guidance vs waypoint logic vs mission management.. right now too many things are done in one class (the guidance class). A lot of these extra condition checks could be simplified if we switch to defining e.g. "line segment" path types and "point" path types. But a discussion for another day..

It would be possible to instead take this passed the waypoint case as a point tracking case (as L1 currently does), but I fear in high winds when the vehicle is flying slow, or simply when the vehicle is far from the track laterally but then passes the waypoint, the old way of dealing with this (as in L1) could lead to jumpy switching between the two cases. Also not a fan of the 100 deg cone in L1, as this will even limit the space in which the aircraft tries to go back to the waypoint. Having the switch logic occur at either in the acceptance radius or passed the terminal waypoint simplifies all this.

[tstastny]

Concerning code structure/ library organization: Assuming we don't find any shortcomings compared to the legacy L1 guidance, do you even see the need of keeping both options on the long run?

As L1 is basically the same controller just with less features and less generality, I don't really see the need to keep it if it is only a question of these two particular formulations. The future question would be if there are other specialized guidance laws (very different from these two), or path following approaches others may want to include .. in which case we should anyway strive towards a more generic framework that can handle this without all the messy conditionals and repeat code / parameters.

[Jaeyoung-Lim]

@tstastny Could you rebase the PR to resolve the conflicts?

I would say we try to get this in as is, and try to deal with possible improvements on a separate PR
Not sure with the navigator changes can go in as is @sfuhrer @RomanBapst ?

[tstastny]

Could you rebase the PR to resolve the conflicts?

Done. Squashed all the review comment commits. Also needs #18996 merged first. Handles case shown #18810 (comment).

NPFG and fw pos ctrl logic wise... nothing has changed, but it seems there may have been some estimator or airspeed selector logic change since before the rebase? When I try to simulate with high winds the airspeed is getting disabled (and thus the wind estimate canceled). @sfuhrer has there been any augmentation to the airspeed estimation checks or something related I could look into to find why airspeed is failing now?

[tstastny]

@dagar I removed controller switching and allow default wind mitigation strategies with NPFG now on 9c94fb5 as discussed.

[tstastny]

rebased on master following #18996 merge

[sfuhrer]

NPFG and fw pos ctrl logic wise... nothing has changed, but it seems there may have been some estimator or airspeed selector logic change since before the rebase? When I try to simulate with high winds the airspeed is getting disabled (and thus the wind estimate canceled). @sfuhrer has there been any augmentation to the airspeed estimation checks or something related I could look into to find why airspeed is failing now?

The validator definitely doesn't like high wind, at least in SITL (on real vehicles we didn't have issues so far with up to 14 m/s). I've patched it with #19011, and would like to spend more time soon to understand what's exactly going on. #19011 is mainly there to unblock testing in high winds once this one is in.

[sfuhrer]

I've tested it quite extensively again in SITL, including simulating airspeed failures and giving npfg wrong wind estimates.The airspeed-less behavior is good, the legacy "groundspeed undershoot" logic seems to work fine together with npfg.
It is also able to keep tracking the loiter reasonably well with wind estimate that's 90° off at 10m/s wind (airspeed = 15), see printscreen. Assuming that even such a big wind estimate error really shouldn't happen in a real flight I would consider it pretty safe to bring in (but think about how we could estimate the validity of the current wind estimate and switch back to non-windy guidance in that case).

[tstastny]

format fixed e0fb286. Thanks @sfuhrer for the deep dive review / sim investigation!

but think about how we could estimate the validity of the current wind estimate and switch back to non-windy guidance in that case

Totally. Happy to work on this with you ;)

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-fixed-wing-mission-acceptance-radius/34693/5