Fixed wing Nav altitude velocity control

[breadoven]

PR changes fixed wing Nav altitude control from position to velocity based. It also refactors the Nav altitude climb rate code to remove rolling altitude position targets to set a climb velocity and instead allows the climb velocity to be set and used directly by both multirotor and fixed wing.

Multirotor still uses the sqrtController for climb velocity. Fixed wing uses a linear reduction in desired climb velocity as target altitude is approached based simply on a function of 5 x max climb rate limit, i.e. climb velocity is constant at max climb rate setting until within 5 x max climb rate distance from target altitude then reduces down to 0 when the target altitude is reached. The 5 factor could be made a setting if this required tuning for different craft. If no target altitude is set/required, such as during RC adjustment, the desired climb rate is used directly by the velocity controller as a constant input (altitude position is irrelevant).

Max altitude limitation has also been improved so it targets the altitude limit directly rather than just limiting climb rate. Emergency landing also targets a max descent rate of 1 m/s when close to takeoff altitude to try and achieve a "soft" landing without the need to reduce the max landing descent rate throughout the descent (an issue for multirotors low on battery perhaps).

Altitude PIDS for fixed wing have been adjusted so the existing settings should work as they are. They're also still using PID_POS_Z for the time being. If this change seems to work as intended they PIDS can be updated to the more correct PID_VEL_Z.

This seems to work OK based on HITL testing for fixed wing including testing with the Baro disabled although it's not clear how much affect this would have with HITL given X Plane doesn't model GPS inaccuracies.

Multirotor has only been ground static tested although this showed the desired Z velocities and positions behaved as expected during PosHold. Also, it's still using the sqrt controller so nothing should have changed regarding control behaviour.

[breadoven]

Actually the multirotor changes need rechecking, not sure they might not cause problems. Fixed wing should be fine though.

[Jetrell]

I was doing some Caddx beta fw testing with a quad today. And I thought I'd load your last commit.
It wasn't a comprehensive test.. I only tested Poshold and Cruise mode altitude changes. Plus emergency landing. They seems to work as you stated.. With no undesirable effects noticed.

[breadoven]

I was doing some Caddx beta fw testing with a quad today. And I thought I'd load your last commit. It wasn't a comprehensive test.. I only tested Poshold and Cruise mode altitude changes. Plus emergency landing. They seems to work as you stated.. With no undesirable effects noticed.

Was that quad or fw testing ? I think quads should behave as before after the last commit.

The velocity control method in this PR makes more sense than the old method when it comes to constant vertical velocity demand such as during RC adjustment where there's no altitude target. The old method converted a desired climb rate into a rolling vertical position target that then got converted back in a climb rate by the MR altitude velocity controller. This PR just passes the desired climb rate directly to the altitude velocity controller without using position at all as it's not required. I don't think the old method caused such a problem for MR so much, just seemed a bit pointless, but for fixed wing the rolling altitude position target method always seemed to upset the PID controller.

When you do have an altitude target it seems to be much easier for fixed wing to work with velocity targets than vertical position targets because velocity is always constrained to an achievable value whereas altitude position isn't, You can have large unachievable altitude position targets which cause problems for the PID control. Having said that WP altitude control still uses the rolling altitude position target method to give a linear vertical flight path between WPs. It should be possible to calculate the required climb rate for a linear flight path between WPs instead and use that to control altitude changes between WPs.

Fixed wing does need checking though when flown without a baro. Seemed to be largely unaffected using HITL but I'm not sure how HITL simulates disabling the baro. Given the baro is disabled in INAV I'd have thought position estimator won't use it so it'll just be relying on the simulated GPS input from X Plane to INAV. However, this apparently doesn't simulate GPS errors so wouldn't be entirely realistic. There was no baro altitude in the BB logs so it didn't appear to be used. I did notice the latest version of HITL has some additional GPS degradation settings available which I need to try.

[Jetrell]

It was only a Quad I was testing with today.. Over the next few days I'm doing some fixedwing testing.. And I'll certainly add this to my list.

I don't think the old method caused such a problem for MR so much, just seemed a bit pointless, but for fixed wing the rolling altitude position target method always seemed to upset the PID controller.

I don't think it caused a problem.. But today, RC commanded changes and even altitude slowdowns seem much quicker and more precise than in the past.. The altitude holding precise in Cruise mode and Poshold was considerable more locked in. With only a shift of 0.2 to 0.8 of a meter on the Vario. Which is not typical in my experience.. But nice to see. This was even when reasonably quick course changes where being made.

The old method converted a desired climb rate into a rolling vertical position target that then got converted back in a climb rate

That's a poor way of going about it.. Great find and alteration.. I'll be interested to see how it works on a fixedwing, with Cruise mode altitude adjustments.

Having said that WP altitude control still uses the rolling altitude position target method to give a linear vertical flight path between WPs. It should be possible to calculate the required climb rate for a linear flight path between WPs instead and use that to control altitude changes between WPs.

This has been why I generally set nav_wp_enforce_altitude = 0 on my fixedwings. Because thermals and even a strong wind blasts can effect hitting the WP altitude target.. And being that the current method doesn't exactly work in its favour... Any wonder why LOL.

[breadoven]

Flight tested fixed wing and multirotor including WP missions and RTH and all worked pretty much as expected. Fixed wing had baro enabled so still needs testing with it disabled. There is a minor issue with WP missions related to the rolling altitude position target method used for a linear climb between WPs which results in the desired climb velocity initially starting small, only increasing to the max limit as the position target moves away from actual. Not sure it didn't do this when using position rather than velocity, probably just more obvious with velocity. but could do with improving all the same. Also, with fixed wing climb rate is limited by nav_auto_climb_rate and also by nav_fw_climb_angle which confuses things. Probably climb angle is more important so you'd be better off setting a large value for climb rate and allow the climb angle to limit ... or find a better way of setting climb limits.

[JulioCesarMatias]

The variables:

• demSPE
• demSKE
• estSPE
• estSKE
• speedWeight
• demSEB
• estSEB
• pitchGainInv

They are part of a small piece of a system called Total Energy Control System, the author who added this to INAV probably couldn't write the rest (90%) of the algorithm for some specific reason, but I don't think it should be removed …Now with SITL operating at INAV, things are a little easier to develop. With these variables and adding the rest of the code that is still needed for this to operate correctly, it is possible to obtain things like:

• Combined control of speed and height using throttle to control total energy and pitch angle to control exchange of energy between potential and kinetic.
• Selectable speed or height priority modes when calculating pitch angle
• Fallback mode when no airspeed measurement is available that sets throttle based on height rate demand and switches pitch angle control to height priority
• Underspeed protection that demands maximum throttle and switches pitch angle control to speed priority mode
• Relative ease of tuning through use of intuitive time constant, integrator and damping gains and the use of easy to measure aircraft performance data

Subtitle:
dem = demand
est = estimated
SPE = Specific Potential Energy
SKE = Specific Kinetic Energy
SEB = Specific Energy Balance

Using the Square Root Controller on fixed-wing aircraft makes no difference, it will just be one more algorithm taking up more machine cycle time... The Sqrt Controller is only useful for small approximations of angle, position, speed and acceleration (which is the case of Att-Hold and Por-Hold using the Multirotor profile).

[breadoven]

The current energy balance code remains in the revision history and can be reinstated if and when someone decides to implement it fully, But I don't see the point of keeping as is given it serves no purpose. I guess it could be left in commented out with a TODO for posterity.

And this PR doesn't use the Square Root Controller for fixed wing, it's still only used for multirotor. Fixed wing just uses a simple linear reduction in climb rate as target altitude is reached, much the same as multirotor before the Square Root Controller was used.

[JulioCesarMatias]

The current energy balance code remains in the revision history and can be reinstated if and when someone decides to implement it fully, But I don't see the point of keeping as is given it serves no purpose. I guess it could be left in commented out with a TODO for posterity.

I vote to keep the Specific Energies for future development (continued)... Even as a TODO as you said.

And this PR doesn't use the Square Root Controller for fixed wing, it's still only used for multirotor. Fixed wing just uses a simple linear reduction in climb rate as target altitude is reached, much the same as multirotor before the Square Root Controller was used.

Regarding the Sqrt Controller, I apologize, I certainly didn't review the algorithm carefully, and I thought it was being used in fixed-wing aircraft.

Thanks!

[MrD-RC]

IMHO TECS is a PITA in ArduPilot and takes ages to get it set up and working well. The way INAV currently works is much simpler and doesn’t need adjusting unless the pilot wants to perfect the flight characteristics. Simple pitch to throttle and zero throttle down pitch work fine for most cases.

[Jetrell]

There is a minor issue with WP missions related to the rolling altitude position target method used for a linear climb between WPs which results in the desired climb velocity initially starting small, only increasing to the max limit as the position target moves away from actual.

I noticed that too. I'm rather certain I've seen it before this. But it seems a little more pronounced now.

I first spent some time checking the tune of this velocity_z controller (old pos_z) in the inflight tuning. By increasing p-term until the altitude hunts, then reducing it until it held. And then tuning the d-term to suit.

After testing this new controller with two fixedwing airframe's, in different modes. Then looking back over the log and DVR.. It would appear to have improved the average altitude targeting by up to 50%, over what it was before. Which I consider to be a good improvement.
But its worth noting for people that may read this in the future. That a fixedwing still can't get close to a multicopter in altitude precision. But an improvement is never the less an improvement. Nice work !

I forgot to turn off the baro, and test only GPS altitude.

[breadoven]

There is a minor issue with WP missions related to the rolling altitude position target method used for a linear climb between WPs which results in the desired climb velocity initially starting small, only increasing to the max limit as the position target moves away from actual.

I noticed that too. I'm rather certain I've seen it before this. But it seems a little more pronounced now.

I first spent some time checking the tune of this velocity_z controller (old pos_z) in the inflight tuning. By increasing p-term until the altitude hunts, then reducing it until it held. And then tuning the d-term to suit.

After testing this new controller with two fixedwing airframe's, in different modes. Then looking back over the log and DVR.. It would appear to have improved the average altitude targeting by up to 50%, over what it was before. Which I consider to be a good improvement. But its worth noting for people that may read this in the future. That a fixedwing still can't get close to a multicopter in altitude precision. But an improvement is never the less an improvement. Nice work !

I forgot to turn off the baro, and test only GPS altitude.

I used to have the Dterm set to 70 for pos_z which seemed the only way of improving large altitude fluctuations with PosHold on fixed wing. Even then it still varied by up to 10m sometimes, worse with wind. A Dterm of 70 with this PR causes oscillations but using the default of 10 works fine. Testing the other day showed variations in altitude with PosHold of around 3-4m although it wasn't that windy. However, I think this is better than the current method although hard to be sure because I think other recent improvements have helped accuracy in general.

I think the issue with having no baro simply comes down to whether or not it renders altitude control unsafe on a fixed wing rather than it just being less precise. So long as it still works reasonably well all's good. And if you want better altitude control then use a baro.

[shota3527]

@breadoven
climb_rate[m/s] = sin(climb_angle[rad]) * current_speed[m/s]), and when angle is less than 30deg, sin(angle[rad]) ~= angle[rad], which means climb_angle = 1/current_speed * target_climb_rate = FFgain * target_climb_rate
cilmb_rate to angle controller should be driven mainly by FF gain, PID is for correction, similar to FW angular rate controller.
Set the default FF gain based on the above equation

Adding nav altitude velocity control seems like to introduce some new parameters. or set the ff gain based on reference airspeed

[breadoven]

@breadoven climb_rate[m/s] = sin(climb_angle[rad]) * current_speed[m/s]), and when angle is less than 30deg, sin(angle[rad]) ~= angle[rad], which means climb_angle = 1/current_speed * target_climb_rate = FFgain * target_climb_rate cilmb_rate to angle controller should be driven mainly by FF gain, PID is for correction, similar to FW angular rate controller. Set the default FF gain based on the above equation

Adding nav altitude velocity control seems like to introduce some new parameters. or set the ff gain based on reference airspeed

Yes this doesn't use FF at the moment but multirotor velocity Z doesn't use FF either. I did try FF with this PR ... not sure it made much difference although I didn't test it much.

The PIDS for this need finalising, current POS should be VEL. There's also the "5" factor used by fixed wing which really seems to be the equivalent of multirotor PID_POS_Z although that doesn't seem to be used in any PID loop as such, it's just used as a basic factor based on the P value which makes you wonder how much use it is, could just be a basic setting.

Last commit updated the WP linear climb to use climb rate rather than rolling position Z. The max altitude limiter was also changed so it doesn't work during RTH or WP mode if altitude enforce is active. It causes climbs to get stuck at the max limit if the relevant altitudes are not set correctly. Needs a proper fix but this will have to do for now.

[breadoven]

Did you ever get to check this with Baro disabled @Jetrell ?

[Jetrell]

With your last commit. I tested it today with inav_use_gps_no_baro = ON.. GPS altitude only.
It appeared to work fine.
Wind was between 10 to 30km/h in the WP missions I ran... With nav_wp_enforce_altitude = 5.
I wasn't sure if you had changed the code to work with enforce altitude ?
But in past WP missions. I would find 5m was very rarely enough to complete a mission, without it missing the target altitude at a Waypoint or two.
But after tuning, it now seems to be adequate with either Baro or GPS altitude sensing.

The only issue I found when using GPS altitude. Was some confusion caused at the first WP marker.. It would hook outwards of WP1, then swing back around and settle into the mission fine.
I tried starting it several times, from different heights and angles.. But the outcome was the same.
Although this effect didn't occur when the Baro was enabled.

[breadoven]

I assume when you tested with inav_use_gps_no_baro = ON that the baro was also actually disabled ? inav_use_gps_no_baro is only for multirotor really because fixed wing will always use GPS if there's no baro regardless of this setting.

Nothing was changed specifically for nav_wp_enforce_altitude . The improvement in better achieving WP altitude is probably due to the last commit which ensures you get the max allowed climb rate as soon as you switch to the next WP. This didn't happen before because of the rolling position Z target method which took time to build up to the max climb rate. The new method used for this PR should be a big improvement where that's concerned. If you check the logs you should notice that the climb rate hits the max setting almost immediately you start a new WP if max is required to reach the WP altitude.

I did try and simulate GPS altitude error on HITL by adding some random error on top of the GPS output coming from X Plane, quite large errors at times, and it just seems to affect the actual altitude relative to the ground, as you'd expect, but doesn't seem to affect the vertical velocity control even when the GPS altitude error changes quite abruptly. There appears to be sufficient damping in the vertical position/velocity estimation to smooth out the errors. I think in reality GPS altitude tends to drift in a relatively consistent way, you don't really get abrupt changes unless you have a low satellite count or there's some sort of interference affecting the signal.

Does sound like this seems to work OK even without a baro so probably just need to sort out the PID settings to complete it.

Not sure what the issue is with the first WP ... nothing in this PR should affect that and I can't see how the baro would affect things either. I'll see if I notice it when testing again.

[Jetrell]

Not sure what the issue is with the first WP ... nothing in this PR should affect that and I can't see how the baro would affect things either. I'll see if I notice it when testing again.

I went back and watched the OSD when this was occurring.. And I missed the system message at the time. It only appeared for a split second.
When it was hooking outwards around the 1st WP, it was effectively a quarter loiter circuit, as it was making a small altitude adjustment.
It was evident in different amounts on the 4 GPS altitude tests, but not in the Baro tests... But It didn't cause any altitude problems for the rest of the mission. So it looks like the last improvements you made are certainly working well.

As a side note... I don't want to load you up with too much. But I believe these will all help people tuning in the future.
When your making the configurator changes for the VEL_Z PID's. What do you say to allowing nav_fw_pos_xy_i and nav_fw_pos_xy_d to also be displayed.

And when you're updating the velocity PID's under the Adjustments tab, for inflight tuning.. Would you mind adding the nav_fw_pos_hdg PID Adjustments to it as well ? Thanks

[breadoven]

I'm having second thoughts about changing POS to VEL partly because it affects a load of stuff, Blackbox, Adjustments etc, but also because as far as the end user is concerned POS would seem to be the more logical term to use for something that ultimately controls altitude position changes. How that is achieved, whether using position or velocity, seems irrelevant to the end user so long as it ultimately controls altitude as expected. Keeping POS also has the advantage of maintaining setting continuity across releases.

There is also the issue that fixed wing doesn't define POS_Z and VEL_Z for the PID controllers used to control altitude, as multirotor does, it just defines a single controller fw_alt. In fact POS_Z for multirotor appears to just be used to adjust altitude control response (possibly ? in fact it's not really clear what this does), it isn't used by a PID controller as such, so it's effectively just a setting more than anything else. The same could be achieved for fixed wing with a single setting to adjust the altitude control response rather than setting up another PID controller that won't be used other than the P term.

So it seems better to just keep the current fixed wing POS PID settings and add a new setting, e.g. nav_fw_alt_control_response, which adjusts the altitude control response as the target altitude is reached.

[DzikuVx]

I took a look at it and it would be safer if we move it to INAV 8 as it might change tune from INAV 7 and as long as possible I'd prefer to keep 7 and 7.1 fully compatible without a need to any retune

[breadoven]

I took a look at it and it would be safer if we move it to INAV 8 as it might change tune from INAV 7 and as long as possible I'd prefer to keep 7 and 7.1 fully compatible without a need to any retune

OK. May as well make a small tweak in 7.1 in that case to improve the multirotor emergency landing issue in #9566. It should be fixed by this PR but it's a simple change to fix the current code to make the behaviour in #9566 less likely (just increase the deceleration distance allowed to go from max descent rate to 1 m/s as you approach takeoff altitude). Although I'm still not convinced there's isn't something wrong with the altitude estimation in some cases which is the real cause of this problem.

[breadoven]

@DzikuVx any idea why this is failing on SITL windows build ... seems to also affect other PRs.

[stronnag]

@DzikuVx any idea why this is failing on SITL windows build ... seems to also affect other PRs.

Possibly a github issue? Master windows SITL builds without a problem in a Win10 VM here.

[stronnag]

Scrub that, updated cygwin locally and the build fails in the same way.
I'll have a look at it.

[breadoven]

I've tested the latest changes to this and all works as expected. Fixed wing was tested back to back for a WP mission with the baro disabled in flight for one run. There was no obvious change in behaviour between the runs so it would appear to work fine with GPS alone although this was with >10 satellites locked, it would likely become less accurate with fewer satellites available but not to the point bad things would happen. Obviously use a baro if you want best altitude accuracy.

Also tested multirotor emergency landing which worked fine. There was none of this yoyo altitude behaviour reported by one pilot, just slows down rapidly to the set distance above takeoff altitude and then continues down at 1m/s.

So this should be good to merge at this point.

[Jetrell]

@breadoven Can I ask you take a look at the way this PR interacts with the current Master build of the configurator.. It appears to be preventing the Tuning tab from loading.
While builds before this PR was added, work with the configurator master.

Although it maybe a false alarm.. I'm seeing so many conflictions. Which appear to be base on configurator timing and the payload size. And that changing from one FC to the next.

[breadoven]

@Jetrell Works OK for me. There are still problems with Configurator ... it's much better than it was but still not entirely reliable, e.g. Flashing an F722, which shouldn't have speed issues, results in no GPS in the sensor icons and (sometimes ?) the Runtime Calibration indicator remains red even though it's fine. Disconnecting doesn't fix it, restarting Configurator does. Probably something isn't being reset after flashing ?

[Jetrell]

@breadoven I've had to de-tune this controller on all my planes, now that the temperatures are colder. Comparing to the testing I did in 7.0, when it was summer here.. Otherwise they all experience significant altitude oscillations.

However, I am aware of other altitude control changes that also occurred in 7.1. But I'm not sure they're related. Being that I did mention issues surrounding this above, when testing 9 months ago.

BUT they all occasionally exhibit this slight altitude oscillations, out of nowhere.. I thought it may have been tuned too tightly.. But this still happens when the tune is reasonably reduced.. So I've run out of testing idea's.

And its also very dependent on the amount of stabilization Feedforward applied to the pitch axis.

I helped a guy autotune his planes, using 8.0. And the pitch FF increased as required.. But both aircraft went from holding a set altitude target in Cruise, WP and Loiter modes, before the tune. To oscillating on the pitch axis after the autotune.
They both required the nav_fw_(pos)vel_p gain to be reduced from 40 to around 30... Just as I experienced the day before with my test aircraft.. Which could be the Vel controller, or the inconsistency of autotune settings on some airframes.
This wasn't noticed when going back to 7.0, using the altitude position controller.. Which was not influenced as much by apparent air density or temperature as the velocity controller is.

I've held off raising the matter for a while. But after seeing this with a number of my own test models, and other peoples. Plus an 8.0 test report on Discord. I thought I better mention it now for awareness, so a possible cause might be found, before 8.0 is released later this year.

[breadoven]

@Jetrell Are these problems similar to #10213 at all ? These planes are using 7.1.1.

I'm not convinced air density changes would affect things that much between 30 degs and 0 degs and in fact density could be the same at different temperatures depending on air pressure. Temperature could affect the sensors on the FC I guess.

I've used the following settings without any issues:

nav_fw_pos_z_p = 30
nav_fw_pos_z_i = 15

Other settings are defaults. Max vertical velocities will also make a difference, nav_fw_auto_climb_rate. Higher settings will increase pitch adjustment sensitivity potentially. Mine are set at defaults or lower.

I don't see it as too much of an issue if retuning settings solves the problem going from 7 to 8 versions. The PID correction factors could be fine tuned if need be but perhaps more feedback is needed before doing that (they were changed from 10 to 100 but the 100 was just a rough adjustment based on HITL testing).

The easiest way of understanding what's happening is to check the log when the oscillations occur obviously.

[Jetrell]

Are these problems similar to #10213 at all ? These planes are using 7.1.1.

I would say similar, but not as wild. Some of that could be incorrect tuning, if they are only using the default settings.

The easiest way of understanding what's happening is to check the log when the oscillations occur obviously.

Logs aren't always easy to gauge the possible cause when oscillations randomly occur in level flight. Because its hard to say if the oscillation is a sensory error, or external influences effecting the control mechanism..
But one time using a DVR file, I referenced it with a plane flying next to me in Cruise mode.. Baro was relatively constant at that time, but it seemed to show the navPos[2] and navVel[2] being incorrect, with navEPV being low and not changing at that moment. So it leaves me wondering if its related to this returning acc drift that is being reported by others. Being that this often occurs after a course or heading change.

Other settings are defaults. Max vertical velocities will also make a difference, nav_fw_auto_climb_rate. Higher settings will increase pitch adjustment sensitivity potentially.

I have my nav_fw_auto_climb_rate set to 800. Which is tuned together nav_fw_pitch2thr and other settings related to it, to reach that maximum vertical rate if called for.
I'd also reduced nav_fw_alt_control_response to smooth the slowdown target response when higher auto climb rates are set.

I've used the following settings without any issues:

I agree with your setting, nav_fw_pos_z_p default has been 40 for a good while using the old controller. But its now too high, especially for elevon controlled aircraft.