NAV PID loop I term wind up fix

[breadoven]

Closes #9026.

Resets I term in Nav PID controller when P term saturated which avoids I term wind up upsetting Nav PID loop when Nav target conditions change.

Tested on HITL sim OK for fixed wing. Needs testing for multirotor.

[Jetrell]

I've actually tested using vertical velocity for fixed wing to control altitude change rather than using Z position .
Seems the reason velocity wasn't used for fixed wing was because a fixed wing might not use a Barometer.

Now that the AHRS is improved and the GPS update rate has been increased, with more constellations available for higher precision.
I think adding both velocity controllers to fixedwing navigation tuning, could be beneficial.. nav_fw_vel_xy and nav_fw_vel_z. And also having them in the Adjustment tab for tuning.

I am aware, that for both of these controllers to perform at their best, they will likely be influenced or limited by functions like these.

nav_auto_climb_rate
nav_fw_climb_angle
nav_fw_dive_angle
nav_fw_bank_angle
nav_fw_pitch2thr
nav_fw_max_thr
nav_fw_min_thr

I raise this idea after reading your reply.. But also after expereincing very little benefit of -
nav_fw_wp_tracking_accuracy
nav_fw_wp_tracking_max_angle
nav_fw_wp_turn_smoothing
When used with many Elevon controlled wings, in WP missions.. Even in low wind conditions.
I'd wondered if its was the navigation controllers in-ability to rapidly react to yaw axis target drift, when the wing is traveling at a reasonable navigation speed. Or a combination of that, and the way some wings may induce more adverse yaw.

[breadoven]

I raise this idea after reading your reply.. But also after expereincing very little benefit of - nav_fw_wp_tracking_accuracy nav_fw_wp_tracking_max_angle nav_fw_wp_turn_smoothing When used with many Elevon controlled wings, in WP missions.. Even in low wind conditions. I'd wondered if its was the navigation controllers in-ability to rapidly react to yaw axis target drift, when the wing is traveling at a reasonable navigation speed. Or a combination of that, and the way some wings may induce more adverse yaw.

I've noticed that the WP tracking accuracy doesn't work well on faster wings which often seem to be flying wings. It always struggled for flying wings anyway given it's controlling heading which flying wings can't do as accurately as a conventional plane for obvious reasons. Having said that it does work better than the previous method on slower conventional craft that are set up correctly so is useful if you need better accuracy. I simply wouldn't use it if accuracy isn't important. Having said that it could be improved probably by adding PID control ... just a question of whether it's worth it.

[Jetrell]

I've noticed that the WP tracking accuracy doesn't work well on faster wings which often seem to be flying wings.

Exactly my findings.

better than the previous method on slower conventional craft

True. Conventional aircraft with a vertical tail seem to benefit more from it..

Having said that it could be improved probably by adding PID control ... just a question of whether it's worth it.

It seems beneficial in theory, and from what you wrote in your post in #9026. Which I assume you wrote for a test some months back ?
The only way to know for sure. Is if it was implemented and tested under different conditions, with different air frame types... Its your call.

[breadoven]

Does this change make sense @DzikuVx ? Seems to make sense on the face of it and it fixes the original problem related to WP missions but I noticed the I term for multirotor velocity PID gets reset to 0 all the time when it probably shouldn't. Might need rethinking.

[DzikuVx]

@breadoven I think it makes sense. The I term on MR indeed should be analyzed. It looks like suboptimal approach indeed

[shota3527]

I think we should try limit/freeze integrator first, Since it is a go-to solution for anti-windup in control engineering.

[breadoven]

As far as Nav is concerned the problem seems to be more to do with the fact the control state, mainly PID related especially I term, isn't reset properly when target conditions change. It gets reset changing between modes but not when target conditions change within a mode, e.g. moving to the next WP which has completely different position and altitude settings. Need to find a clean way of resetting the control state when required. Perhaps one way is to simply check if the new desired position has changed significantly from the last desired position which can be done in setDesiredPosition easily enough.

[DzikuVx]

@breadoven in such case I set it to Future release. Feel free to set it back in the planned release when you will be confident about the solution again

[shota3527]

@DzikuVx
plase have a look at #9224
There is a fact that accumulated I term is Opposite and excessive.
Opposite I term is accumulated by back calculation anti wind-up mechanism. It was just hiding under the excessive p term. Until excessive p term is gone.
#9224 is a more direct and simple fix to it.
A saturation in pid shouldn't reset the i trem. What if a spike in the setpoint/current value?

[breadoven]

I think #9224 is a better solution than this PR as it stands and it appears to fix the original problem although you'd need to check it doesn't affect other things given that the PID controller is used for other stuff such as multirotor velocity control.

However, I still think that for WP mode in particular it's sensible to reset the PID loop between WP's simply because the target setpoints between WPs will typically have changed so significantly as to render the current PID status irrelevant. Certainly any other factors such as wind affects will be insignificant compared to the large change in WP target setpoints.

[shota3527]

I think #9224 is quite safe since it only zeros the back calculation in a specific condition.
The i-term accumulation/anti-windup in a normal situation remains the same.
A similar equation has implemented in rate pid in VTOL test branch and no obvious issues were found

@Jetrell . In #9222, is the current method here refer to #9224 ?

I tested that same mission with the current method. And it performed the same as normal.

[shota3527]

I think #9224 is a better solution than this PR as it stands and it appears to fix the original problem although you'd need to check it doesn't affect other things given that the PID controller is used for other stuff such as multirotor velocity control.

However, I still think that for WP mode in particular it's sensible to reset the PID loop between WP's simply because the target setpoints between WPs will typically have changed so significantly as to render the current PID status irrelevant. Certainly any other factors such as wind affects will be insignificant compared to the large change in WP target setpoints.

The WP setpoint sometimes changes significantly and sometimes doesn't. So it is better to improve the robustness of the PID controller first. If that is impossible/difficult then consider a Reset.
About the significant change. take the angular rate pid for example. target rate changed from 0deg/s to +700deg/s then -700deg/s, It is better to reset the i-term? What if the motor trust between motors is very unbalanced? Reset I term must deteriorate the tracking accuracy.
About the wind. the wind is still same at the moment the new WP is loaded so same i-term should be used. Wind might change between WPs, but new i-term can be accumulated while traveling to new WP

[Jetrell]

In #9222, is the current method here refer to #9224 ?

I tested that same mission with the current method. And it performed the same as normal.

@shota3527 At the time, I only tested #9222, with bad results.
When I stated I compared it with the current method. I meant the conventional way the Navigation PID controller has always worked.
I haven't tested #9224, that shrinks the integrator.. But the principle would seem a more subtle way to do it.

[breadoven]

This isn't really much of an issue anyway, only causing problems if you set unrealistic altitude changes between waypoints that you should know can't be achieved.

Perhaps it's time to try using velocity control for for fixed wing altitude rather than distance. This problem would go away in that case. Can't imagine there are many planes that don't use a Baro if that is the only reason for not using velocity. GPS altitude will probably still work OK just not as accurate.

[Jetrell]

Perhaps it's time to try using velocity control for for fixed wing altitude rather than distance. This problem would go away in that case. Can't imagine there are many planes that don't use a Baro if that is the only reason for not using velocity. GPS altitude will probably still work OK just not as accurate.

You wont get any arguments from me.. Both fw_vel_z and fw_vel_xy I hope ?

This issue I see with nav_fw_pos_xy and nav_fw_pos_z alone, for a fixedwing navigation use... Is they lack the urgency required to respond to a shift in the aircraft's position, created by wind, thermals etc.
Its like the position controller just tells the control surfaces to move at an average rate to acquire the target... Instead of giving the control surfaces a heap of deflection when required, to acquire the target position faster and more precisely, according to the atmospheric conditions.

[shota3527]

This isn't really much of an issue anyway, only causing problems if you set unrealistic altitude changes between waypoints that you should know can't be achieved.

Perhaps it's time to try using velocity control for fixed wing altitude rather than distance. This problem would go away in that case. Can't imagine there are many planes that don't use a Baro if that is the only reason for not using velocity. GPS altitude will probably still work OK just not as accurate.

Is it a really good idea to add velocity control for for fixed wing on altitude?
For fixed wing altitude control, The final output is pitch angle and pitch angle is generally proportional to vertical velocity. The control output is proportional to the first derivative of the setpoint. So P Gain can handle it well. If using velocity control, we must introduce FF gain for vertical velocity to the pitch angle controller.
Multi-rotor has a different story. The final output is motor trust. and motor trust is generally proportional to vertical acceleration. In this case, In both the velocity to throttle controller and position to velocity controller, the control output is still proportional to the first derivative of the setpoint.

Unrealistic altitude changes between waypoints can easily happen. due to user error on unit conversion(M to CM) or miscount of 0s when setting the waypoint altitude.
For example, a waypoint altitude of 10000->110000->12000 will continue to pitch down after the 110000WP and might get altitude to 0, but it can be avoided if the issue is fixed.

[breadoven]

Closing since it's unlikely to be used.