Refactor Feedforward Angle and RC Smoothing - mashup of 12578 and 12594

[ctzsnooze]

This PR combines #12578 and #12594, which are now closed. Both shared a lot of common changes in pid.c and rc.c, and would be difficult to merge sequentially. It includes the current updated Angle and Horizon code.

Note: Users who want the old Angle mode behaviour should set expo to zero in their Rates, and:

set angle_feedforward = 0     // default is 50
set angle_earth_ref = 0       // default is 100
// restore angle strength (now angle_p_gain) and angle_limit to your previous value/s

Note: Horizon users should adjust the following parameters to suit their preference:

horizon_level_strength   // how aggressively the quad self-levels when flat
horizon_limit_sticks     // the percentage stick deviation at which there is no levelling
horizon_ignore_sticks    // when on, stick travel will not attenuate the levelling
horizon_limit_degrees    // the angle of the quad at which there is no levelling
horizon_delay_ms         // the time constant affecting the onset of the levelling

There are two functional changes that are added by this PR, on top of the changes in #12578 and #12594:

1. Delayed onset of levelling in Horizon when returning sticks to the centre. As suggested by @justainchoe, this adds a delay whenever the levelling strength increases in Horizon mode. Decreases in levelling strength are unaffected and happen immediately. The delay is achieved by applying a first order lowpass filter, with a time constant of 500ms by default to the horizon strength factor, where a horizon_delay of 50 means 500ms. The code then uses the lowest of the un-smoothed and smoothed value, where a low value means no smoothing. This results in a delay only when the horizon strength value increases. The pilot can do a flip and bring their sticks hard back to centre, and the levelling will come on relatively slowly, avoiding the previously unwelcome 'snap' due to the combination of self-levelling and the pilot bringing the sticks to a halt. It there's too much 'snap', or if you want the levelling to come in only after a longer time, increase the time constant (up to 250 or 2.5s). For more immediate levelling, as perhaps preferred by a person learning Acro via Horizon, the delay can be reduced. If set to zero, there is no delay.

2. Small modification to feedforward_max_rate_limit. Now this value affects the slope of the limiting line, with zero feedforward always by the time setpoint reaches maximum, rather than moving the line left and right, while the users PID "P" value modifies the slow. Racers may prefer feedforward_max_rate_limit at values around 100-120 to maintain full feedforward until quite late in a full stick move. Cinematic users or those with heavier quads may want a smoother, less aggressive feedforward, in which case feedforward_max_rate_limit could be around 50, and the RC smoothing on feedforward increased a bit (lower cutoff than default).

Please refer to #12576 and #12594 for detailed notes on the other changes..

There is a lot of efficiency related refactoring, resulting in significant CPU and flash space savings.

Summary of changes:

• bug fix for an issue that caused feedforward RC smoothing to get a different value from setpoint in auto-rc-smoothing mode
• fixes issues that caused the RC link speed to be calculated incorrectly or inaccurately, especially at high telemetry rates. - rc_smoothing calculations now use lowpass filtering with much simpler code to detect link speed changes - many thanks to @KarateBrot for advice on how to best do this. RC smoothing is now updated every 3 valid samples.
• fixes an issue where the user's PID P value altered the feedforward max_rate_limit_factor attenuation slope. This was by design, but a bit annoying, and it was confusing that high P caused feedforward to be stronger at the start of flips, and vice versa. Now the feedforward_max_rate_limit value controls the slope of the attenuation line, so that when it is set low, feedforward will, at the start of a flip, ease back earlier, and when set high, stay strong for later. Default of 90 remains the same and suits high authority quads. Quads that overshoot at the start of a flip should consider a lower than default feedforward_max_rate_limit.
• fixes and issue where Horizon mode would get RC stepping in the levelling element with slow link speeds
• user-adjustment of Angle Mode Earth Referencing strength is now possible via OSD
• refactoring and simplification of the feedforward code to improve efficiency (many changes)
• minor fix to stop overflow of debug values in rc_smoothing_rate debug
• quite a few debug changes since some parameters don't exist anymore
• editorial changes to rename variables named 'rate' when the value was a time interval
• about 1k less flash space required
• On F411, 3% less CPU in acro, 4-4.5% less in angle/horizon.

There should be no change in feedforward or RC smoothing functionality. All quads should fly the same, just a bit more smoothly. CPU load / instability should be a bit less of a problem on marginal setups.

RC smoothing values and reported link speeds should be accurate in all circumstances. Previously when the RC smoothing calculations were in error, RC link stepping could get through to the motors via feedforward or setpoint. This shouldn't happen any more, including in Angle and Horizon mode.

The code should be able to deal with radios that change links dynamically, even in small steps. Previously there was an issue detecting change from 333 to 250hz. Now it should respond to any change greater than 1hz, and do so within a few packets.

Please also test feedforward by flying and noting normal feedforward behaviour, and log feedforward.

If you find any issues with unexpected link intervals or rc smoothing issues, please either raise an issue here on GitHub or a pre-issue on Discord.

Feedforward, when a new RC packet arrives, requires 125 cycles without averaging, jitter or transition. This increases to 170 cycles with averaging, 196 cycles when also adding jitter reduction, and 209 cycles with "the lot".

RC Smoothing requires 18 cycles while idle, about 160 cycles when a new RC packet arrives in that is not much different from the previous interval, and about 250 cycles when the interval is sufficiently different that new cutoffs are required.

Recoding for efficiency mostly involved:

• not using "#DEFINE" values, instead using constants in the code, or setting the value when declaring the variable that needed it, with a comment
• avoiding unnecessary int to float and float to int conversions, especially 'hidden' or inside loops
• generally using floats if at any point they would be multiplied by a float or divided by a float
• using const variables whenever possible, especially for values that were repeatedly calculated inside loops
• pre-calculating values required for divisions in the pidRuntime init, so we only need to do multiplications.

[TheIsotopes]

@ctzsnooze everything works as expected again.
smoothness is slightly worse than with the current master and the same settings.

I don't notice any difference when flying, but the logs look a bit different than usual.

BTFL_BLACKBOX_LOG_Egodrift_20230406.txt

[haslinghuis]

Only needs to update unit tests and remove comments.

[TheIsotopes]

tested again with rebased PR on ghost and frsky sbus.
everything flies as expected, to me all looks fine. 👍

[haslinghuis]

Need to fix unit tests before merging

[ctzsnooze]

Need someone to help get the 'container' for the unit tests. I'll do the tests themselves, but I can't figure how to do them now the code is in a different place. So if not one person here can help, then we should merge the PR without the unit tests.

[ctzsnooze]

@borisbstyle @haslinghuis - in this PR:

• getFeedforward(axis) returns the feedforward value for the axis from rc.c, where it is calculated from incoming rc data.
• in pid.c, pidSetpointDelta is set to getFeedforward(axis), or zero in Angle mode.
• then, simply:

        if (feedforwardGain > 0) {
            float feedForward = feedforwardGain * pidSetpointDelta;
            pidData[axis].F = feedForward;
        } else {
            pidData[axis].F = 0;
        }

In the previous 'master' code the functions applyFeedforwardLimit, feedforwardInit, feedforwardApply, and shouldApplyFeedforwardLimits were included in the unit test.

However, the feedforwardApply calculated setpointDelta from simulated setpoint values locally. The function itself, which resided within feedforward.c, was not called; instead, a value for setpointDelta was calculated within the test itself from simulated setpoint values. This did not test the function of the feedforwardApply function within feedforward.c at all. Also, the transition factor was calculated inside the test, and did not use the transition code in pid.c.

In short, the old tests didn't test the code in master anyway - it 'cheated'. The 'test' only tested the code in the test, not the code we flew. At least that's how I see it.

After this PR, these old functions don't exist anymore.

To do the unit test entirely within the pid_unitest, we would need to get rc.c to calculate pidSetpointDelta using getFeedforward(axis) from RC values that we send to rc.c. That seems very difficult.

I think that to test the feedforward code itself, we would need need a test in rc.c that checks what would return for getFeedforward(axis), given certain stick position changes.

Then only test required in pid.c is to multiply a simulated feedforward value by the feedforward gain value, and check it is zero in angle mode.

I have updated the pid_unittest.

It now returns sensible values for feedforward for a given simulated setpointDelta value.

I don't think I have the capability to write a unit test for rc.c.

[haslinghuis]

@borisbstyle this should be good for now as we can improve the tests later.
@ctzsnooze thanks for reverting clang version - as I overlooked that.

[github-actions]

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!

[TheIsotopes]

@ctzsnooze here another testflight ... hope now that's the last before merge

#12605_testflight.zip

[nerdCopter]

b17cd84cf LOS Horizon Mode.
set debug_mode = RC_SMOOTHING_RATE
ELRS; TX 500hz, 1:4 telem
BTFL_BLACKBOX_LOG_APEX-6inch_20230423_123855_LOS_HORIZ_PR12605.BBL.zip

# 

# version
# Betaflight / STM32F7X2 (S7X2) 4.5.0 Apr 23 2023 / 15:14:14 (b17cd84cf) MSP API: 1.46
# board: manufacturer_id: APEX, board_name: APEXF7

# rc_smoothing_info
# RC Smoothing Type: FILTER
# Detected Rx frequency: 503Hz
# Active setpoint cutoff: 25hz (manual)
# Active FF cutoff: 25hz (manual)
# Active throttle cutoff: 188hz (auto)

# status
MCU F722 Clock=216MHz, Vref=3.28V, Core temp=53degC
Stack size: 2048, Stack address: 0x20010000
Configuration: CONFIGURED, size: 3361, max available: 16384
Devices detected: SPI:1, I2C:1
Gyros detected: gyro 1 locked dma
GYRO=MPU6000, ACC=MPU6000, BARO=BMP280
OSD: MAX7456 (30 x 13)
BUILD KEY: 7e43be5093e31b54bc8383ab9a0594d0 (4.5.0-zulu)
System Uptime: 35 seconds, Current Time: 2023-04-23T17:41:19.109+00:00
CPU:34%, cycle time: 124, GYRO rate: 8064, RX rate: 500, System rate: 10
Voltage: 295 * 0.01V (0S battery - NOT PRESENT)
I2C Errors: 0
FLASH: JEDEC ID=0x00ef4018 16M
Arming disable flags: NOPREARM CLI MSP

# tasks
Task list             rate/hz  max/us  avg/us maxload avgload  total/ms   late    run reqd/us
00 - (         SYSTEM)     10       7       0    0.0%    0.0%         0      1    429       6
01 - (         SYSTEM)    995      13       0    1.2%    0.0%        23     72  42290       2
02 - (           GYRO)   8006      15       1   12.0%    1.4%       578      0 342169       0
03 - (         FILTER)   8006      29       9   23.2%    7.2%      3135      0 342169       0
04 - (            PID)   8006      64      28   51.2%   22.7%      9807      0 342169       0
05 - (            ACC)    992      18       2    1.7%    0.2%       131     45  42254       9
06 - (       ATTITUDE)    100      27      11    0.2%    0.1%        46      7   4694      22
07 - (             RX)    388      32      16    1.2%    0.6%       773     65  47997      18
08 - (         SERIAL)    100  296907       1 2969.0%    0.0%       559      0   4171      24
09 - (       DISPATCH)    993      14       0    1.3%    0.0%        27     61  42295       6
10 - (BATTERY_VOLTAGE)     50      15       3    0.0%    0.0%         4      3   2131      10
11 - (BATTERY_CURRENT)     50       7       0    0.0%    0.0%         1      6   2131       4
12 - ( BATTERY_ALERTS)      5       9       2    0.0%    0.0%         0      1    215       8
13 - (         BEEPER)    100      13       0    0.1%    0.0%         5      9   4170       9
14 - (           BARO)     40      41      23    0.1%    0.0%       119     10   5083      38
15 - (       ALTITUDE)    100      15       2    0.1%    0.0%        12      7   4170      11
16 - (      TELEMETRY)    499      19       0    0.9%    0.0%        36     21  20937       6
17 - (            OSD)     12      55      17    0.0%    0.0%       179     57  13227      32
19 - (            CMS)     20       9       2    0.0%    0.0%         1      3    852       8
20 - (        VTXCTRL)      5      35       1    0.0%    0.0%         3      4    215      34
21 - (        CAMCTRL)      5       6       0    0.0%    0.0%         0      0    215       5
23 - (    ADCINTERNAL)      1      11       3    0.0%    0.0%         0      2     43      10
24 - (       PINIOBOX)     20      15       2    0.0%    0.0%         2      2    852      11
25 - (SPEED_NEGOTIATION)    100      11       1    0.1%    0.0%         5      2   4170       5
RX Check Function                  14       0                        66
Total (excluding SERIAL)                                32.2%

# 

Fixed per review

[KarateBrot]

@haslinghuis Dismissing my review is not okay. We were still in the review process. Please never do this again!

[haslinghuis]

@KarateBrot Just trying to keep the project going here. Assumed your request was fixed.

We always can fix upon (any) work if it doesn't turn out. As this PR was hanging and multiple requests for merging were pending and tested were positive I deemed it ready for merge.

[ctzsnooze]

Sorry @KarateBrot I thought that I had addressed your comments. And I remain very grateful for the time you took to make those reviews.

My error was to mark those which I thought had been addressed, as 'resolved', which I did for housekeeping purposes, so I could track which had been dealt with. I thought that this could always be reversed if we wanted to go back and check on how they were 'resolved'.

Unfortunately, I now see that, on merging, all review discussions marked as 'resolved' have vanished. I didn't expect this.

It would be much better that all review discussions, including those marked as 'resolved', are retained on merging.
I don't know if this is a GitHub option. In future I will leave all reviews 'open' even if they are 'resolved' because the discussions in those reviews can be useful in future.

The remaining review questions that I didn't resolve are still open, eg the query about the CLI print line function.

@blckmn I wonder if those review discussions can be restored somehow? Or if we can change our GitHub settings to not delete 'resolved' review discussions? Could it be that the 'squash' associated with the merge causes our review discussion to be lost?

[nerdCopter]

recommend author(s) set long-standing PR(s) as draft until truly "ready".

[nerdCopter]

we can still see some comments/unresolved via the "files changed" tab.
e.g. https://github.com/betaflight/betaflight/pull/12605/files#r1172738403 & https://github.com/betaflight/betaflight/pull/12605/files#r1172778392

maybe the conversations drop-down as well