-
Notifications
You must be signed in to change notification settings - Fork 17.1k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Do not Read FIFO faster than requested rate for ICM45686 #27573
base: master
Are you sure you want to change the base?
Do not Read FIFO faster than requested rate for ICM45686 #27573
Conversation
b251b5d
to
e80cbd8
Compare
@andyp1per need your review on this one |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
@@ -140,4 +140,5 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend | |||
|
|||
float temp_filtered; | |||
LowPassFilter2pFloat temp_filter; | |||
uint32_t sampling_rate_hz; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Where is this used other than in the boot message?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
But you can just calculate the value as backend_rate * gyro rate? Don't think you need to hold a variable anywhere
// always fast sampling | ||
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; | ||
uint8_t odr_config; | ||
backend_rate_hz = 800; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Also you asserted that this only affect non-fast sampling in the call but it affects fast sampling as well
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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
why not 7?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
For variety
need to defer till @andyp1per can flight test this |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
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", |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
// always fast sampling | ||
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; | ||
uint8_t odr_config; | ||
backend_rate_hz = 800; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
backend_rate_hz = 1600; | ||
// always fast sampling | ||
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; | ||
uint8_t odr_config; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Where is the 800Hz value set in the ODR?
@@ -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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This is just a rename, I think you should remove this from the PR to make review a little easier
we really need to know why the CPU cost is so high. @bugobliterator will try to do some profile to see why that is |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
None of my comments have been addressed and it does affect fast sampling
// @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. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think you should provide a typical value - so "e.g. 1Khz on an ICM-42688"
@@ -140,4 +140,5 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend | |||
|
|||
float temp_filtered; | |||
LowPassFilter2pFloat temp_filter; | |||
uint32_t sampling_rate_hz; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
But you can just calculate the value as backend_rate * gyro rate? Don't think you need to hold a variable anywhere
// always fast sampling | ||
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; | ||
uint8_t odr_config; | ||
backend_rate_hz = 800; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Also you asserted that this only affect non-fast sampling in the call but it affects fast sampling as well
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); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Changes fast sampling backend rate
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. |
@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. |
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