Freeze yaw I-term above bank angle threshold

[avsaase]

This PR freezes the yaw I-term when the bank angle of the airplane is more than fw_yaw_iterm_limit_bank_angle degrees. This addresses the problem of the rudder counteracting turns by temporarily (partially) disabling the yaw stabilization when the airplane is banked. See #5800 and #5865. The problem is not fully solved because the P-term is still able to fight the turn, but with the INAV convention of setting low P-gains on fixed wing this should not be noticeable. I could also either freeze or zero out the P-term but I think that is not necessary.

The default of fw_yaw_iterm_limit_bank_angle = 180 effectively disables this feature.

With this PR there is a hard cut-off point after which the I-term is frozen and I am not sure how (if at all) noticeable this would be in flight. I could also make it so the I-gain update is gradually decreased from rateError * pidState->kI * dT to zero. Would that make sense?

A banked turn also induces a pitch rotation so I considered also freezing the pitch I-gain, but with bank-and-yank flying the pilot does pull on the elevator stick so I think this rotation is not seen as a rate error by the flight controller. I also thought it safer not to touch the pitch stabilization.

I have tested this on the bench and it works as expected. However, I would still appreciate it if someone would carefully review my code to catch any rookie C mistakes. I will try to fly this as soon as possible but the weather forecasts look pretty bad so it might take a while. Any feedback is more than welcome!

[avsaase]

@DzikuVx, I changed the code as you suggested. A quick bench test confirms that this is different from what I had before. Using applyItermLimiting when the bank angle is above the threshold constrains the I-term between the positive and negative of the I-term from the moment the threshold was crossed. This means that the extent to which stabilization is turned of in turns depends on the size of the I-term before the turn.

In my mind the rudder I-gain mostly serves as a automatic trim. Say rudder is not perfectly trimmed so the I-term is large and negative to obtain a left rudder deflection (idk if this is how it is defined in the code, this is just for the example). Banking to the left to make a left turn then leaves a lot of room for the I-term to go to the positive side and still counteract the turn. I think it is better to just freeze the I-term in place. I'll try to come up with a cleaner way to achieve that.

[avsaase]

Okay, I have now implemented the freezing of the yaw I-term similar to how the I-term is limited. It seems to work well on the bench. I will try to fly this a soon as possible and to find some other testers as well.

My only concern at this point is that this feature depends on a correctly measured bank angle, so horizon drift would interfere with it. Perhaps the logic could be improved by also conditioning on the (filtered) yaw rotation, but I think tuning that would be more difficult. WDYT?

[DzikuVx]

OK, I will have to test it on the bench

[avsaase]

I found it easiest to observe the effect with these settings:

set fw_iterm_throw_limit = 0
set fw_i_yaw = 30
set fw_yaw_iterm_limit_bank_angle = 10

[avsaase]

I just had the opportunity to test this in flight and it worked exactly as expected. Looking at the attached log file, the yaw I-term is frozen when I was making bank-and-yank turns (see for example around 2:26), which kept the rudder nice and centered. I am not 100% sure, but I think that the hard cutoff point of 10 degrees bank angle was sometimes noticeable when making less aggressive turns around this bank angle.

Overall I am already quite happy with how this worked out, but I can think of a couple potential improvements:

1. Linearly reduce the I-term updates around fw_yaw_iterm_limit_bank_angle to avoid a noticeable cutoff point.
2. Somehow take into account the commanded yaw input, for example by scaling down the I-term updates with the yaw rate error. This would have the added benefit of also working when making very flat turns as Marc Hofman asked about on FB.

LOG00014.zip

[MrD-RC]

Awesome feature. Has this been set up as an in flight adjustment? I think it's something that could be useful to tune in flight.

[avsaase]

I have not felt the need to really tune this value, i just set it to 10 degrees. During very flat turns the I-term can still wind up but the yaw rate during such turns is so low I don't think you would really notice it.