Run rate controller at full gyro rate

[night-ghost]

I measured execution time of calls within Copter::fast_loop(), results (revomini, 1kHz, ekf3, armed, mean on 10s interval, in uS):

ins.update(); 9.83
attitude_control->rate_controller_run(); 13.02
motors_output(); 49.16
(***)
read_AHRS(); 217.37
read_inertia(); 15.66
check_ekf_reset(); 4.45
update_flight_mode(); 14.34
update_home_from_EKF(); 1.67
update_land_and_crash_detectors(); 6.79
camera_mount.update_fast(); 0.36
Log_Sensor_Health(); 1.75

Here we see that a time from gyro data to PWM output caused by only 3 first calls and is only ~75uS or near 1/4 of full fast_loop() time.

So if to separate first calls and do rate_controller and motors_output on each gyro sample, it will not cause loop saturation even on 8KHz. Frequency of remaining part is not so important (8bit APM was run at 50Hz), moreover accel samples are comes at less rate.

Fast reaction to gyro will allow to compensate propwash without high CPU load and achieve a high quality of stabilization even at low loop frequency

Platform

[ ] All
[ ] AntennaTracker
[X] Copter
[ ] Plane
[ ] Rover
[ ] Submarine

[auturgy]

Thanks for this!
I’ve pinged Leonard and Randy outside GitHub to have a look, as I think it’s awesome that you (a) have the depth of knowledge do this; and (b) chose to share it.

[rmackay9]

I think this is in @lthall's plan. I know last week Leonard and Tridge tested increasing the main loop rate to 1khz and it flew fine.

[night-ghost]

I know last week Leonard and Tridge tested increasing the main loop rate to 1khz and it flew fine.

I fly on 1KHz for 2 month, and it was I who suggested it to Tridge some days ago :)

But for the further growth of the frequency you can't make the heavy ekf run at full speed - moreover it is not necessary. I offer to divide the control loops, then the EKF can be left at the current frequency of 400Hz, and the angular rate controller, due to its simplicity, can and should operate at full gyro speed.

[lthall]

Hi @night-ghost ,

Yes, this has been on the plan for well over a year now, maybe 2 years. Tridge and I have been planning this with the ChibiOS update as it is so much more efficient. It was good to see your work up and running that clearly showed that our plan was sound!

I have the PID loop design done that will let us run just the rate loops at a higher rate or the whole attitude controller. This properly deals with the lower update rate from the pilot and from the EKF.

In my proposed design both the update rate for the rate loops and the EKF rate are completly flexible to deal with the various platforms. We can lock the rate loop to a particular gyro read if the hardware supports it or run it independently.

My goal is to get rate loops synced to the gyro reads at 2kHz (moving average of 4 samples) and maybe higher.

[night-ghost]

My goal is to get rate loops synced to the gyro reads at 2kHz (moving average of 4 samples) and maybe higher.

This issue about the fact that attitude control loop (and all others more slow control loops) can run on different (low) speed than angular rate controller.

Let's see to Betaflight in acro mode. There is only angle rate controller wich controlled by RC directly. Does pilot has the same control loop frequency? - sure no.

Now we give Betafligt and add a 2nd FC which emulates pilot and controls Betaflight. Should 2nd FC have the same loop frequency? - sure no.

Now we remove Gyro&Accel from 2nd FC and replace them by data stream from Betafligt. Yes now we CAN to have the same loop rate, but havn't to. We just want to rescale input stream to loop rate of 2nd FC.

What we got now? Mavic pro :) It has a small and cheap board with CPU and gyro&accel called "flight controller", and huge board with lots of CPUs called "main board", wihch takes gyro&accel data from FC and controls it.

And now I offer the same logick in single FC when different control loops can run at different speed. But the diagram in your post shows that ALL parts have the same loop rate...

And the simplest way - as described in 1st post, to run almost independent angular rate controller at high speed when all another parts uses rescaled sensor data and runs at (for example) 200Hz

[khancyr]

Interesting ! Could you share how you measured execution time of calls ? I would like to test your solution on current pixhawk code !

[lthall]

@night-ghost

Sorry you have not understood my diagram. The new Rate loop concept is in the box at the bottom. It replaces the existing rate loop in the system above. You can see the system above is running at 400 Hz but the new rate loop box runs anywhere from the attitude controller rate (400Hz in the diagram) to the gyro read rate.

So I am sure you will be happy to know we are on exactly the same page. I have been slowly working towards this goal for well over a year and we are almost there.

[night-ghost]

Sorry you have not understood my diagram.

Sure, sorry. But box at bottom is not labelled and not described its place on upper diagram.

I have been slowly working towards this goal for well over a year and we are almost there.

Cool, thanks. BTW, now I tries to connect Betafligt as slave FC to ArduCopter as master FC to move angle rate control to real Betaflight (which can run at 32K), but still don't know how to get suitable outputs from Arducopter. I need 4 floats that represents desired angular acceleration and throttle level, or just the same on PWM outputs. Could you consult me?

Could you share how you measured execution time of calls ?

see ArduCopter.cpp, Copter::fast_loop() in my fork, branch RevoMini (full-featured branch). But I havn't working solution yet because now INS can't supply data to different rate consumers.

[lthall]

Sure, sorry. But box at bottom is not labelled and not described its place on upper diagram.

Yeh sorry about that. This is the first time I published the diagram. It is from my personal notes and plans. It has my latest concept in there but I didn't take the time to make sure it was self explanatory.

but still don't know how to get suitable outputs from Arducopter.

It depends what you want. If you want to use Betaflight to control angle and rate then you will need to send _attitude_target_euler_angle from the AC_AttitudeControl via pwm to the BetaFlight board. If you want ArduCopter to to angle stabilization and BetaFlight to do rate control then you should use _attitude_target_ang_vel.

But I haven't working solution yet because now INS can't supply data to different rate consumers.

Yes, it has taken a long time to get the pieces in place but we are almost ready to do that. So I hope you will not have that issue very soon and you will be able to make the most out of your RevoMini branch with minimal additional work.

I understand that to run it faster than 1kHz we need to run the rate controllers in a different thread because it doesn't leave enough time for the scheduler to run the other tasks. I am not a coder myself (I couldn't write a "hello world" in C) so forgive me if I have not used the correct terminology there or have misstated something.

[night-ghost]

ArduCopter to to angle stabilization and BetaFlight to do rate control then you should use _attitude_target_ang_vel.

Thanks, I already found this in code, but was not sure. Throttle I should get in AC_AttitudeControl_Multi::set_throttle_out() before call to _motors.set_throttle(throttle_in), yeah?

we are almost ready to do that

Glad to know, thanks.

[lthall]

all to _motors.set_throttle(throttle_in), yeah?

Sorry I forgot about throttle.

So if you are running with only the rate controllers on BetaFlight then you will probably need to use the variable that is passed here:
_motors.set_throttle(throttle_in);
in
void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)

If you are using their attitude controller and it has its own angle boost then you will need the variable on the second line of that function:
_throttle_in = throttle_in;

[magicrub]

@khancyr Here's the source of the timing. It's a bunch of micros() timestamp diffs after each function call in the main loop. Simple and effective.
https://github.com/night-ghost/ardupilot/blob/RevoMini/ArduCopter/ArduCopter.cpp#L287

[dgrat]

@lthall
I think it would be nice to publish this diagram in the forum. Unfortunately, I am still not really sure about the bottom part of the diagram (slew?, signal sources?). Can you make a diagram showing the complete new (PID) loop structure. Are there more drafts or just discussions with Tridge?

To the new PID: Indicating the blocks are loops, why is the loop reading the gyro inside another loop? If it is meant that both loops are concurrent, the inner block should be outide the outer block (but in such a scenario there is aliasing). If not, there shouldn't be an inner loop at all and the sensros are just read with the speed of the outer loop (otherwise: how can the inner loop run faster than the outer loop?)?! Did I miss something?

Then I wonder about the frequency range. For simplicity we may just write there the aimed best target speed, instead of the ranges which somtimes overlap with the current implementation.

Edit: looks a bit like LabView FPGA stuff :)

[lthall]

Hi dgrat,

This is all my work based on my experience with racing aircraft and helping tune aircraft from 100g to >100kg.

This diagram is my notes rather than a publish ready diagram. I would need to spend a few hours getting it ready to publish properly.

To answer you question. The rate loops are only run up to the speed of the gyro read but depending on the board may need to be run much slower or not locked to the gyro read or based on the moving average of multiple sensors.

The additional block you refer to are there to address the issues with the rate loops being run faster than the attitude controller or the pilot inputs. As you suggest aliasing is an issue and must be addressed by careful filter design and controller architecture. When the rate loops are run slower than the gyro reads this is primarily achieved using the low pass filter on the gyro output.

[R-Lefebvre]

Leonard, please allow Rate loops to run at common helicopter frequencies:

50Hz lowest (analog servos), 125Hz, 250Hz, 333Hz.

The last one will likely be most common with fast digital PWM servos. I think SBUS doesn't like to run faster than 250Hz, so that will be another common one.

I think you can tie these outputs based on the Servo Rate or the SBUS rate, both of which are already parameters.

[amilcarlucas]

How about CAN based ESCs ? Those are can be run at 1KHz or something like that, right ?

[BloodSakura3774]

Any news on this topic?

[andyp1per]

Will be fixed by #27029