fixed wing - dive at waypoints if target altitude is not reached

[RomanLut]

Current Behavior

If target altitude is not reached at the waypoint, navigation produces dangerous dive, with down pitch and minimum navigation throttle.

If mission is started at the taget altitude, then behaviour is ok.

Steps to Reproduce

1. Setup mission at very high altitude
2. Start mission below waypoint 1

Expected behavior

Navigation should gain altitude contantly

Demo

Mission is setup at 900m, with navigation throttle override enable, and throttle at 100% (where this effect is even more dramatic).

https://user-images.githubusercontent.com/11955117/235655648-89f3a94b-57c7-486b-9b3f-4599aec77a6a.mp4

If mission is started at target altitude ( 900m), behaviour is ok:

https://user-images.githubusercontent.com/11955117/235656469-fbd2d90b-5998-4166-a24e-25a6aa80b3ef.mp4

In the real flight, mission was set to 300m altitude and flight was started at 30m altitude. Plane has climb ratio about 1.5m/s. Plane crashed at the last RTH waypoint

https://user-images.githubusercontent.com/11955117/235660080-2388e410-5366-46b7-95d1-2f8697c2afbd.mp4

Unfortunatelly I do not have blackbox (only FPV recording with OSD and onboard HD), but problem can be easily reproduced in HITL.

Affected versions: 6.0 release and current master.

INAV_6.0.0_cli_FPV_SURFWING_dive-mission.txt

[RomanLut]

Attached blackbox log, generated in HITL.

blackbox_log_2023-05-02_143047.TXT

[breadoven]

This does look like the issue hopefully fixed by #8896. I'm surprised nobody noticed it before because it's probably been there for some time.

[breadoven]

Actually looking at the log it's the same issue as #8882 but working in a different way due to the way target altitude is set during each WP leg and the affect this has in the altitude PID P and I values. It would need a different fix, probably resetting the PIDs at the start of each leg. Flagging as a bug for now.

[RomanLut]

Not fixed by https://github.com/iNavFlight/inav/pull/8896.

Attached is a video from 6.0 + #8896, nav_fw_pos_z_p=35 nav_fw_pos_z_p = 5 nav_fw_pos_z_d = 10 with POSZ PID controller outputs on OSD.

Note that real flight video above is done on firmware from master branch (later than 6.1 release)

https://user-images.githubusercontent.com/11955117/235903308-5d8e47c3-07a1-4081-b214-71fd1ca73f86.mp4

[breadoven]

I've actually tested using vertical velocity for fixed wing to control altitude change rather than using Z position which is the way it works currently. Using velocity doesn't upset the PID loop the way it does using Z position so avoids the problem shown here. Seems the reason velocity wasn't used for fixed wing was because a fixed wing might not use a Barometer ... which doesn't really make sense. Even without a Baro the Z velocity should still be consistent enough to adequately control fixed wing altitude.

Seems to work better using velocity when testing on HITL and offers other benefits as well. Would need flight testing with Baro disabled though to really understand how well it works with variations in GPS altitude.

[shota3527]

I dont think this is a bug, this issue is just caused by anti-windup in nav pid controller, iterm turns the opposite when the controller is saturated

[b14ckyy]

Was this fixed with #9224?

[RomanLut]

The BUG is fixed. Still drops throttle in waypoints. I guess because target height slope is built starting from current altitude in each waypoint, as can be seen by P term drop. Closing.

https://github.com/iNavFlight/inav/assets/11955117/91130988-c75d-40fe-9f6d-73cacafced98