Navigation altitude and position controller reset improvements

[breadoven]

Closes #8882.

Should improve control behaviour when switching directly from one Nav mode to another, although change mainly affects AltHold and PosHold modes.

Tested on HITL fixed wing sim OK. Still needs testing on a multirotor.

[DzikuVx]

awesome, thanks a lot!

[Jetrell]

I ran some multi-rotor tests with this today.. Switching out of RTH in the midst of the climb out, to Althold and also Poshold.
In both cases, the climb was halted, and the altitude was held. With no sinking observed.

@breadoven Does this fix cross over with your issue here #5855 (comment)
Concerning fixedwing nav_rth_home_altitude target over-shoot. When the plane is in a descending loiter from nav_rth_altitude ?
No matter what changes I make to the settings on my planes. It always over-shoots, between 10 - 20mtrs, depending on the wind.. Then starts climbing back to the target.

[breadoven]

I ran some multi-rotor tests with this today.. Switching out of RTH in the midst of the climb out, to Althold and also Poshold. In both cases, the climb was halted, and the altitude was held. With no sinking observed.

The only question with this is when should the altitude controller should be set up again following a mode change. setupAltitudeController resets the hover zero point for a multirotor, e.g. you could switch from WP mode to position hold with the throttle stick lower down which as far as I can see would set the zero point with limited lower throttle travel (if throttle is fully low it defaults to using mid stick). Might be better to only set it up again when you change from no previous mode as it was before.

@breadoven Does this fix cross over with your issue here #5855 (comment) Concerning fixedwing nav_rth_home_altitude target over-shoot. When the plane is in a descending loiter from nav_rth_altitude ? No matter what changes I make to the settings on my planes. It always over-shoots, between 10 - 20mtrs, depending on the wind.. Then starts climbing back to the target.

The problem with descending to the RTH home altitude is the abrupt change from demanding descent to suddenly demanding hold at the home altitude. The controller just tries to maintain the set descent rate without taking into account the distance to the target altitude so the PID values remain constant-ish until the target altitude is reached then take time to unwind and reverse to a climb following the inevitable overshoot. The overshoot is probably worse for a fixed wing because it can't change vertical direction as quickly as a multirotor can. I'd have thought the only solution here is to taper off the descent rate as you get closer to the home altitude. I ended up setting the descent rate to 2m/s rather than the default of 5 m/s to limit the overshoot.

[Jetrell]

setupAltitudeController resets the hover zero point for a multirotor, e.g. you could switch from WP mode to position hold with the throttle stick lower down which as far as I can see would set the zero point with limited lower throttle travel (if throttle is fully low it defaults to using mid stick)

The transition was smooth in flight. I didn't notice any altitude jumpiness.

The problem with descending to the RTH home altitude is the abrupt change from demanding descent to suddenly demanding hold at the home altitude.

It just seems like a rather poor altitude PID controller.
For new users, this has always been a trap. They have to take into account that nav_rth_home_altitude = 30, actually means 40 - 50m. Or you will smash into the tree tops that your should be loitering above... It sort of makes the setting value irrelevant.

[breadoven]

Looking at it again the descent to RTH home altitude switches from a constant descent rate to targeting the home altitude itself 1s before arriving based on current descent rate. Likely the 1s is more relevant to multirotor rather than fixed wing so perhaps it would make sense to increase the 1s to some higher value for a fixed wing, perhaps as much as 5s. Although that's a different issue to this so would need to be done in a separate PR.

[breadoven]

Removed setting up altitude controller for WP mode. Doesn't seem relevant given WP mode doesn't allow RC altitude adjustment so no need to reset the throttle zero point. Other than that this is good to merge I think.

[helicocrasher]

Tested on fixed wing with iNAV6.1 RC1 - works awesome!
See here: #8882
before and after.