Attenuate i-term based on pidsum limit

[tbolin]

Implements an additional check to prevent i-term windup if the pidsum for an axis is too low to saturate the mixer.

If the pidsum limit for the axis is >= 500, the behavior should be identical to the current mixer range only anti windup mechanism.
see #13486 for details.

[github-actions]

Do you want to test this code? You can flash it directly from Betaflight Configurator:

• Simply put #13506 (this pull request number) in the Select commit field of the Configurator firmware flasher tab (you need to Enable expert mode, Show release candidates and Development).

WARNING: It may be unstable. Use only for testing!

[tbolin]

fixes #13486

[ctzsnooze]

This PR is fascinating!

It has caused me think about fundamental things that few have thought about for a long time :-). Thank you!

Background....

The current iterm_windup / dynCi method suppresses iTerm growth based on motorMix. MotorMix itself directly related to pidSum. As motorMix increases above the threshold, iTerm accumulation drops to zero at the point where motorMix reaches 1.0.

Usually motorMix reaches around 1.0 at a pidSum of 500.

If the iterm_windup factor is at default of 85%, iTerm cannot accumulate to more than 425, approximately, if pidsum_limit is 500, since dynCi would be zero at a pidSum of 425.

If pidsum_limit is set, like pidSum on yaw, to 400, and if yaw is the sole source of PID activity, then motorMix for that axis can't exceed 0.8. This means motorMix on that axis doesn't quite reach the required value of 0.85, and hence iTerm can still accumulate, possibly all the way to the iTerm_limit of 400!

I think that the reason why the iTerm limit is 400 on yaw is to leave a bit of 'wiggle room' for unimpaired movements on the other axes, in situations where the yaw pidSum is very big. A limit of 400 means that yaw cannot saturate the motors completely. pidSum can still change up or down by another 100 before the motors saturate. Otherwise I'm not very sure why the limit on pidSum is set lower than roll and pitch.

This PR provides an alternate path for suppression of iTerm growth, per axis, in proportion to the pidSum_limit for the axis. This would prevent that possibility of large increases in iTerm on yaw during constrained yaw moves.

The main downside is that PR does maths every PID loop on each axis, and derives two very similar numbers but multiplies and constrains both of them separately (four times in all per PID loop).

Also I realised that our separate iterm_limit value, which is a simple hard 'output' limit on iTerm per-axis, could perhaps be used as the limit value for this new attenuation factor. If we did this, there could be no possible increase in iTerm whenever pidSum exeeded the iterm_limit value, because awe would effectively be constraining iTerm to that limit at the input side.

Hence this suggestion....

Perhaps we calculate dynCi like this, once per PID loop, returning a 1-0 value:

 float dynCi = 1.0;
if (pidRuntime.itermWindupPointInv > 1.0f) {
    dynCi = 1.0f - getMotorMixRange();
}

and then make a per-Axis pidSum based limiter, as suggested here, but use iTerm_limit as the limit factor, returning a value that goes from 1 through zero to some negative value. Then we get the lowest of these, multiply by the threshold inverse, and limit to 0-1... something like this:

float axisCi = (pidRuntime.itermLimit - fabsf(pidData[axis].Sum)) * pidRuntime.invItermLimit; // per axis
axisCi = constrainf(fminf(dynCi, axisCi) / pidRuntime.itermWindupPoint, 0.0f, 1.0f); // min of the two methods
const float iTermChange = (Ki + pidRuntime.itermAccelerator) * axisCi * pidRuntime.dT * itermErrorRate;

pidData[axis].I = previousIterm + iTermChange;

This should return the same values as the PR code, with very much simpler maths. On an F7X2 the above code saves 24 ITCM bytes compared to master, instead of adding 240bytes.

Since our existing iTerm_limit is 400, and since axisCi will be zero anytime pidSum reaches 400, and since iTerm is always less than PIDsum, we have a functional limiter on iTerm at 400, that's why new don't need a constraint to iTerm_limit anymore. And functionally if P is already say 100, then iTerm can't grow higher than 300. This will help avoid adding a lot of iTerm on impacts where both P and D can be high, and in the same direction.

Because motorMix and pidSum are closely linked, it may be possible to remove the iterm_windup / motorMix maths entirely. Large heavy yaw-sluggish machines could then get effective yaw iTerm windup suppression - and indeed on other axes - by using a lower iTerm_limit, instead of using iterm_windup. That would be a nice simplification.

[ctzsnooze]

Found a bug while testing the modified values.

As iTerm_windup approaches 100, there should be less and less effect. This is achieved by having the pidRuntime.itermWindupPointInv value approach a large value as iterm_weight approaches 100.

However, in the specific case of pidProfile->itermWindupPointPercent (iterm_weight) = 100, the current code returns a value of 1.0. That's roughly what we get with iterm_weight configured as 1.0.

To avoid this, the code needs to be:

    pidRuntime.itermWindupPointInv = 100.0f;
    if (pidProfile->itermWindupPointPercent < 100) {
        const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
        pidRuntime.itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
    }

[ctzsnooze]

Here is an image showing the modified code, as suggested above, with default PIDs, iTerm_windup set to 100 (to disable it), and on the left, iTermLimit of 200, vs 400 on the right.

In both cases, pidSumLimit is 400, and all inputs are yaw, bench testing, no stick input. The gyro trace shows some fast yaws and some slow ones.

The code works as it should. When P is high, iTerm stops growing when pidSum reaches the iTermLimit value, and that's what's intended.

In both cases, iTerm does not exceed the limit value. However it often is limited before reaching that value, due to the pidSum already being higher than the limit.

On the left, with iTerm limit of PIDsum hits maximum of 400 only when P is large. With slow movements only, iTerm can only build up close to 200. Note that during slow moves, where iTerm would normally build uo to a large amount, it is limited at 200, and pidSum stays close to 200. However when P is high, it can push PIDsum to 400.

Note that during slow iTerm-dominant moves, motors are not developing high motormix; that's exactly what we want.

On the right, with iTerm limit of 400, and pidSum limit of 400. iTerm now can have as much effect as P at max.

In both cases, pidSum is limited to 400.

[haslinghuis]

@tbolin please rebase - as it should build now.

[tbolin]

@tbolin please rebase - as it should build now.

done!

[haslinghuis]

How about implementing suggestion from Chris? We are about to release.

[tbolin]

How about implementing suggestion from Chris? We are about to release.
Going to take a look at them soon-ish.

Found a bug while testing the modified values.

As iTerm_windup approaches 100, there should be less and less effect. This is achieved by having the pidRuntime.itermWindupPointInv value approach a large value as iterm_weight approaches 100.

However, in the specific case of pidProfile->itermWindupPointPercent (iterm_weight) = 100, the current code returns a value of 1.0. That's roughly what we get with iterm_weight configured as 1.0.

To avoid this, the code needs to be:

    pidRuntime.itermWindupPointInv = 100.0f;
    if (pidProfile->itermWindupPointPercent < 100) {
        const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
        pidRuntime.itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
    }

itermWindupPointPercent is limited to between 30 and 100 (in cli https://github.com/betaflight/betaflight/blob/8b20cb096943f82eab1108a606fd6d6959bfe963/src/main/cli/settings.c#L1135C142-L1135C165), so the way I read the code setting itermWindupPointPercent = 100 is the only way to turn the limit off. Not intuitive but I suspect it is working as intended.
Wouldn't mind see it changed though, but I don't think I'm going to do that here.

If setting *itermWindupPointPercent * to 100 should effectively make the attenuation a yes/no the default should be a lot larger than 0.

[tbolin]

float axisCi = (pidRuntime.itermLimit - fabsf(pidData[axis].Sum)) * pidRuntime.invItermLimit; // per axis
axisCi = constrainf(fminf(dynCi, axisCi) / pidRuntime.itermWindupPoint, 0.0f, 1.0f); // min of the two methods
const float iTermChange = (Ki + pidRuntime.itermAccelerator) * axisCi * pidRuntime.dT * itermErrorRate;

pidData[axis].I = previousIterm + iTermChange;

I'm not sure this code results in exactly the same behavior. There are some cases where the output is not limited but the i-term would still be attenuated (e.g. if i_term_limit < pid sum < pid_sum_limit then the i-term would still change with the PR code, but be attenuated with the suggested code, as far as I can tell), I don't have an opinion on which behavior is preferable in the long run, but I think the the PR behavior changes the attenuation behavior less while still fixing the issue. Larger changes should be a separate PR IMO.
I agree that the code in the PR is naive and I will look at optimizing it tomorrow.

[haslinghuis]

Agree with separating fix and improvement here.

[ctzsnooze]

Hi again - yesterday I had time to flight test the modifications I suggested previously. Tested with iTerm limit down to 100. Unable to detect any change in flight performance, or PIDs, though I don't think I ever came close to hitting any iTerm or pidSum limits in-flight. Has been bench tested and definitely works.

This squashed branch on my GitHub shows my suggestion for changes from master.

This branch shows one commit of changes over this PR code.

This redraft prevents iTerm winding up in excess of iTerm limit, since the amount that can be added to iTerm goes to zero as pidSum approaches its limit. If there is no P or D or FF, iTerm can reach the iTerm limit. Otherwise, if other PIDs are active, iTerm accumulation falls, ultimately to zero a value if its contribution to pidSum would exceed the iTerm limit.

This prevents iTerm windup above iTerm limit under any conditions.

So it's basically a kind of dynamic iTerm limit, and will prevent iTerm windup under conditions where previously iTerm would wind up.

I think this is likely to fly a lot better than before, and should prevent iTerm windup in susceptible machines quite well.

The only 'quirk' is that there is an interaction between the user's iterm_windup value and the nature of the limiting. There is a 'slope' to the limiting behaviour that depends on the value of iterm_windup - a kind of soft-knee limiter. This contrasts with, and is preferable to, the hard limiting of the old code.

• if the iTerm windup value is high (approaching or at 100), iterm_limit behaves as a classical limiter, with a steep cutoff slope.
• if the iTerm windup value is at default 85, and iTerm limit is 400, the iTerm input gain is 1.0 up to pidSum of 340 and then falls linearly to 0 at 400. Very steep, but not a sharp cutoff.
• if the iTerm windup value is 50, with iTerm limit at 400 the iTerm input gain is 1.0 up to pidSum of 200, falling linearly to 0 at 400. A more gentle cutoff.

I like the idea of having a softer knee rather than hard limiting. We have a soft limit for the motorMix element in dynCi.

With defaults I don't think anyone will notice the soft limiting of iTerm, and I think that generally it would be better like that.

The soft knee have some practical effect only if iTerm weight is very low, and if also iterm_limit is low.

For example, setting an iTermLimit of 100 with iterm_windup of 30 means iTerm will only accumulate normally while pidSum is less than 30, which is a very low value. It will then be attenuated to zero when pidSum is 100. Keep in mind that an iTermLimit of 100 constrains iTerm to no more than 20% of the motor range, which is a very strong constraint. If they want to us iTerm limit at such values in the new code, they probably won't want to use iterm_weight at extreme values.

Note that iterm_weight is itself influenced by pidSum. In fact if pidSum approaches 500, motorMix will approach 1.0. These two factors effectively achieve very similar results. In logging there is not a lot to choose between them. The benefit of having both factors is that if P alone was to push pidSum to 350, with motorMix at 70%, the dynCi factor would be normal, and in the old days, iTerm itself could still accumulate to 400, whereas the pidSum factor method would limit iTerm to only 50 while P was that high.

So we do need both factors, for sure.

This redrafting requires less ITCM RAM than master, and involves fewer computations, and with the added benefit of soft-knee limiting, makes the modifications very attractive.

I didn't update the unit tests.

@tbolin please test with your windup-prone whoop. The debugging changes are temporary for review purposes; the factors can be seen in sensors tab with a whoop.

[ctzsnooze]

I made a crude emulation of my suggested code on Desmos.

The horizontal red line shows the functional limit to iTerm for given values of iTermLimit, pidSum, and motorMix. The iterm_windup value can be entered with a slider.

[BRNKR]

Looks like some major impact on control side. Is that just tested by one person shortly before releasing 4.5? Maybe I misunderstood the functionality of the changes, but can it lead to crashes?

[ctzsnooze]

@BRNKR no, this won't cause crashes. In any case, the best way to help out is to load it onto a quad and fly it. Can be an indoors whoop for instance. Or do some validation of the debugs via sensors tab.

@tbolin I haven't heard back from you, and there are people keen to release RC4 with this fix in place. We need to resolve the code. Perhaps I should make a separate PR with the alternate method I propose, so that it can be tested?

I made an updated emulation to play with, which:

• lets you enter input PID P and I values, and sets pidSum to the sump of these
• sets motorMix based on the pidSum, where motorMix would be 1.0 at pidSum of 500
• calculates an iTermLimit based on the old Master method.

With this we can see how if the pidSum limit is set low, motorMix cannot reach the value needed to suppress iTerm accumulation.

From doing the emulations, I think there is a second 'bug'...

If the user configures pidSumLimit to be less than iterm_limit, eg to 350, then the existing method will never get any iTerm_windup attenuation, because the motors will never exceed the threshold.

This also affects the pidSum method, because if it is limited below iTerm_limit, we will never get full pidSum based attenuation, either.

So I think we need to add a check in pid_init.c to give precedence to the pidSumLimit, in the form of:

pidRuntime.itermLimit = MIN(pidProfile->itermLimit, pidProfile->pidSumLimit);

That fixes this specific issue. It clearly makes no sense to permit an iTerm limit in excess of the overall pidSumLimit.

Also....

iterm_windup only works when the motorMix exceeds the threshold, and whenever pidSum_limit is set significantly below 500, it then gets hard for iterm_windup to work, if at all. For example, if iterm_windup is set to 300, motorMix can only exceed 0.6 if more than one axis is active. Since 0.6 is below the default iterm_windup of 0.85, iTerm can accumulate on single axis issues up to its limit, with no constraint from iterm_windup.

Adding the pidSum based attenuation, that problem disappears.

[ctzsnooze]

On further testing, I start to wonder if we should, with this change, remove the old motorMix / dynCi method completely.

motorMix considers the combined effect of pidSum on all axes, once mixed, on all motors. It isn't 'axis-specific'. The new pidSum based method is axis-specific.

Emulation testing shows that on a single axis, the modified pidSum method always suppresses iTerm more strongly than motorMix. I think that iTerm windup problems should properly be considered on a per-axis basis, alone.

I'll test this out. We can make the dynCi value always 1.0. The itermWindupPointInv will still act as a kind of 'soft knee` factor, with low values causing an earlier and more gentle introduction of the iTerm suppression, and high values making a more abrupt and steep limiting.

[ctzsnooze]

@tbolin a radical simplification, with complete removal of the motorMix limiting, inspired by your ideas, is in PR #13525.

[tbolin]

@ctzsnooze sorry for not getting back yo you. Life got in the way.
The idea with this PR is to avoid a huge change in anti windup behavior with different values of pid_sum_limit, while changing the current behavior as little as possible. IMO that's the sanest approach for a bug fix.
It is not intended to make the anti windup protection sane. That's a much bigger project and in my opinion something that should wait until the next version.