Unlocking the Potential of Faster Attitude Rates in Copter Control

I am testing this with a 5" copter build on FlywooF745 now.

So far, I could make it stable with 2kHz gyro rate (INS_GYRO_RATE = 1) and 1kHz rate loop (FSTRATE_DIV = 2). Bigger gyro rates appear to be not really feasible in my setup.

With an attempt to fly a 2kHz loop, the copter flipped in a fraction of a second at an attempt to raise throttle just a little, and with no logs written at that time to save some CPU, I can not yet say what the reason was. I am inclined to make 1kHz work first.

I have just run an autotune, and here are the changes for pitch and roll:

  • ATC_ACCEL_P_MAX: 580825 => 657815
  • ATC_ACCEL_R_MAX: 660160 => 822474
  • all ATC_ANG_*_P were above 30 and remained there
  • ATC_RAT_PIT_P: 0.139 => 0.191
  • ATC_RAT_PIT_D: 0.00218 => 0.00265
  • ATC_RAT_RLL_P: 0.0742 => 0.1382
  • ATC_RAT_RLL_D: 0.00199 => 0.00192

All that with AUTOTUNE_AGGR = 0.08, I’ve lost track with what is the recommended value now, and some of the recommendations about low values appeared counter-productive for this machine in the past.

I am not including yaw here as I’ve changed some filtering, but the max acceleration increased by 10%.

Roll’s max acceleration of 822474 is truly impressive, never reached that much before.

I have a question though. If my INS_GYRO_FILTER, as well as FLTD and FLTT filters, are all at 100Hz, and increasing these significantly seems to increase noise and interfere with tuning, how exactly does increasing the attitude rate help? I seem to be missing something…

Generally you want:

  • INS_GYRO_FILTER - as high as you can get without cooking your motors. Often copters have resonance around 120Hz so it is hard to get higher than this without notching out frame resonance.
  • FLTD should be INS_GYRO_FILTER/2 on roll and pitch and INS_GYRO_FILTER/4 on yaw. This is because D is more active, lets more noise through and is more likely to cook your motors. I would not recommend setting it above 0.75*INS_GYRO_FILTER
  • FLTT should be at the highest rate you want the EKF or your fingers to control the copter. Values of 30 should be fine, higher has diminishing returns but does not affect the tune.

Increasing the attitude rate reduces the latency for the actual to track the target and the error. Note that it is the error term that is important as that can move much more quickly than the target.

1 Like

@andyp1per @Leonardthall
Andy provided logs containing Sys ID of the roll axis for me to analyze. The rate controller loop rates were 800hz and 4khz. It appears that they were both optimally tuned.

Here is a comparison of the frequency response of target rate (RATE.Rdes) to actual rate before filters (SIDD.Gx). Those were the parameters I used for the frequency response, but I am showing the target rate to the roll attitude in this plot. This is what is used to calculate the bandwidth for both phase and gain.

4khz:
Gain Bandwidth: 35.6 rad/s
Phase Bandwidth: 115.1 rad/s

800hz
Gain Bandwidth: 39.8 rad/s
Phase Bandwidth: 81.6 rad/s

The current practice used for manned handling qualities evaluation looks at the lower of the two bandwidths. In this case we gain bandwidth is the lowest bandwidth and would be considered the limiting factor for pilot in the loop flight. It is interesting, though that the lower loop rate has a higher gain bandwidth and the higher loop rate has a higher phase bandwidth.

I just wanted to get this out there. I think there is some benefit but it is mainly out a high frequency
above 15 hz.

1 Like

Here is the plot of the target rate to actual rate. You can see that the tune pushes the rigid body mode to have some overshoot. You can also see the 800hz rate loop (dashed line) has more phase loss at the higher frequencies.

I am not sure that Fast Attitude Rates are connected to this, but just in case…

@andyp1per, how likely it is to accidentally ignore or misread AHRS_TRIM values in some of the flights with these changes?

I have calibrated trims of my copter quite well, even to the degree of compiling firmware for my 1Mb board with IMU temperature compensation and performing calibration. In >50% of starts it behaves nicely, drifting only slowly when sticks are let go (for autotune). In some <50% of starts, it starts up to four degrees off. In this case, power cycling typically make the next flight nice.

I don’t remember this to happen before I started to experiment with fast attitude rates.

Can you provide a log? Sounds like the EKF is learning bad offsets

I will try later in the day. I’ve had yet another case of this behavior earlier today, but the log was overwritten by a subsequent too long flight.

Here are two logs with rather nice behavior, and one log with a difference of few degrees towards the end of the flight.

Though I’ve spotted some unusual noise though at a frequency just below the propellers’ RPM, and I wonder whether this is responsible in any way. “Unusual” means that in some previous tests with higher (!) filter settings there was no such clear peak…

Your EKF is learning bad gyro biases and I would say its temperature related:

Okay, so fastrate is out of question, and I should attempt a better TCAL, right?

Certainly better TCAL. It’s possible they are related because the MCU will be working harder and therefore getting hotter.

1 Like

Here are some updated results. I’ll confess that I made a mistake in my previous post regarding the phase bandwidth. I’ll correct that and add an run that @andyp1per performed with the 4k hz rate controller using the same tuning gains as the 800 hz rate controller. This will provide a better comparison of the response difference with loop rate.

4khz with the same tune as 800 hz:
Gain Bandwidth: 46.4 rad/s
Phase Bandwidth: 87.4 rad/s

800hz
Gain Bandwidth: 39.8 rad/s
Phase Bandwidth: 81.6 rad/s

This shows that for the same rate controller gains, the 4k hz loop rate would provide a better response both in gain and phase out to higher frequencies. Thus being about to push the aircraft harder which is shown by the ability to tune out to higher Rate P and Rate D gains for the optimal tune.

Here is a plot of the three for the target roll rate to actual roll rate.


4k loop rate with optimal gains - solid line
4k loop rate with 800hz loop rate gains - dashed line
800 hz loop rate - dash dot line

You will notice that the 4k hz loop rate gain trace matches the 800 hz loop rate. the benefit of the higher loop rate is shown in phase as there is less phase loss by the 4k hz loop rate. This demonstrates the reduction in time delay by 1.9 milliseconds. For smaller copters with high frequency dynamics, this shows a definite improvement. I don’t think this would be as big of a benefit for larger aircraft.

4 Likes

I got a crush dump when tuning it today, not sure if this is related to fstrate but you could take a look, I don’t have the log unfortunately.

This has the hallmarks of stack overflow. Can you get me a dump of @SYS/threads.txt from your copter when armed?

Yes, there is a lot stack overflow error and I have to reboot the fc after each disarm

2024/8/2 19:47:23 : PreArm: Internal errors 0x800000 l:182 stack_ovrfl

Ok the attitude stack clearly overflowed. I will up the size and redo the builds.

1 Like

ok new builds in the same place under v2. There have been a few small fixes as well since v1

I let my friends tried it. They both gave me identical answer: it could be used for long range and some light freestyle but still not good for intense flying.
Firstly they say the drone doesn’t feel locked in as bf, and the drone is still moving when releasing stick. I watched the log and did some manual tune: raised fltd to 75, gyro filter to 150 and this let the roll/pitch pid raised by another 50%, after that they say it does feel more locked in but still not as responsive as bf, feels like there is still a lot of lag. I didn’t let them try 400hz to compare with 4khz and I don’t know how to improve the response further at this point.

Post a log. Normally this will be to do with noise - if you haven’t taken out the frame resonance for those filter settings that will be reducing your maximum PIDs

1 Like

Flying your v2 build on a 3.5" with a matek h743. Flies great and the stick feel is much better for Betaflight pilots.

Can’t wait for this to hit main in 4.6

1 Like