Hi. I’m using pixhawk 2.4.8 hardware with px4 last stable version. When I’m switching to mission mode plane start following waypoints but it assuming reached about 50 meter. I tried to change L1_Period to reduce L1_distance as stated in this documentMission Mode | PX4 User Guide (changed from 20 to 12)
@sfuhrer I couldn’t find this information - MISSION mode points to NAV_ACC_RAD which says this is set by “NPFG switch distance”.NPFG_SW_DST_MLT but that points to " track error boundary" - at which point I am lost. Would be good to put this info in our new mission mode fixed wing doc
1 Like
Thanks for your answer. I will try to find more info. Also I couldn’t find anywhere what NPFG stands for ? It’s a general control term or specific for PX4 ?
NPFG is the replacement for the Fixed Wing L1 navigation in PX4 v1.14. I’m still waiting for a bit of explanation on how it differs and what docs we need (though we fixed up the parameters in various places in the docs). I am GUESSING it stands for Nonlinear Path Following Guidance or similar, but who knows. Perhaps @tstastny or @sfuhrer could answer that too.
I found PR about this topic. As you said it’s stand for “Nonlinear path following guidance”. :
PX4:master ← ethz-asl:pr-npfg
opened 10:35PM - 07 Dec 21 UTC
**Describe problem solved by this pull request**
High winds can degrade positio… n 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
https://github.com/PX4/PX4-Autopilot/blob/495f1c916564c867bc216ccd0ef28361fea341e3/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp#L2113-L2116
and inefficient (w.r.t. energy demands.. see similar discussion on ground speed undershoot / air-ground angle in #10219), see
https://github.com/PX4/PX4-Autopilot/blob/495f1c916564c867bc216ccd0ef28361fea341e3/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp#L384-L395
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)
[](https://youtu.be/oM690LO29kM)
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..
https://github.com/PX4/PX4-Autopilot/blob/ea7a56290ad38a320ece772711e94752b7d5c40d/src/lib/npfg/npfg.cpp#L517-L519
**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!)
Someone who want to get idea rapidly about NPFG can watch this interesting video about Flying Backwards:
By the way NPFG not enabled by default. My acceptance radius problem solved with L1 tuning. I’m not sure but I guess waypoint tracking working better when launch the mission with automatic takeoff instead of switch to mission in air.
Again thank you very much for your interest. And thanks for development and test video to @tstastny
Hi @Alp_Ucan - the video you found is a bit dated for the current implementation on PX4 - you can find a better explanation with params etc in last year’s dev summit talk: https://youtu.be/LY6hYBCdy-0?si=PMqub0RA0ZW2ukqO
When I’m switching to mission mode plane start following waypoints but it assuming reached about 50 meter. I tried to change L1_Period to reduce L1_distance as stated in this document
There is no notion of L1 distance as a parameter on previous iterations of L1 or in NPFG - the L1 distance is calculated automatically based on the speed the aircraft is flying, and your tuning gains, PERIOD and DAMPING. The idea is that you want consistent dynamic behavior at any speed. You should find a tuning of period and damping that satisfy the convergence behavior to a line that you like. For switch distance, it does use the L1 distance, calculated from the prior variables and speed… but if you are unhappy with it’s default, you can use the SW_DST_MLT to adjust that per it’s param description.
Hope this helps.
1 Like
