Unlocking the Potential of Faster Attitude Rates in Copter Control
For the past few weeks, I’ve been diving into the world of fast attitude rates. This journey started somewhat by chance. Initially, I thought Leonard would tackle the challenge, but, when I discussed it with Tridge, he surprised me by saying it was more his domain. He quickly put together a thread-based rate loop, which worked, but not sufficiently well for there to be any discernible advantage - when I pushed it harder, things started falling apart. Tridge was somewhat sceptical about its potential, but I decided to take on the challenge and see if I could push it further!
After fixing several issues, I had a breakthrough during auto-tuning. The responses were significantly snappier, giving me the confidence to continue. Let me explain what’s happening.
Imagine you’re flying a copter. Its control system is based on a PID (Proportional-Integral-Derivative) controller. When you command a roll to the right, say at 90 degrees per second, the copter’s current rate is compared to this target. The PID controller calculates the error – the difference between the target and actual rate.
- P (Proportional): Applies input proportional to the error. For example, with a P value of 1, it applies a rate of 90 degrees per second if the error is 90 degrees per second.
- D (Derivative): Prevents overshoot by responding to the rate of error change. Faster changes in error increase the D response, countering the P response to prevent overshooting.
All this control takes time. If the loop rate is 400 Hz, it takes a minimum of 2.5 milliseconds to detect an error and respond. While this is fast for humans, it’s a significant delay when handling disturbances like prop wash or high-rate oscillations. In theory, running the PID controllers faster can improve control. Betaflight, for instance, normally runs the PID controllers at at least 4kHz.
ArduCopter’s main loop can run up to 800Hz, maybe 1000Hz if you are lucky, improving control for smaller copters. However, running it faster is limited by the computational expense of the EKF (Extended Kalman Filter) maths. Tridge’s innovation was to separate the rate updates into a different thread, allowing them to run as fast as needed.
Ideally, rate updates should match the gyro data rate. On my test copter, the gyros run at 4kHz. By running the rate loop at 4 kHz, we reduce the detection time from 2.5 milliseconds to 250 microseconds, enhancing the copter’s responsiveness to disturbances and inputs. This tighter control allows for more aggressive tuning.
Oscillation happens when the PID controller overcompensates. With faster rates, we can apply more P without causing oscillation because D can respond quickly enough to moderate the P response. On my test copter, tuning with a 4kHz rate loop has allowed me to double the PIDs, which is a very substantial improvemen!
This change is a game-changer and is likely to be included in version 4.6. While it works beautifully on H7 boards, F4 boards might struggle due to the computational load. On an H7 running at 4kHz, the rate loop consumes 25% of the CPU, a significant chunk. F4 boards might manage at 1kHz, but higher rates could be challenging.
Configuration
There are only two new settings for enabling fast rate attitude, but these have an effect on several other settings:
- FSTRATE_ENABLE
- 1 - enable dynamic fast rate, on high CPU load (as determined by either the main loop being slow or the gyro sample queue filling up) the rate will be reduced to an integer divisor of the gyro rate until the CPU stabilises. This is the safest way of trying the support.
- 2 - enable dynamic fast rate, but fix to the requested rate on arming. This allows un-armed operations such as log download to proceed effectively. Note that you must be sure that you have sufficient CPU to enable this option or control can be compromised.
- 3 - enable fixed fast rate. This is useful for testing
- FSTRATE_DIV - this defaults to 1 and represents the maximum fast rate gyro divisor. So a value of 2 will yield a rate loop of gyro rate / 2. This allows a fixed fast rate that is lower than the gyro rate. This parameter is dynamic and can be modified in-flight.
So the easiest way to try this is set FSTRATE_ENABLE = 1. If your flight controller is struggling it will reduce the attitude loop rate until normality is restored. For fixed rates I do not recommend trying this until you have seen reasonable performance with dynamic rates. One way to gain more CPU is to switch off additional IMUs using INS_ENABLE_MASK another way is to reduce INS_GYRO_RATE. The main CPU load is from the INS and attitude threads.
When the fast attitude loop is enable, several other settings are affected:
- SERVO_DSHOT_RATE - this represents a multiplier of the main loop rate to output to the motors. When the fast rate loop is enabled this is normally set to 1, however if the calculated rate will not exceed the gyro rate, higher values are allowed. For instance if INS_GYRO_RATE = 2 and FSTRATE_DIV = 4 then values of SERVO_DSHOT_RATE up to 4 are allowed.
- Fast attitude and PID logging in LOG_BITMASK - setting these bits usually logs at the main loop rate, however if fast attitude is enabled these will be logged at the minimum of the fast attitude rate and 1kHz
- SCHED_LOOP_RATE - since all the heavy lifting of attitude control is being done in a separate thread, SCHED_LOOP_RATE can be run at normal values - for instance 400Hz or even less.
As part of this support I have written a lua script that allows you to switch in-flight between two sets of rates and tunes - switch_rates.lua. This can be packaged in the firmware of flash log based flight controllers, so I will include this support in firmware builds that I do. Set RTSW_ENABLE = 1 and RCx_OPTION = 300 for the RC channel that you want to use to switch between rates. The following parameters will be switched by the RC function.
ATC_ACCEL_P_MAX
ATC_ACCEL_R_MAX
ATC_ACCEL_Y_MAX
ATC_ANG_PIT_P
ATC_ANG_RLL_P
ATC_ANG_YAW_P
ATC_RAT_PIT_P
ATC_RAT_PIT_I
ATC_RAT_PIT_D
ATC_RAT_PIT_D_FF
ATC_RAT_PIT_FLTD
ATC_RAT_RLL_P
ATC_RAT_RLL_I
ATC_RAT_RLL_D
ATC_RAT_RLL_D_FF
ATC_RAT_RLL_FLTD
ATC_RAT_YAW_P
ATC_RAT_YAW_I
ATC_RAT_YAW_D
ATC_RAT_YAW_D_FF
ATC_RAT_YAW_FLTD
ATC_THR_G_BOOST
ACRO_RP_RATE_TC
ACRO_Y_RATE_TC
FSTRATE_ENABLE
FSTRATE_DIV
MOT_SPIN_MIN
MOT_SPIN_MAX
MOT_THST_EXPO
SERVO_DSHOT_RATE
Initially you can just load a standard tune into both positions, but after you have done this enable the fast rate attitude in one and try setting up a different tune. Note that it is not possible to change SCHED_LOOP_RATE dynamically so if you want a attitude rate different to SCHED_LOOP_RATE you will need to use FSTRATE_DIV instead.
Let me know in the comments what flight controllers you want to try this on and I will do builds.
PR is here:
Firmware is available here:
Current builds:
- MambaH743v4 (compiled with -O2)
- MatekH743-bdshot (running at 480mHz)
- Pixhawk6X
- Pixhawk6C
- CUAVv5Nano
I have done a stats version of each of the builds so that people can inspect the CPU load. I do not recommend you fly with this version. CPU profile information can be seen via @SYS/threads.txt in mavproxy or Mission Planner
Lua rate switching applet is here - ardupilot/libraries/AP_Scripting/applets/switch_rates.lua at pr-fast-rate-thread · andyp1per/ardupilot · GitHub and docs are here - ardupilot/libraries/AP_Scripting/applets/switch_rates.md at pr-fast-rate-thread · andyp1per/ardupilot · GitHub