Nav altitude control improvements

[breadoven]

Should improve loiter altitude control behaviour during navigation.

Removes use of climb rate controller to control altitude change during certain Nav phases. The required altitude is instead set directly as the target altitude. Seems to give better control due to improved PID behaviour avoiding overshoot issues seen on fixed wing.

Adds target altitude to climb rate controller to allow climb rate reduction as the target altitude is reached. This was used to improve the above initially but then became redundant when climb rate control was removed. However, it still seems useful for Emergency landings to allow a higher rate of descent initially, reducing as take off altitude is approached (doesn't help if landing above take off obviously but it's a useful compromise perhaps). If the target altitude option is not used the altitude rate controller just provides a constant unlimited climb rate.

Also adds a change to reduce max climb pitch for a fixed wing when performing loiter climbs. This is for stall prevention and currently set at 2/3 of the normal max climb pitch setting (nav_fw_climb_angle).

HITL sim tested OK for FW. Untested on multirotor as of yet.

[Jetrell]

Just to clarify for Multicopter testing.

Removes use of climb rate controller to control altitude change during certain Nav phases. The required altitude is instead set directly as the target altitude. Seems to give better control due to improved PID behaviour avoiding overshoot issues seen on
fixed wing.

Untested on multirotor as of yet.

I wasn't completely sure where the two platforms would crossover in the use of this controller.. Does this now mean MC nav_auto_climb_rate will provide a fixed acceleration in the RTH climb out phase... i.e. instant hard acceleration ? If so, that could be a problem for larger MC's, If the attitude YAW gain was under tuned.

[breadoven]

Just to clarify for Multicopter testing.

Removes use of climb rate controller to control altitude change during certain Nav phases. The required altitude is instead set directly as the target altitude. Seems to give better control due to improved PID behaviour avoiding overshoot issues seen on
fixed wing.

Untested on multirotor as of yet.

I wasn't completely sure where the two platforms would crossover in the use of this controller.. Does this now mean MC nav_auto_climb_rate will provide a fixed acceleration in the RTH climb out phase... i.e. instant hard acceleration ? If so, that could be a problem for larger MC's, If the attitude YAW gain was under tuned.

The RTH climb phase for MC is unchanged. It never used the altitude rate controller, just set the target altitude directly which uses the MC specific altitude velocity controller to control climb rate. That was part of the reason for making this change, the existing code is a bit inconsistent with controllers used. The altitude rate controller should probably only be used where you want to set a climb rate with no specific target altitude, e.g. when adjusting altitude or performing an emergency descent.

[Jetrell]

@breadoven I tested these changes with a multirotor.
I first ran an RTH and a WP mission with varying altitudes at each Waypoint, then with a landing at the end... That was all fine as expected.

Then I proceeded to use my MC Cruise mode.. A combination of Acro, Althold and Heading hold modes..
This mode may seem like a rather unorthodox combination.. But I've always found it to be useful in holding a hands-off set bank angle, and a fixed altitude and heading, when cruising over longer distances, at higher speeds.
With this mode, I did notice a continual climbing of altitude, throughout the tests Which included, a headwind, tailwind and crosswind... Over my limited test distance (400m).

To elaborate on this combination flight mode.
I am aware of Altitude hold limitations for multirotors... Being that Navigation auto throttle is based on both hover throttle and bank angle... Which means, if you increase the bank angle excessively, the Nav throttle will end up transfering more of that thrust to forward propulsion, than to altitude control.
So I took this into account while testing to ensure the bank angle was constant.

I don't know if this is related. But it always proceeded to climb in Althold, by a meter every couple of seconds.
I didn't have a chance to test it back to back with the master.. But did compare it to other DVR files from a while back, using this mode. And the held altitude was always constant in those flights.

[breadoven]

Theoretically this change only affects multirotors during the RTH home altitude adjustment phase and during an emergency landing. It shouldn't have any other affect, certainly shouldn't make any difference to AltHold. The main benefit should be for fixed wing.

If the AltHold altitude is drifting it will be obvious by checking desired altitude in the BB log. Only reason that should change after AltHold is enabled is by RC adjustment.

[Jetrell]

I looked back through the logs.. And it had me stumped for a bit... But found the problem in another test.. Its all good.
The cause of this is often brought up by MC users at least once a month on Discord... Surrounding both Althold and Poshold.
I started a discussion #8933, surrounding this, if you'd be interested in joining in.

[Jetrell]

@breadoven I got around to testing this feature today with a fixed wing.
The wind was between 5 - 15km/h.. So I could observe the difference between nav_rth_home_altitude being made, while entering the loiter circuit with a headwind and tailwind... I did 8 tests to get a mix of both.

It now works much better than before... As it used to descend below the target by up to 15m, when the plane was in the descending loiter, on the headwind side of the circuit.
But now, under the same conditions, it starts pulling up about 4 - 6m before the target. Then levels off at the target. Then holds within a couple of meters, as it continues to loiter.

When nav_rth_home_altitude is made on the tailwind side of the loiter circuit, The altitude is a little more conservative (6 -10m). Meaning that it takes a little while longer to descend to the target altitude (about half a loiter circuit).. But I find that exceptable, because it never descends below the target as before. So it doesn't risk hitting tree tops, as it did with the previous code.

[Jetrell]

The RTH climb phase for MC is unchanged.

Theoretically this change only affects multirotors during the RTH home altitude adjustment phase

@breadoven In your above two statements.. I assume one is referencing the actual climb out altitude. And the other is the controlled altitude, as its traversing the distance between RTH activation and the home location ?

I notice some discrepancies in a few of my multi-rotor RTH tests recently, using the last commit.

The observation was seen in the RTH climbout phase.. And the RTH fly home altitude.
The quad falls short of reaching the nav_rth_altitude, by a meter or so... And it would also lose a couple of meters of altitude, on the trip back.. But this is not observed with the current method.

I thought I'd leave this as a reference, in case you return to this PR at some point.

[breadoven]

This change shouldn't affect MR during climb and on route RTH phases. Only affect is during the adjustment to home altitude. I guess the easiest way of checking if something unforeseen is happening is to look at the desired Z position in the log file. This will confirm if it's targeting the correct settings or not.

[Jetrell]

. I guess the easiest way of checking if something unforeseen is happening is to look at the desired Z position in the log file.

I checked out the log.. As it turns out. The altitude estimation wasn't perfect on the first few flight and the Sat count was rather low in those tests. And it appeared to improved a bit, by the time I moved back to test the original method.

So I ran another few tests today, with both firmware's, back to back. With a different quad, that's using an M10 GNSS unit.
The altitude estimation was spot on this time, with both the new method, and the original..
The home altitude appears to start the quad slowing it a little earlier than previous, but it still settles fine..
Although I don't imagine anyone adds a setting value to nav_rth_home_altitude for multicopters. Because most people want the quad to RTH and land, rather than just sitting there hovering above the ground.
,
I wasn't sure which controller drives nav_land_maxalt_vspd and nav_land_slowdown_minalt ? But both setpoints work fine, in slowing the quads descent speed before touch down.

[Jetrell]

@breadoven I tested this again today with a MC. Using the same settings as linear decent mode.
And also again with nav_rth_home_altitude set to a value 9m above the ground, with nav_rth_allow_landing = NEVER.
This stopped it precisely at that altitude in a hover.
And with it making a benifical difference to fixed wing loiter altitude control... Can this now be merged ? I think its passed the Testing Required stage. 😊

[breadoven]

Well I don't see any reason not to merge if testing seems OK. Should be an improvement on the current situation.