Option to use position for fixed wing nav altitude control

[breadoven]

Adds an option to use position based nav altitude control on fixed wing instead of velocity based control which doesn't appear to work reliably on some plane types. Option enabled with new setting nav_fw_alt_use_position (OFF by default).

nav_fw_alt_control_response still works with the position based control in the same way as for velocity based control.

Only tested on HITL so far where it seems to work well. The new method of controlling altitude rate appears to be a big improvement on the old position based altitude control method with rates being held more accurately with limited pitching oscillations compared to previously. PID values may need further fine tuning.

[breadoven]

@sensei-hacker It needs testing but it's not experimental. Pretty much as it was before #9471 but with some improvements for rate control.

[ProfDrYoMan]

There is an issue open which might be relevant for this one:
#10773

[Jetrell]

@breadoven I flew a few flights today with one of the planes that had tuning issues using the velocity altitude controller.
I used the OSD/Adjustments for inflight tuning. Starting out with the PID's that I had set for the Vel controller, which still caused it to pitch oscillate. P = 18, I = 8, D = 2

After enabling nav_fw_alt_use_position. I noticed oscillations where no longer there... But as expected, it was very loose with that tune.
After playing around for a while in Cruise, RTH and Loiter. I settled on P = 32, I = 9, D = 8. And nav_fw_alt_control_response = 24.

When entering a descending loiter and then watching it level out at nav_rth_home_altitude. I was surprised when it consistently only went 2 - 3m below the target, and recovered in half a loiter circuit... I seem to recall the old altitude position controller was often much worse.
I will add to this. That I have my DIVE angle at 17° and from the EXTRA altitude. It was descending 60m. So it gets-up some momentum.

I could have tuned it a bit tighter and you would never have noticed any problems. But when panning the camera back to look at the elevator. I could see slight oscillations. That's why I settled with the values I have.

So it seems beneficial for this plane.. I'll be interested to hear what others think.

[breadoven]

@Jetrell You could set nav_fw_alt_control_response lower if you want to limit overshoot even more. One thing of note is the D term used internally by the PID loop is effectively 3x larger than the value used previously for the old position based altitude control. Seems to work better I thought with higher D terms. I was using setting values of 70 before without any issues although that was probably overkill. With default values you got a lot of altitude oscillations during position hold.

Good to see this works especially the new method used for rate control. Much better than the old method.

[majianjia]

I like the idea with rate control vertical speed. However, I think the default value can be improve. The new default was a bit on the aggressive side.

[Jetrell]

After running some more tests with this today. And proceeding to increase the gains. There was no possibility of getting near what could be achieved with the old nav_fw_pos_z controller.
For name sake; I'm leaning towards this still having more in common with the velocity controller, than the old Position controller, when it comes to tuning.

If I had to rate it in tuning similarity from 1 to 10.. 1) Position and 10) Velocity. I'd say it's an 8.
With nav_fw_control_smoothness playing a huge part in its tuning. A nice tune with it set to 9, goes very pear shaped if that setting is dropped down to 2 or less.

However it has still taken the edge off those harder to tune planes. But it's not as care-free as the old position controller. But it's also more precise than it was.

[breadoven]

After running some more tests with this today. And proceeding to increase the gains. There was no possibility of getting near what could be achieved with the old nav_fw_pos_z controller. For name sake; I'm leaning towards this still having more in common with the velocity controller, than the old Position controller, when it comes to tuning.

If I had to rate it in tuning similarity from 1 to 10.. 1) Position and 10) Velocity. I'd say it's an 8. With nav_fw_control_smoothness playing a huge part in its tuning. A nice tune with it set to 9, goes very pear shaped if that setting is dropped down to 2 or less.

However it has still taken the edge off those harder to tune planes. But it's not as care-free as the old position controller. But it's also more precise than it was.

Is this related to rate control behaviour or maintaining an altitude such as during cruise or both ?

If it's an issue during cruise when it's trying to maintain an altitude then the only things that are different to the old postion based control are:

1. D term is based on error rather than measurement. I'll switch this back to measurement and see if it changes anything.
2. As mentioned above the D term factor is roughly 3 times more effective than previously. It should be possible to compensate for this simply by reducing the D term by a factor of 3.
3. PID factors are 10 times smaller than used previously because the target and measured values used in the PID loop are 10 times bigger (because of a gravity factor used for the old energy balance calcs used for position based control). Theoretically these differences should cancel out but maybe they subtly don't for I and D. You could change things back so it's the same as before by factoring up the target and measured altitudes by 10 and reduce PID factors accordingly. We could certainly try this, might be interesting to see if that's the issue.

If the above changes are made then the PID control will be exactly the same as before. If it still behaves differently then I'm afraid it'll be down so some other change that's come in at version 8.0.0 because as far as the PID controller is concerned inputs and the PID factors will be the same as before so it should output the same pitch control demands as previously.

[Jetrell]

Is this related to rate control behaviour or maintaining an altitude such as during cruise or both ?

It was related to both.

Yesterday I started out with P = 30, and had I and D set to the old defaults for the Position controller, 5 and 10.
With it causing wild oscillations as soon as I entered Cruise mode.
So I landed and reduced D = 5 and set nav_fw_alt_control_response = 14.. And that was with nav_fw_control_smoothness = 3, which is what I run it on this plane.
The result was, the oscillation where 90% gone in Cruise.. So I landed again and reduced P =25, D = 2.
This smoothed out Cruise mode and WP linear climbs. But didn't remove oscillations when max nav_rth_alt_mode = EXTRA was called.
So I increase nav_fw_control_smoothness = 5, which reduced it by maybe 50%... Although it shouldn't have to rely on a work-around.

D term is based on error rather than measurement. I'll switch this back to measurement and see if it changes anything.

Judging by how much effect D-term has on this controller. I'd say this is likely the case. Being that the tune didn't end up much different to the velocity altitude tune,

[rts18]

As a late comer to the party. My tests show the same. The required tune for the position z changes where practically the same as that of the velocity z tune. However it appeared to calm things a little.

I have not had the opportunity to test the changes made in commit 83d162c

[breadoven]

I realised I've been HITL testing this with control smoothness set to 7. So I tested again still using error for D term but with control smoothness set to 0 and it starts oscillating in pitch ... most obviously when changing altitude in Cruise. After switching to using measurement for D term instead and changing the PID D term factor back to the original value (30 to 100) the pitch oscillation is gone. Obviously this is HITL but I suspect using measurement for D term makes a difference.

So I've changed the PID D term factor back to the equivalent of the old method which together with the using measurement rather than error should ensure it now behaves as before.

Edit:
I've now changed velocity based control to also use measurement for D term with the latest commit. This is mainly as a test. If this resolves the problem for velocity it will render this PR unnecessary since it will have been the problem all along. Just needs someone to test it on a problem plane.

[Jetrell]

I've now changed velocity based control to also use measurement for D term with the latest commit. This is mainly as a test. If this resolves the problem for velocity it will render this PR unnecessary since it will have been the problem all along. Just needs someone to test it on a problem plane.

So that should mean we can switch nav_fw_alt_use_position = OFF when testing this firmware, and it will revert back to the Alt Velocity controller, with IT now using d-term (CD) to drive distance to the target by Measurement, in place of Rate ? Just as you have it with Alt Position in the earlier commits of this PR ?
As you said. If it works. Then the Position part of these commits won't be needed.

[breadoven]

I've now changed velocity based control to also use measurement for D term with the latest commit. This is mainly as a test. If this resolves the problem for velocity it will render this PR unnecessary since it will have been the problem all along. Just needs someone to test it on a problem plane.

So that should mean we can switch nav_fw_alt_use_position = OFF when testing this firmware, and it will revert back to the Alt Velocity controller, with IT now using d-term (CD) to drive distance to the target by Measurement, in place of Rate ? Just as you have it with Alt Position in the earlier commits of this PR ? As you said. If it works. Then the Position part of these commits won't be needed.

With nav_fw_alt_use_position = OFF the D term will be based on velocity measurement (Vel Z) rather than error (Target Vel Z - Vel Z). I tried this when testing before and it didn't seem to make much difference in my case so didn't think it was significant but it must become significant when control stability becomes more marginal. I can see that using measurement is likely to provide a more consistent measure of rate of change (D term) than using error when trying to maintain the target altitude where the error becomes more sensitive and erratic.

[Jetrell]

It now looks more like the way it used to work with the old nav_fw_pos_z controller.
It obviously doesn't have the response the newer nav_fw_pos(Vel)_z controller has. But it is more tame on planes that are problematic to tune with the velocity controller.

With the old stock tune of P = 30 -35, I = 5, D = 10... It will hold altitude in Cruise, with a 1 meter drift at most.
While in WP or RTH climbs/descents it would drift within a few meters of the target altitude. And it was sometimes even tighter when there is little wind.

I tried increasing the P up to 40 and D up to 50, as the old position controller could handle. It only caused small pitch oscillations. But it also didn't improve its ability to acquire and hold the altitude target any better either.
While I was at it. I also adjusted nav_fw_alt_control_response... It may have climbed a bit harder with it set higher, with no overshoot. But It wasn't significantly noticeable.

So in conclusion. This addition to the fw altitude controller does the job, and is more care-free in it setup. And is plenty good enough for general use.
While those who's have planes with good natural stability. And they require better altitude target response. The newer (8.0- 8.1) altitude velocity controller is good for that.

I'll personally use nav_fw_alt_use_position = ON with two of my planes. And the rest will be fine with the 8.0 velocity altitude version.

[breadoven]

Did you test this PR with nav_fw_alt_use_position = OFF @Jetrell ?

[Jetrell]

Did you test this PR with nav_fw_alt_use_position = OFF

It might have been a good thing to check this. I was out today using the last commit. Which is the same as last week.
I ran both the Vel z and Pos z controllers on two different profiles, using each ones required tune.
Firstly I notice the velocity seemed to work fine in RTH climb-out and holding altitude in Cruise mode.
But when it came to WP altitude changes, it was either considerably over the target altitude or under it... And it didn't look like it wanted to try much either.
I can't remember it being like this, even with this plane, with the Vel z before these commits... BUT, I've been tweaking so many settings under different conditions to nut out as many possibilities as I could, that I may have missed something.

I thought it could have been a problem with profile switching between them. But when I just used the Vel z, the issue was the same with WP altitude.. But when using nav_fw_alt_use_position = ON, it would reach the WP altitude target within a meter or so,

I've been looking over the DVR and logs for the last hour. And I can't think of any other reason this would be. So I thought I'd mention it to you, in case it there is an issue.

[breadoven]

So it appears that using measurement for D term for velocity based altitude control fixes the instability problem ? Interesting that it makes such a difference.

I assume the WP issue is related to the actual altitude lagging the desired altitude when flying between waypoints with different altitude settings ? If you fly between waypoints with the same altitude setting does it hold altitude OK ? I think the lagging issue is likely caused by nav_fw_pos_z_ff being too small. The default of 10 was just a starting point really but having checked some logs recently I think it needs to be higher. If you look at the logs you'll probably find fwAltI ends up pretty large when adjusting altitude. If FF was working correctly the I term should be much smaller with FF doing most of the work driving the desired velocity. I need to work out a better default and add it to this PR ... 35 looks like it might be around the figure required.

I'm thinking the best thing would be to go with this PR as it is other than a change to increase the default FF setting. If it turns out that the instability issue using velocity for altitude control is fixed using measurement for the D term then the position option can simply be removed in a later release if it is considered redundant.

As a side note the only nav related control left using error for D term is roll control for heading adjustment. Makes you wonder if that might work better using measurement given I noticed you get oscillations with WP course tracking and control smoothness set to 0 (HITL testing). Might be worth a quick test.

[Jetrell]

Just a final word on this, after being out flying today.
The last two commits have really made the biggest difference to both the Vel and Pos z controllers.
It even appears to handle somewhat badly tuned stabilization Rate and FF again, now that it's using measurement for the nav_fw_pos_z_ d. And with nav_fw_pos_z_ff increased.

Using the Vel_z, I'd also found I can increase nav_fw_pitch2thr_smoothing = 8 with these planes, to smooth any pitch2thr response within its deadband,. This allowed me to increase the nav_fw_pos_p /d more (but not quite to default). But still using the default nav_fw_alt_control_response..
Now I no longer see any elevator wagging when looking back with the camera.

@breadoven Thanks for your perseverance with this. I was rather impressed with how responsive it can now be tune without side effects, on these higher level aerobatic planes.

[breadoven]

It does make you wonder why you'd want to use error for the D term when it just seems to cause problems ... or in this case makes the PID loop fundamentally unstable. Still need to test the roll control using measurement rather than error, see if it makes any difference.

I thought the following was interesting:
https://controlguru.com/pid-control-and-derivative-on-measurement/

[rts18]

@breadoven Sorry it took so long to conduct the tests.
We also concluded that the instantaneous measurement of the process variable provides smoother and more consistent altitude control, when comparing to the PV rate of change.
This would seem logical due to the range of unknown factors which can influence altitude control at any point in time. Which makes derivative prediction based on rate of change of the error, less than reliable.

We found the velocity controller was more responsive than the position controller. It had no quames with aggressively maxing out nav_fw_auto_climb_rate , nav_fw_max_thr and nav_fw_climb_angle to reach the setpoint.
These are the settings we settled on. I also tried increasing nav_fw_pitch2thr_smoothing as Jetrell suggested. This made a difference to fixedwings with large elevator throws.
nav_fw_pos_z_p = 23
nav_fw_pos_z_i = 6
nav_fw_pos_z_d = 2
nav_fw_alt_control_response = 45

The use of the altitude position controller nav_fw_alt_use_position = ON. Was less aggressive in its climb rate. It showed a noticeably smoother rate of climb than the velocity controller. With the setpoint not always being made, or slightly overshooting in dives depending on the wind when comparing the two.
However it was more power efficient in climbs, which may make it more desirable for some.
We also found it had a narrow tuning window. Increasing the P or D gains didn't increase responsiveness. But instead started to induce mild oscillations. This is what we settled on for its use.
nav_fw_pos_z_p = 30
nav_fw_pos_z_i = 5
nav_fw_pos_z_d = 11
nav_fw_alt_control_response = 35

When tuning either altitude controller. There is a balance required, between nav_fw_alt_control_response and nav_fw_pos_z_d. Too little control response will make it sluggish even with higher D gain.
But to much control response will begin to effect the D gain. Making the position controller oscillate, and the velocity controller far too aggressive.