Navigator: Fix fixed-wing first order altitude hold #9850
Merged
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Issue
Navigator's fixed-wing first-order altitude hold (FOH) is currently causing altitude reference oscillations when in any LOITER mode. See screenshot below. Not only altitude, but also pitch and throttle can thus oscillate significantly. We observed this during a recent test flight.
Log file from SITL where this can be seen: https://review.px4.io/plot_app?log=f18c45ab-622e-4b1d-9819-03cfb06af2b5 . Here, after t=3:20 the true altitude setpoint is still the same as before t=2:20 (i.e. 630m AMSL) but the FOH logic just wrongly sets it to 660m and adds slight oscillations on top. The exact amount of oscillations depends on waypoint distance, altitude difference etc, but i have seen altitude ref oscillations of up to 30m, resulting in full pitch up/down of the aircraft.
Analysis
See the commit for the location of the code where this is happening. Essentially, every time that the loiter radius of any loiter WP is larger than the acceptance radius calculated from the L1 turn distance, then the Navigator FOH would not consider the waypoint reached and would thus not stop modifying the current altitude setpoint. This leads to the oscillations or offsets in the altitude reference.
Solution
If in any loiter mode, consider the loiter radius times a factor of 1.2 (same as for the waypoint_reached logic here as the acceptance radius. Tested in SITL, which should be sufficient for this case.
@acfloria @ryanjAA @Antiheavy FYI
@dagar Made this PR as a quick hotfix independently of your PR at https:/PX4/Firmware/pull/8883/files which supposedly also fixes this but is much larger and where it is more uncertain when this would be merged.