Do not Read FIFO faster than requested rate for ICM45686 #27573

bugobliterator · 2024-07-17T04:07:09Z

TLDR; this PR allows users to be able to better control how fast we read ICM45686, previously we were only limited to 3.2KHz and 6.4KHz. With this PR 0.8, 1.6, 3.2 and 6.4 KHz will be achievable. And for all boards except CubeRedPrimary, the default Sensor read (FIFO read) rate will be 1.6KHz, which was previously 3.2KHz.

Additional Notes:

We seem to be always setting ICM45686 backend loop rates atleast at 3.2KHz when fast_sampling was requested. And at 1.6KHz when it isn't.
This PR modifies the rate at which we read from FIFO depending on requested gyro rates. We still sample at 3.2KHz or 6.4 KHz depending on requested backend loop rates, we just read from FIFO based on what read rate is requested.

When fast sampling is not requested we read FIFO at 0.8KHz

Currently there are 6 flight boards that are using ICM45686, CubeOrangePlus, CubeRed, Here4FC, Pixhawk6X and ZeroOneX6. As it stands in master all 6 boards are polling the sensor 3.2KHz.

There is a hard limitation in current master which doesn't even allow users to set the backend rate to value lower than 3.2KHz. Basically even if you had fast sampling turned off the backend rate will be stuck at 3.2KHz.

With this PR following configurations are achievable:

On CubeOrangePlus, Here4FC, Pixhawk 6X and ZeroOneX6, this PR will change the polling rate from 3.2KHz to more reasonable value of 1.6KHz as the default (As all of these boards have FAST_SAMPLING turned on for all IMUs by default). The sample rate on the IMU still remains as 3.2KHz though
If user needs to have faster polling, they can set INS_GYRO_RATE parameters to get polling rate of 3.2 and 6.4.

On CubeRedPrimary, this will put the primary IMU at polling rate of 1.6KHz and other two at 800Hz. This is because default INS_FAST_SAMPLE mask is set to 1.

As a means of safety this PR will ensure that current users setup don't break in case they are using higher SCHED_LOOP_RATE values #28672

The PR also adds support for showing both the Loop Rate and Sample Rate of the IMU for InvensenseV3. This signifies the difference between the two values. We have been displaying these values for Invensensv2 and v1 as well. We did it by the means of two values separated by / like so :

        snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz",
            gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate * 0.001, _gyro_backend_rate_hz * 0.001);

Flight Logs:

CubeOrangePlus LR 0.8 / SR 3.2 : https://droneshare.cubepilot.com/#/s/log?b=1721188076568&c=5a75a9ee-43ef-11ef-a740-b360934a095f&d=CubeOrangePlus+LR+0.8+%2F+SR+3.2&e=2&a=cbfcd2fc-3fa9-11ed-b631-b97a8244eade

CubeOrangePlus LR 1.6 / SR 3.2 : https://droneshare.cubepilot.com/#/s/log?b=1721188057353&c=4f01d6a9-43ef-11ef-a740-7fca103c5c87&d=CubeOrangePlus+LR+1.6+%2F+SR+3.2&e=2&a=cbfcd2fc-3fa9-11ed-b631-b97a8244eade

CubeOrangePlus LR 3.2/ SR 3.2 : https://droneshare.cubepilot.com/#/s/log?b=1721187986195&c=2497d6a7-43ef-11ef-a740-11b81cb1ae46&d=CubeOrangePlus+LR+3.2%2F+SR+3.2&e=2&a=cbfcd2fc-3fa9-11ed-b631-b97a8244eade

tridge · 2024-07-17T08:30:38Z

@andyp1per need your review on this one

andyp1per

I think this is probably a reasonable direction to go in, but needs to be done in a way that people's existing tunes and setups are not compromised.

andyp1per · 2024-07-18T15:19:33Z

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h

@@ -140,4 +140,5 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend

    float temp_filtered;
    LowPassFilter2pFloat temp_filter;
+    uint32_t sampling_rate_hz;


Where is this used other than in the boot message?

Yes, that's it. Its used to convey that backend rate is different than the actual sampling rate. We do that for other Invensense drivers which do the same

But you can just calculate the value as backend_rate * gyro rate? Don't think you need to hold a variable anywhere

This is what v1 does, why not just do this? These two should be consistent, so you should change both or neither, my preference is to remove the variable and just calculate the value on the fly - makes the code much simpler:

snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz", gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate * 0.001, _gyro_backend_rate_hz * 0.001);

andyp1per · 2024-07-18T15:22:25Z

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

-    // always fast sampling
-    fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
+    uint8_t odr_config;
+    backend_rate_hz = 800;


Problem is this changes the backend rate for all boards using 45686, not just the CubeRed. I suspect it will also change people's tunes, although I haven't fully got my head around what this is actually changing other than lowering the backend rate (which is the filter rate and thus changing people's tunes). I think you are going to need some way of keeping current setups as they are while changing the default for new setups.

This is not changing the sampling rate though, the samples are still getting consumed at the same rate as before i.e. 3.2KHz. We just flush the data from IMU FIFO at rate slower than 3.2KHz.

We still pass each and every sample at 3.2KHz sampling rate through to the Notchfilter, so I don't think there should be any impact to the results of the NotchFilter, or anything that gets called in notify(gyro/accel) call. We already have code under notify to handle bunched up FIFO samples and calculating delta time between samples.

The problem is that you have increased the latency, and its the latency that matters for control - there are two benefits to faster rates - filtering at higher rates which benefits aliasing and lower latency is response which benefits control. Your change means we now only get the first.

Also you asserted that this only affect non-fast sampling in the call but it affects fast sampling as well

@andyp1per what do you mean by latency here? The filtered gyro should be identical in both cases, exactly the same samples are going through exactly the same filters, it is just batching them together

andyp1per · 2024-07-18T15:23:40Z

libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat

 SPIDEV ms5611_1         SPI4 DEVID2  BARO_1_CS          MODE3 20*MHZ 20*MHZ

 IMU Invensensev3 SPI:icm42688_1 ROTATION_YAW_90
 IMU Invensensev3 SPI:icm42688_0 ROTATION_PITCH_180_YAW_270
 IMU Invensensev2 SPI:icm20649 ROTATION_PITCH_180
 IMU Invensensev3 SPI:icm45686 ROTATION_YAW_180

+define HAL_INS_HIGHRES_SAMPLE 5


For variety

tridge · 2024-07-24T07:34:43Z

need to defer till @andyp1per can flight test this

andyp1per

I think what this PR is achieving is reducing the default non fast sample rate from 1600 to 800 and the default fast sampling rate on H7 from 3.2Khz to 1.6Khz. I think the PR should just do that - the renaming and message changes are confusing and not helping IMO. I also think there is a bug in the rate setting that is hard to see with the other changes. In general I agree that changing the range from 1.6Khz->6.4Khz to 800Hz->6.4Khz is a sensible one and ties in better with our existing range of 1Khz->8Khz, but it will affect the latency of samples for existing users. That's probably ok as part of the 4.6 cycle, but probably not ok as a 4.5 backport.

andyp1per · 2024-08-19T15:20:53Z

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

-    if (fast_sampling) {
-        snprintf(banner, banner_len, "IMU%u: fast%s sampling enabled %.1fkHz",
-            gyro_instance,
+    snprintf(banner, banner_len, "IMU%u:%s%s sampling LR:%.1fkHz SR:%.1fkHz",


Not sure LR/SR is descriptive enough _ I certainly don't know what they mean. The rate that matters is the one that the filters are run at.

Why not just go with what I already did for Invensense v1? It just shows SR/LRHz

andyp1per · 2024-08-19T15:24:35Z

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

-    // always fast sampling
-    fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
+    uint8_t odr_config;
+    backend_rate_hz = 800;


The problem is that you have increased the latency, and its the latency that matters for control - there are two benefits to faster rates - filtering at higher rates which benefits aliasing and lower latency is response which benefits control. Your change means we now only get the first.

andyp1per · 2024-08-19T15:33:15Z

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

-    backend_rate_hz = 1600;
-    // always fast sampling
-    fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
+    uint8_t odr_config;


Where is the 800Hz value set in the ODR?

andyp1per · 2024-08-19T15:40:35Z

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

@@ -649,21 +648,21 @@ void AP_InertialSensor_Invensensev3::register_write_bank(uint8_t bank, uint8_t r
 }

 // calculate the fast sampling backend rate
-uint16_t AP_InertialSensor_Invensensev3::calculate_fast_sampling_backend_rate(uint16_t base_odr, uint16_t max_odr) const
+uint16_t AP_InertialSensor_Invensensev3::calculate_fast_sampling_backend_rate(uint16_t base_backend_rate, uint16_t max_backend_rate) const


This is just a rename, I think you should remove this from the PR to make review a little easier

tridge · 2024-08-28T07:57:36Z

we really need to know why the CPU cost is so high. @bugobliterator will try to do some profile to see why that is

andyp1per

None of my comments have been addressed and it does affect fast sampling

andyp1per · 2024-08-31T12:37:34Z

libraries/AP_InertialSensor/AP_InertialSensor.cpp

-    // @DisplayName: Gyro rate for IMUs with Fast Sampling enabled
-    // @Description: Gyro rate for IMUs with fast sampling enabled. The gyro rate is the sample rate at which the IMU filters operate and needs to be at least double the maximum filter frequency. If the sensor does not support the selected rate the next highest supported rate will be used. For IMUs which do not support fast sampling this setting is ignored and the default gyro rate of 1Khz is used.
+    // @DisplayName: Gyro rate multiplier for IMUs with Fast Sampling enabled
+    // @Description: Gyro rate multipier for IMUs with fast sampling enabled. The gyro rate is the sample rate multiplier of base output data rate at which the IMU filters operate and needs to be at least double the maximum filter frequency. If the sensor does not support the selected rate the next highest supported rate will be used. For IMUs which do not support fast sampling this setting is ignored and the default gyro rate is used.


I think you should provide a typical value - so "e.g. 1Khz on an ICM-42688"

I don't think "sample rate multiplier of base output data rate" is correct - this is currently the output data rate (as opposed to the sensor rate which happens on chip).

andyp1per · 2024-08-31T12:41:26Z

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h

@@ -140,4 +140,5 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend

    float temp_filtered;
    LowPassFilter2pFloat temp_filter;
+    uint32_t sampling_rate_hz;


But you can just calculate the value as backend_rate * gyro rate? Don't think you need to hold a variable anywhere

andyp1per · 2024-08-31T12:42:48Z

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

-    // always fast sampling
-    fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
+    uint8_t odr_config;
+    backend_rate_hz = 800;


Also you asserted that this only affect non-fast sampling in the call but it affects fast sampling as well

andyp1per · 2024-08-31T12:42:58Z

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

+    if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() && fast_sampling) {
+        // For ICM45686 we only set 3200 or 6400Hz as sampling rates
+        // we don't need to read FIFO faster than requested rate
+        backend_rate_hz = calculate_fast_sampling_backend_rate(800, 6400);


Changes fast sampling backend rate

andyp1per · 2024-08-31T12:51:47Z

I really don't think this is the right way to do this. To get what you want on cube's I think you should introduce a new parameter say "INS_GYRO_FO_RATE" which allows you to control the backend rate independently from the sample rate. You could even call it "INS_GYRO_DIV_RTE" with a default of 1 which represents the FIFO rate divider. You can then set this as high as you like on Cube's but also allows you to do some kind of upgrade path so as to not affect current users. The its not one size fits all and both the current behaviour and what you want can be supported.

Even better you could resurrect #27841 and then for the non-primary set the FIFO rate really low. You would then get the best of all worlds - low latency on the primary gyro where it matters and low CPU on the backups. The EKF won't care, its only the rate and attitude controllers that care about the latency.

andyp1per · 2024-09-05T14:58:22Z

@bugobliterator I have done a demo in #27841 of what I mean. I think this is a much better way and means everyone gets a CPU improvement without anyone being penalized by higher control latency. I think it also means that all Cube's benefit rather than just the ones with only this one sensor.

tridge · 2024-09-18T09:12:32Z

@andyp1per I really don't understand what you mean by latency in your comments. Can you explain it in terms of what happens on a 400Hz copter? As far as I can see this PR just changes the batching of the FIFO reads, so we do less SPI transactions when the loop rate doesn't need the transactions at the higher rate. I can't see any sort of latency that impacts for a vehicle not using the rate thread

andyp1per · 2024-11-19T17:49:23Z

libraries/AP_InertialSensor/AP_InertialSensor.cpp

-    // @DisplayName: Gyro rate for IMUs with Fast Sampling enabled
-    // @Description: Gyro rate for IMUs with fast sampling enabled. The gyro rate is the sample rate at which the IMU filters operate and needs to be at least double the maximum filter frequency. If the sensor does not support the selected rate the next highest supported rate will be used. For IMUs which do not support fast sampling this setting is ignored and the default gyro rate of 1Khz is used.
+    // @DisplayName: Gyro rate multiplier for IMUs with Fast Sampling enabled
+    // @Description: Gyro rate multipier for IMUs with fast sampling enabled. The gyro rate is the sample rate multiplier of base output data rate at which the IMU filters operate and needs to be at least double the maximum filter frequency. If the sensor does not support the selected rate the next highest supported rate will be used. For IMUs which do not support fast sampling this setting is ignored and the default gyro rate is used.


I don't think "sample rate multiplier of base output data rate" is correct - this is currently the output data rate (as opposed to the sensor rate which happens on chip).

andyp1per · 2024-11-19T17:50:40Z

libraries/AP_InertialSensor/AP_InertialSensor.cpp

    // @User: Advanced
-    // @Values: 0:1kHz,1:2kHz,2:4kHz,3:8kHz
+    // @Values: 0:x1,1:x2,2:x4,3:x8


This is removing useful information - we need to at least give the user an idea of what this might look like. Specifying the rates for common sensors would fix this

andyp1per · 2024-11-19T21:41:20Z

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

-    if (fast_sampling) {
-        snprintf(banner, banner_len, "IMU%u: fast%s sampling enabled %.1fkHz",
-            gyro_instance,
+    snprintf(banner, banner_len, "IMU%u:%s%s sampling LR:%.1fkHz SR:%.1fkHz",


Why not just go with what I already did for Invensense v1? It just shows SR/LRHz

andyp1per · 2024-11-19T21:49:31Z

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h

@@ -140,4 +140,5 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend

    float temp_filtered;
    LowPassFilter2pFloat temp_filter;
+    uint32_t sampling_rate_hz;


This is what v1 does, why not just do this? These two should be consistent, so you should change both or neither, my preference is to remove the variable and just calculate the value on the fly - makes the code much simpler:

snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz", gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate * 0.001, _gyro_backend_rate_hz * 0.001);

andyp1per · 2024-11-19T23:24:23Z

@bugobliterator I have done a simpler alternative here - #28682
I think this gives you what you want because on CubeRed/CubeOrange the H7 means that the gyro rate is set to 1 which means that you get 2x the backend rate. Since the lower bound is now 800Hz this means that your backend rate is 1600Hz which is the same as in your PR (so you get the CPU benefit you are looking for). It does still keep the backend and ODR rates the same (so ODR is 1600Hz in this example), but I would prefer to keep it that way for the backport to 4.6 and then address the backend/ODR split more generically via either a new config parameter or via dynamic FIFO rates (
I do agree that we should reduce the FIFO rate relative to the ODR rate in a way that makes sense, but we should do it for all sensors rather than ad-hoc using some arbitrary rules).

bugobliterator · 2024-11-20T06:13:47Z

Actually I don't think with your PR I get what I want. My goal is to keep the sampling rate same as before, and use FIFO to buffer sample. In your PR you are reducing the ODR of the sensor, It will give more CPU time sure, but at a severe cost of low sampling rate.

I also don't believe I am applying any arbitrary rule, in fact I am making the driver more consistent with Invensensev1 and v2. Splitting the notion of backend rate and sampling rate is not novel we have been doing it for both of the remaining two versions of Invensense sensors.

Regarding the remaining sensors supported by Invensensev3, As far as I am aware they all are limited to 1600Hz max ODR. So, it sort of made sense for doing one sample per loop in this driver to begin with. But for ICM45686 it doesn't make sense to sample one sample per loop at 3.2KHz, FIFO buffering is a better alternative here, not reducing the ODR.

…M45686

bugobliterator requested review from tridge, tpwrules and andyp1per and removed request for tpwrules July 17, 2024 04:07

bugobliterator force-pushed the pr-icm45686-loop-rate branch from b251b5d to e80cbd8 Compare July 17, 2024 04:07

bugobliterator added the DevCallEU label Jul 17, 2024

tridge removed the DevCallEU label Jul 17, 2024

tridge assigned andyp1per Jul 17, 2024

andyp1per requested changes Jul 18, 2024

View reviewed changes

andyp1per reviewed Jul 18, 2024

View reviewed changes

bugobliterator added the DevCallEU label Jul 24, 2024

tridge removed the DevCallEU label Jul 24, 2024

bugobliterator added the DevCallEU label Aug 14, 2024

andyp1per requested changes Aug 19, 2024

View reviewed changes

tridge removed the DevCallEU label Aug 28, 2024

andyp1per requested changes Aug 31, 2024

View reviewed changes

andyp1per mentioned this pull request Sep 25, 2024

Next generation rate control #28230

Open

bugobliterator added the DevCallEU label Oct 15, 2024

tridge removed the DevCallEU label Nov 13, 2024

bugobliterator force-pushed the pr-icm45686-loop-rate branch 2 times, most recently from 801aae0 to 93fc998 Compare November 19, 2024 01:13

andyp1per mentioned this pull request Nov 19, 2024

AP_InertialSensor: constrain ICM45686 to 800Hz-6.4KHz #28682

Open

andyp1per requested changes Nov 19, 2024

View reviewed changes

tridge mentioned this pull request Nov 20, 2024

ArduPilot 4.6 issues list #28612

Open

21 tasks

bugobliterator force-pushed the pr-icm45686-loop-rate branch 3 times, most recently from d7f9bd2 to 129d7e5 Compare November 22, 2024 00:33

AP_InertialSensor: do not read FIFO faster than requested rate for IC…

50db2a9

…M45686

bugobliterator force-pushed the pr-icm45686-loop-rate branch from 129d7e5 to 50db2a9 Compare December 9, 2024 10:10

bugobliterator added the DevCallEU label Dec 11, 2024

tridge removed the DevCallEU label Dec 11, 2024

Do not Read FIFO faster than requested rate for ICM45686 #27573

Are you sure you want to change the base?

Do not Read FIFO faster than requested rate for ICM45686 #27573

Conversation

bugobliterator commented Jul 17, 2024 • edited Loading

tridge commented Jul 17, 2024

andyp1per left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bugobliterator Jul 24, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bugobliterator Jul 24, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tridge commented Jul 24, 2024

andyp1per left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tridge commented Aug 28, 2024

andyp1per left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

andyp1per commented Aug 31, 2024

andyp1per commented Sep 5, 2024 • edited Loading

tridge commented Sep 18, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

andyp1per commented Nov 19, 2024

bugobliterator commented Nov 20, 2024 • edited Loading

bugobliterator commented Jul 17, 2024 •

edited

Loading

bugobliterator Jul 24, 2024 •

edited

Loading

bugobliterator Jul 24, 2024 •

edited

Loading

andyp1per commented Sep 5, 2024 •

edited

Loading

bugobliterator commented Nov 20, 2024 •

edited

Loading