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

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

We seem to be always setting ICM45686 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 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

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

…M45686

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

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 · 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.

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"

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.

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

bugobliterator added 3 commits July 17, 2024 14:07

AP_HAL_ChibiOS: CubeRedPrimary: update config for icm45686

8c05591

AP_InertialSensor: fix comments surrounding fast rate sampling

4bd5cd8

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

e80cbd8

…M45686

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

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

bugobliterator commented Jul 17, 2024 •

edited

Loading

tridge commented Jul 17, 2024

andyp1per left a comment

andyp1per Jul 18, 2024

bugobliterator Jul 24, 2024 •

edited

Loading

andyp1per Aug 31, 2024

andyp1per Jul 18, 2024

bugobliterator Jul 24, 2024 •

edited

Loading

andyp1per Aug 19, 2024

andyp1per Aug 31, 2024

andyp1per Jul 18, 2024

bugobliterator Jul 24, 2024

tridge commented Jul 24, 2024

andyp1per left a comment

andyp1per Aug 19, 2024

andyp1per Aug 19, 2024

andyp1per Aug 19, 2024

andyp1per Aug 19, 2024

tridge commented Aug 28, 2024

andyp1per left a comment

andyp1per Aug 31, 2024

andyp1per Aug 31, 2024

andyp1per Aug 31, 2024

andyp1per Aug 31, 2024

andyp1per commented Aug 31, 2024

andyp1per commented Sep 5, 2024 •

edited

Loading

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

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

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

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

andyp1per commented Aug 31, 2024

andyp1per commented Sep 5, 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