Next generation rate control

[andyp1per]

This is a brief write-up of what I think we should do for rate control, primarily in copter but affecting other vehicles as well. There are a number of seemingly competing ideas floating about, but I think that there is a happy path for these where everyone gets what they want (and perhaps more).

Requirements:

• Low latency rate control - this means both that the time between gyro sample and action is low and that the update rate of the rate controller is high
• High device (IMU) sample rates and corresponding filter updates - running the filters at a high rate (which is only possible with a high device sample rate) is important for preventing aliasing in the noise profile.
• Low CPU usage - faster usually comes at a cost. We don't want to compromise users with slower flight controllers.
• Low/no configuration, simple upgrade

Rather than delve into the theoretical details, I'll simply put my proposal here:

1. Run FIFO read rates at the loop rate. This is theoretically as fast as the FIFO reads need to occur as long as the loop rate is synchronised to the availability of data. It produces the minimum latency for a given loop rate assuming that the device read rate is kept high. This is generally what @bugobliterator is trying to do in Do not Read FIFO faster than requested rate for ICM45686 #27573 with some modifications
2. Change wait_for_sample() to synchronize on the primary IMU. Currently we wait for all IMUs to provide data and since we have no control over the order in which these appear we could currently synchronize on the non-primary meaning that latency in the worst case with multiple IMUs is a whole loop cycle.
3. Implement rate control in a separate thread on copter and make it the default. This allows rate updates to occur at a rate faster than the main loop without the additional overhead of EKF updates etc. This is what Copter: run rate loop at full filter rate in its own thread #27029 does.
4. Allow the rate thread to run the FIFO read rate at a higher rate than the main loop. If we don't do this then the rate thread will not get any latency benefit. The FIFO read rate should be the same as the rate thread rate and the rate thread should therefore be able to control this. This is the inverse of what Dynamic FIFO Buffering #27841 does. That PR slows down unused IMUs, but we now want the opposite - we want to speed up the primary IMU since they are all running at the loop rate by default.
5. Provide a configuration option to allow users to control the relationship between the device rate and the rate loop rate. Copter: run rate loop at full filter rate in its own thread #27029 already does this via FSTRATE_DIV, the only additional work is to make sure that this also sets the FIFO read rate

A couple of things to note:

• For existing users no configuration changes are required. The only new required configuration is enabling the rate loop thread.
• Dynamic FIFO is really a safe change - all we are doing is changing the period on FIFO reads which we already do and if it is not safe then any slowdown here is not a safe change. There are no changes to the device settings or read logic for IMUs, the only thing we are doing is modifying the read period.

I think this really takes us into new territory where we are carefully managing the gyro reads, rate update latency and CPU cost in a way that benefits everyone.

[tridge]

@andyp1per note that the rate loop rate and the rate at which IMU samples go into the sensors FIFO (the sensors ODR) are rarely an integer multiple of each other. This means if you want the rate loop period to be constant then you cannot synchronise on the IMU ODR.
A simple example is a sensor providing data into the FIFO at 1kHz, but main loop rate is 400Hz. You will sometimes have 2 samples and sometimes 3. The EKF is aware of this via the delta_angle/delta_velocity APIs which provide the time delta of each sample, but the rate loop is not aware of this, so is imposing an artificial delta time with a odd/even style of latency change. This is complicated by the filtering which means that a single filtered sample is composed of data from multiple periods. Each notch filter adds a delay.
So are you proposing to have a variable (odd/even) rate loop period? If not, then what does it actually mean to "synchronise" two things which are not running at an integer multiple rate?
We could choose an integer multiple and force the rate loop to run at a rate that has a period that is a multiple of the sensor ODR, but that will have odd flow-on effects. For example, a copter configured for 400Hz with a 1000Hz ODR IMU would have to choose 333Hz or 500Hz, and may have to change main loop rate by a large amount on an EKF lane change.

[lthall]

@andyp1per note that the rate loop rate and the rate at which IMU samples go into the sensors FIFO (the sensors ODR) are rarely an integer multiple of each other.

But if we are planning on putting the rate loop in a separate thread we can make it an integer multiple. If we want the best rate loop performance this is exactly what we should do. The rate loop should be run as soon as possible after the IMU gyro read is done.

If we change IMU's that we are using and they are operating on a different update rate then we will need to change the dt but that is something that isn't hard to deal with.

The statements above all assume we can output at significantly higher rates than 400 Hz such that we can achieve minimum latency and minimise dt jitter.

If our update rate is limited by our ability to communicate with the actuators as it is with PWM (nominally 400 Hz), then we are no longer able to operate at a multiple of our gyro sample rate if it is 1kHz. In this case we must synchronise our rate loop with our ability to output the new actuator setting. We need to make assumptions as we do now and that inconsistent timing is treated as timing jitter.

In the case where our IMU reads are done at 800 Hz or 2 kHz we could synchronise our rate loops to run immediately after the sample is read and passed through the filters. This would result in minimal additional latency.

[andyp1per]

note that the rate loop rate and the rate at which IMU samples go into the sensors FIFO (the sensors ODR) are rarely an integer multiple of each other. This means if you want the rate loop period to be constant then you cannot synchronise on the IMU ODR.
A simple example is a sensor providing data into the FIFO at 1kHz, but main loop rate is 400Hz. You will sometimes have 2 samples and sometimes 3. The EKF is aware of this via the delta_angle/delta_velocity APIs which provide the time delta of each sample, but the rate loop is not aware of this, so is imposing an artificial delta time with a odd/even style of latency change. This is complicated by the filtering which means that a single filtered sample is composed of data from multiple periods. Each notch filter adds a delay.
So are you proposing to have a variable (odd/even) rate loop period? If not, then what does it actually mean to "synchronise" two things which are not running at an integer multiple rate?
We could choose an integer multiple and force the rate loop to run at a rate that has a period that is a multiple of the sensor ODR, but that will have odd flow-on effects. For example, a copter configured for 400Hz with a 1000Hz ODR IMU would have to choose 333Hz or 500Hz, and may have to change main loop rate by a large amount on an EKF lane change.

All of the schemes that involve slowing the FIFO read rate make the assumption that the ODR rate is the sample rate as far as the EKF is concerned. So as you know we are not measuring intervals we are using a mostly fixed dt - this means that whether we read 2 or 3 samples does not matter as far as the EKF is concerned, the dt is still fixed between them and the EKF is happy. Note that I am making the assumption that the device sample rate is high (see my second requirement above) and thus the issue of non-integer multiples is greatly reduced because the ODR is >> 1Khz. Even when you have 2 or 3 samples in a read, what you care about is the latency between the latest sample that you read and the action, it doesn't really matter how many samples preceed that (I mean it does because that's the reason for running a high rate rate thread, but for the slower case it doesn't really matter) but it does matter how long you delay after reading. The synchronization I am talking about is between the main loop and the FIFO read, so I am proposing that (for instance) both are run at 400Hz. What you are trying to ensure is that each time the main loop ticks it has a FIFO read that is as fresh as possible so that the latency is at worst 1/ODR.

[tridge]

All of the schemes that involve slowing the FIFO read rate make the assumption that the ODR rate is the sample rate as far as the EKF is concerned. So as you know we are not measuring intervals we are using a mostly fixed dt - this means that whether we read 2 or 3 samples does not matter as far as the EKF is concerned, the dt is still fixed between them and the EKF is happy

The EKF is already happy. The EKF is designed to handle variable dt, the dt the EKF uses is summed based on the number of actual ODR samples used in an update. The EKF is perfectly happy with this varying between updates. That is why the delta_angle and delta_velocity APIs return a delta_time for each request. That delta_time is the sum of the deltas for each ODR sample.
What I'm trying to point out is that while the EKF can handle variable dt between loops, the rate controller doesn't have a mechanism to handle that. So truly synchronising to the ODR data, which is what true synchronising to the actual hardware sample would be, is not possible with the current rate controller design.

Note that I am making the assumption that the device sample rate is high (see my second requirement above) and thus the issue of non-integer multiples is greatly reduced because the ODR is >> 1Khz

we also need to cope with slower IMUs. For example, the popular BMI270 runs at approximately 1600Hz (and actually varies quite a bit from that).

[lthall]

I think we are getting crossed wires on what the term "synchronising" means. Everybody is on the same page that the EKF is designed to handle variable dt properly.

From the perspective of the rate loops we know that you will always have timing jitter because of the non-integer relationship between the ODR sample rate and the and rate loop rate. Understanding that we can only do so much with the current system we want to minimise latency given the limitations of this system.

The timing inconsistency due to the variable dt can be represented as noise from an engineering perspective. So we do have a mathematical way of handling / representing this. We do want to minimise it of course.

Average latency is going to be approximately half the rate loop period + rate loop processing time and signalling delay + the average delay after the ODR sample is taken to the time the rate loop is run.

What Andy is trying to do is minimise the last part of that latency equation. If we can minimise the time delay between the last ODR read and the output of the rate loop we will be maximising the potential of this system.

The rate loop doesn't care when the first few samples are read and processed, just the last sample before it runs. This would suggest that the lowest latency would be achieved by processing and emptying the FIFO and computing the filters just before we are due to run the rate loop so that we can process a single sample then run the rate loop with the lowest possible additional time delay.