ArduPilot
Do not Read FIFO faster than requested rate for ICM45686
- 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
we really need to know why the CPU cost is so high. @bugobliterator will try to do some profile to see why that is
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 https://github.com/ArduPilot/ardupilot/pull/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.
@bugobliterator I have done a demo in https://github.com/ArduPilot/ardupilot/pull/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.
@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
@bugobliterator I have done a simpler alternative here - https://github.com/ArduPilot/ardupilot/pull/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).
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.
