Refactor feedforward, use acro Rates in Angle mode #12578

ctzsnooze · 2023-03-27T01:49:27Z

This is a big refactoring of our Feedforward, Angle and Horizon code. It flies really well and fixes several bugs.

Apart from not having the unit test, the code is, I think, ready to merge. I am very confident about the changes and we need them tested more widely.

Fixes:

an issue in existing 4.5 code where RC smoothing wasn't being applied correctly for 50hz RC links (thanks glennv).
another issue where feedforward wasn't calculating correct RC smoothing setpoints (see Fix feedforward auto rc smoothing calculation #12594)
saves about 750 bytes of flash space, and uses about 1.5-2% less CPU when Acc is enabled.

I'd be really grateful for technical/coding advice on restoring the feedforward unit tests into rc.c

Main functional change:

Angle mode uses Acro rates to adjust stick sensitivity
PID feedforward in Angle mode

Notes:
This PR inherits the earth referencing and input feedforward of the previous Angle PR's for 4.5. There is no change to these factors, although smoothing may be improved / more reliable.

On defaults, Angle mode has become much quicker to respond than before. It definitely requires expo to attenuate the speed of those responses. Hence, this PR removes the old angle-specific angle_roll_expo and angle_pitch_expo settings; the user must use Acro Rates to modify Angle/Horizon stick responsiveness. When moving from Angle to Horizon, Angle to Acro, or between pitch and roll in Level Race mode, the stick 'feel' is more consistent.

Pilots who previously flew older Angle code, with customised angle expo settings, may not like this. Please give this Rates method a good try, however. The old Angle expo settings can be emulated using our current Rates tools - it's easy adjust the curves, and see what is happening in the Rates panel; there's no need to go to the CLI to adjust angle_expo and get an uncertain outcome.

As before, Angle feedforward 'strength' is adjustable with the angle_feedforward factor. When set to zero, there is no angle feedforward, bringing the responsiveness back to the slow 'old way', with its overshoot and lag. Likewise, the earth referencing can also be disabled by setting it to zero, but then you'll get the wobbles after quick yaw inputs again.

Detailed changes:

removed feedforward.c and feedforward.h files completely
moved the feedforward code that should run when a new RC packet arrives into rc.c's processRcCommand() function, which handles incoming rc data and calculates setpoints. This will make it much easier to provide duplicate interpolation on setpoint, to remove duplicate glitches on P as well as feedforward (later PR, only benefits Rx like FrSky, ELRS doesn't send duplicates).
refactored the feedforward limit code that pulls back on pid 'F' when the quad approaches maxRate so that it runs when a new packet arrives, not every PID loop, resulting in less CPU usage and smoother adjustments
tweaked the feedforward code to handle interpolation a bit better than before.
removed the special expo/setpoint calculations unique to Angle setpoint, using normal Acro Rate setpoint values instead, making the stick feel similar. This can only be done because Angle mode is now almost 10x more responsive than it was
Angle mode's input feedforward uses the same feedforward code as Acro (the same value). This provides proper RC smoothing, removal of duplicates, etc. The angle_feedforward strength factor is user-adjustable, as before. Default is 50, or 50% of the amount of feedforward that would apply in Acro, but that doesn't mean it is twice as weak in the Angle mode setting.
in 4.4, Horizon code was a bit dull right on centre, because it had no input or output feedforward on the Angle elements. Now Horizon mode is quite responsive at all stick positions.
removed all the additional duplicated feedforward code that was added by the earlier 4.5 Angle/Horizon PR's

Limitations and issues:

In Angle mode, there is no feedforward in the PID loop. All feedforward goes into the input side of the Angle controller, and there is no PID feedforward applied to the setpoint which is sent to the PID loop. As a result, the PID loop is very smooth, and there is the usual setpoint to gyro delay that you get with no PID loop feedforward. However the Angle setpoint is now much faster and more aggressive than before, and there never was feedforward in the PID loop, previously. So the overall stick 'feel' is still very responsive. It is technically simple to include PID loop feedforward in Angle mode. Testing revealed that Angle Setpoint noise would get through into the feedforward, especially at low RC link rates. The best solution is a dedicated additional filter on the affected axes, like the gyro D filter, after the Angle Setpoint was calculated. Otherwise the user would have to configure the setpoint smoothing to be twice as strong as normal, but that would affect Angle P and also affect the normal acro filtering, and would not be specific to the affected axes only. Additionally, feedforward can lead to overshoot, and that is already an issue with the Angle P controller. On balance it was felt best to not provide feedforward in the PID loop in angle.

To Do:

One last thing.Fix the feedforward unit tests, which must to be migrated to the rc.c unit test

Confirmed:

Feedforward strength isn't affected by PID loop rate or Rx link speed.
Boost and jitter reduction work normally
Feedforward max_rate_limit works normally
Normal RC smoothing and feedforward smoothing
feedforward averaging works properly
the interpolation works really well on FrSky telemetry packets.

blckmn · 2023-03-27T03:17:21Z

AUTOMERGE: (FAIL)

github identifies PR as mergeable -> FAIL
assigned to a milestone -> PASS
cooling off period lapsed -> PASS
commit count less or equal to three -> FAIL
Don't merge label NOT found -> PASS
at least one RN: label found -> PASS
Tested label found -> FAIL
assigned to an approver -> PASS
approver count at least three -> FAIL

Need help with feedforward

ctzsnooze · 2023-03-27T06:17:54Z

It flies really well.
Here is a quick roll move input in Level mode.
The green line shows me moving my roll stick out and then back within ⅓ of a second. This means, quickly gain angle in about 170ms, and then get back to level again.
The orange line is 'calculated roll attitude' from the log viewer.
The quad lags by only about 100ms. This is the kind of lag we used to get in Acro, in the past.
The input feedforward in Angle, plus angle P, results in the Roll Setpoint line that is visible in pink, and closely followed by roll setpoint.
The PIDs show normal behaviour with relatively weak but smooth feedforward.
Everything is nice and smooth.
Note that the PID feedforward element will not be included in this PR.

ctzsnooze · 2023-03-27T06:20:28Z

This is a quick 360 roll in Horizon mode, a bit difficult to explain everything, but it shows the system working as expected.
Horizon overshoots the end of a fast roll because of it's self-levelling behaviour, but otherwise flies like Acro. All PID feedforward is normal Acro feedforward, but there is input feedforward in the self-levelling component.

ctzsnooze · 2023-03-27T06:23:08Z

This is another roll in Horizon with a slower start and controlled stop, looks just like acro :-)
But, if I centre the sticks the quad self-levels gracefully.

tbolin

I like the move to dividing the calculation from the get, and triggering the calculations from RC rather than the PID loop.

I have mostly looked at the feedforward calculation function and maybe got into a bit too much detail.

I'll probably have more comments later

src/main/fc/rc.c

tbolin · 2023-03-27T10:39:43Z

src/main/fc/rc.c

+    }
+}
+
+FAST_CODE_NOINLINE void calculateFeedforward (int axis)


Can this be made static or STATIC_UNIT_TESTED? It's not supposed to be called from outside rc.c, right?

Also, having the actual arguments as arguments rather than an index to a static array would make it a lot easier to write tests for this function. Same with returning the feedForward delta rather than just storing it.
There are a lot of de facto arguments, so maybe a struct containing all arguments for an axis would be a good idea.
e. g. instead of

static float prevRcCommand[3]; static float prevRcCommandDeltaAbs[3]; // for duplicate interpolation etc etc

there would be a struct like

typedef struct feedforwardState_s { float prevCommand; float prevCommandDeltaAbs; etc... } feedforwardState; static feedforwardState feedforwardStates[XYZ_AXIS_COUNT];

and then the signature of the function would be something like

float calculateFeedforward(feedForwardState* ffState)

Just a thought (Not sure every argument should be in the struct)

Thanks, this function could definitely be STATIC.
As for unit tested, IDK what STATIC_UNIT_TESTED changes over just STATIC?
Using a struct may help tidy things up, for sure.

STATIC_UNIT_TESTED translates to static unless Betaflight is built for unit testing, then it translates to nothing. That way the function can be reached from outside the translation unit when being tested (i.e. from outside the same .c file).

Thanks, I'd like to implement the struct with some advice exactly how, since some parameters come from pid.c, and the main benefit would be if we could simplify getting those multiple values using a struct or something.

I'll add STATIC_UNIT_TESTED once I've figured how to restore the feedforward unit tests (currently all deleted).

tbolin · 2023-03-27T10:45:28Z

src/main/fc/rc.c

+    float setpointAcceleration = 0.0f;
+
+    // calculate an attenuator from average of two most recent rcCommand deltas vs jitter threshold
+    float ffAttenuator = 1.0f;


Why change the name from jitterAttenuator? That was more descriptive to me at least. Adding ff in the feed forward calculation function doesn't really give me any more information.

Good catch.
Ah, it's also used to cause attenuation to zero in some other places, that are not specifically jitter-related. ie it's not exclusively used for jitter control.
If I rename it to feedforwardAttenuator which is shorter than feedforwardJitterAttenuator.
I'll do that.

feedforwardAttenuator doesn't really tell me more either. That we are in a function called calculateFeedforward is enough of a hint that the variable has something to do with feedforward.
Maybe call it jitterAttenuator again and use different variables for the other cases?
It might look inefficient but the code will probably be easier to follow and the compiler will probably sort things out in the end.

After checking the code functionally, I needed a value to force some lines to zero, independently of a value to attenuate on the basis of the jitter attenuation code. So we're back to the original jitterAttenuator again :-)

tbolin · 2023-03-27T10:56:53Z

src/main/fc/rc.c

+//                 DEBUG_SET(DEBUG_FEEDFORWARD, 2, lrintf(setpoint)); // un-smoothed setpoint after interpolations
+//             }
+
+    float absSetpointSpeed = fabsf(feedforwardDelta); // unsmoothed for kick prevention


absSetpointDelta might be a better name.
A speed should always be absolute and calling this a speed when it's a difference between two setpoints, with no invlovement of the time between them is a bit odd to me.

I'll do a bunch of re-naming, hopefully to tidy up these loose ends. Thanks for finding them.

tbolin · 2023-03-27T11:38:31Z

src/main/fc/rc.c

+    float absSetpointSpeed = fabsf(feedforwardDelta); // unsmoothed for kick prevention
+
+    // calculate acceleration, smooth and attenuate it
+    setpointAcceleration = feedforwardDelta - prevSetpointDelta[axis];


Calling this an acceleration makes me a bit confused.
In the current code it was an acceleration since it was calculated as

setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis]; setpointAcceleration *= rxRate * 0.01f;

and setpointSpeed was the derivative of the setpoint, including the multiplication with rxRate (so really it was setpoint velocity but...).
Here setpointAcceleration is the difference of a difference. Not really sure what to call that, but calling it an acceleration instantly have me looking for a multiplication with rxRate and assume that there is a bug when I can't find one.

In the current master code the acceleration is calculated based on the smoothed velocity.
Here the delta is smoothed after the "acceleration" is calculated. Is that intended?

The setpointSpeed or setpointDelta is the rate of change of setpoint over time, or setpoint 'velocity'.
The rate of change in setpointDelta over time is the second derivative, or 'acceleration' of setpoint.
I'll look at the smoothing etc again.

The problem I have is that the over time part is missing to make either value a rate of change.
If the rxRate was factored in there I wouldn't have any problem with these names.

Yes good point, I was too quick with the original draft. I'm back to using RxRate to convert the delta to a 'velocity', and have set the names accordingly. This was necessary to ensure that the 'velocity' was the same at different RC rates, and to make the interpolation work correctly.

tbolin · 2023-03-27T14:14:04Z

src/main/fc/rc.c

+    feedforwardDelta += feedforwardBoost;
+    feedforwardDelta *= rxRate;


Maybe this value should use another name since it no longer a difference between setpoints?
Something like:

Suggested change

feedforwardDelta += feedforwardBoost;

feedforwardDelta *= rxRate;

float feedforwardValue = (feedforwardDelta + feedforwardBoost) * rxRate;

and then keep using the new name further down.

Done, thanks.

tbolin · 2023-03-27T14:41:41Z

src/main/flight/pid.c

-        pidSetpointDelta = feedforwardApply(axis, newRcFrame, pidRuntime.feedforwardAveraging, rawSetpoint, rawSetpointIsSmoothed);
-#endif
+        // -----calculate D component
+        float pidSetpointDelta = currentPidSetpoint - pidRuntime.previousPidSetpoint[axis];


Won't this make a pretty significant change to the behavior of D_MIN?

Good point.
I re-arranged it a bit so that it will now work exactly as it did before.
The 'setpoint' value to Dmin will, as before, be the feedforward value calculated from the feedforward code. This will be true except when the axis is under Angle mode control, because setpoint then does not come direct from rates, but from the angle control code. Mostly those setpoint changes are slow, so Dmin would not likely be boosted much towards DMax, but for quick stick inputs where the quad was given a fast setpoint change, Dmin would rise as it would in acro.

tbolin · 2023-03-27T14:45:02Z

src/main/flight/pid.c

+            // these axes will already have stick based feedforward in the input to their angle setpoint
+            // we can use the simple setpointDelta from Dterm to generate a pidF element to help control motor lag
+            // this will not apply in Horizon mode; it is not needed because the acro normal setpoint feedforward is present
+            pidSetpointDelta = pidSetpointDelta * pidRuntime.pidFrequency * pidRuntime.angleFeedforwardGain;


Maybe create a new variable here instead of reusing pidSetpointDelta?

tbolin · 2023-03-27T14:47:45Z

src/main/flight/pid.c

+            pidSetpointDelta = getFeedforwardDelta(axis);
+        }
+#endif
+
 #ifdef USE_ABSOLUTE_CONTROL
        // include abs control correction in feedforward
        pidSetpointDelta += setpointCorrection - pidRuntime.oldSetpointCorrection[axis];


I know it hasn't been changed, but this part raises a warning flag for me. It looks like it assumes that setpointDelta is still a difference between setpoints, which it is not. I'm a bit to tired right now to do a through analysis.

Thanks for your comment, it's a good point.
In pid.c, pidSetpointDelta is simply currentPidSetpoint - pidRuntime.previousPidSetpoint[axis].
As such it is the difference in setpoint per PID loop interval, and PID loop interval can vary.
I have checked that, in this PR, changing loootime does not affect the reported pidSetpointDelta value. To be honest, I was concerned, but there is definitely no problem:

tbolin · 2023-03-27T15:09:34Z

src/main/flight/pid.c

+            const float feedforwardMaxRate = pidRuntime.feedforwardMaxRate[axis];
+            const float Kp = pidRuntime.pidCoefficient[axis].Kp;
+            if (axis < FD_YAW && feedforwardMaxRate != 0.0f) {
+                if (feedForward * currentPidSetpoint > 0.0f) {
+                    if (fabsf(currentPidSetpoint) <= feedforwardMaxRate) {
+                        feedForward = constrainf(feedForward, (-feedforwardMaxRate - currentPidSetpoint) * Kp, (feedforwardMaxRate - currentPidSetpoint) * Kp);
+                    } else {
+                        feedForward = 0;
+                    }
+                }
+            }


Is this new or old functionality?
It looks a bit out of place compared to all other FF calculations goes pretty far to avoid using the pid rate setpoint.

This is the original function, and hasn't been changed in a long time. I just moved it here from where it was. It works as it did.

I've removed this from pid.c and refactored it within the feedforward() calculation in rc.c. It ends up a bit simpler there, and doesn't need recalculating every pid loop.

setpoint noise gets through the simple delta

github-actions · 2023-04-02T04:45:36Z

Do you want to test this code? Here you have an automated build:
Assets
WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

ctzsnooze · 2023-04-03T02:43:42Z

Closing since code now incorporated into #12605