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.

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

Navigator: Fix fixed-wing first order altitude hold #9850

Merged
merged 1 commit into from Jul 16, 2018

Conversation

Projects
3 participants
@philipoe
Copy link
Contributor

philipoe commented Jul 6, 2018

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.

image

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://github.com/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.

Navigator: Fix fixed-wing first order altitude hold, i.e. the altitud…
…e reference oscillations caused by it in LOITER mode
@dagar

This comment has been minimized.

Copy link
Member

dagar commented Jul 6, 2018

This actually needs a little thought to review. There are a few situations where the position setpoint type and mission_item type are not the same.

@dagar dagar self-requested a review Jul 6, 2018

@philipoe

This comment has been minimized.

Copy link
Contributor Author

philipoe commented Jul 6, 2018

I can of course also check for (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) if you prefer that... just let me know.

@dagar

This comment has been minimized.

Copy link
Member

dagar commented Jul 6, 2018

If you can do it entirely from the position setpoint (ignoring mission_item) it should effectively avoid the edge cases (LOITER_TO_ALT, mission work items, etc).

@philipoe

This comment has been minimized.

Copy link
Contributor Author

philipoe commented Jul 6, 2018

So you'd also want to use acc_rad = _navigator->get_acceptance_radius(fabsf(pos_sp_triplet->current.loiter_radius) * 1.2f); instead of using the mission item loiter radius?

@dagar

This comment has been minimized.

Copy link
Member

dagar commented Jul 6, 2018

Yes, but drop the mission_item entirely and do a quick skim of the entire altitude_sp_foh_update() to make sure nothing is dependant on the mission item. This is one of the reasons I wanted to move it to the position controller. Multicopter skips this thing entirely.

@dagar dagar self-assigned this Jul 6, 2018

@dagar
Copy link
Member

dagar left a comment

Possibly not 100% correct in all situations to mix _mission_item and position_setpoints here.

@dagar

This comment has been minimized.

Copy link
Member

dagar commented Jul 6, 2018

Yes, but drop the mission_item entirely and do a quick skim of the entire altitude_sp_foh_update() to make sure nothing is dependant on the mission item. This is one of the reasons I wanted to move it to the position controller. Multicopter skips this thing entirely.

I'll can do this tomorrow if you can't get to it.

@philipoe

This comment has been minimized.

Copy link
Contributor Author

philipoe commented Jul 6, 2018

I'll can do this tomorrow if you can't get to it.

Sure, I guess you know exactly how you'd like to do it. Let me know if i should retest it afterwards then.

@dagar dagar added this to In progress in Fixed Wing Jul 12, 2018

@dagar

This comment has been minimized.

Copy link
Member

dagar commented Jul 16, 2018

Sure, I guess you know exactly how you'd like to do it.

Not really, I just see a number of subtle edge cases that make me uncomfortable.

@dagar

This comment has been minimized.

Copy link
Member

dagar commented Jul 16, 2018

I played around with this a bit, but there's still another (small) possible hole when the mission item nav_cmd is out of sync with the current position setpoint type.

Let's merge this now, but work on moving it to the position controller soon. From there it's much easier to safely handle the altitude ramp or hand off between loiter <-> position without fighting to plug numerous holes.

Fixed Wing automation moved this from In progress to Reviewer approved Jul 16, 2018

@dagar

dagar approved these changes Jul 16, 2018

@dagar dagar merged commit 79f172e into PX4:master Jul 16, 2018

2 checks passed

WIP ready for review
Details
continuous-integration/jenkins/pr-head This commit looks good
Details

Fixed Wing automation moved this from Reviewer approved to Done Jul 16, 2018

@dagar dagar added this to the Release v1.8.1 milestone Jul 27, 2018

dagar added a commit that referenced this pull request Jul 27, 2018

Navigator: Fix fixed-wing first order altitude hold (#9850)
i.e. the altitude reference oscillations caused by it in LOITER mode

@dagar dagar referenced this pull request Jul 27, 2018

Closed

PX4 Stable v1.8.1 Planning #10064

dagar added a commit that referenced this pull request Oct 19, 2018

Navigator: Fix fixed-wing first order altitude hold (#9850)
i.e. the altitude reference oscillations caused by it in LOITER mode

dagar added a commit that referenced this pull request Oct 20, 2018

Navigator: Fix fixed-wing first order altitude hold (#9850)
i.e. the altitude reference oscillations caused by it in LOITER mode
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment