Skip to content
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.

Sign up for GitHub

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

Merged
merged 1 commit into from
Dec 18, 2024
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
74 changes: 34 additions & 40 deletions libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -343,15 +343,11 @@ void AP_InertialSensor_Invensensev3::start()
// pre-calculate backend period
backend_period_us = 1000000UL / backend_rate_hz;

if (!_imu.register_gyro(gyro_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype)) ||
!_imu.register_accel(accel_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype))) {
if (!_imu.register_gyro(gyro_instance, sampling_rate_hz, dev->get_bus_id_devtype(devtype)) ||
!_imu.register_accel(accel_instance, sampling_rate_hz, dev->get_bus_id_devtype(devtype))) {
return;
}

// update backend sample rate
_set_accel_raw_sample_rate(accel_instance, backend_rate_hz);
_set_gyro_raw_sample_rate(gyro_instance, backend_rate_hz);

// indicate what multiplier is appropriate for the sensors'
// readings to fit them into an int16_t:
_set_raw_sample_accel_multiplier(accel_instance, multiplier_accel);
Expand Down Expand Up @@ -381,16 +377,15 @@ void AP_InertialSensor_Invensensev3::start()

// get a startup banner to output to the GCS
bool AP_InertialSensor_Invensensev3::get_output_banner(char* banner, uint8_t banner_len) {
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 %.1fkHz/%.1fkHz",
gyro_instance,
fast_sampling ? " fast" : " normal",
#if HAL_INS_HIGHRES_SAMPLE
highres_sampling ? ", high-resolution" :
highres_sampling ? " hi-res" :
#endif
"" , backend_rate_hz * 0.001);
return true;
}
return false;
"", backend_rate_hz * 0.001,
sampling_rate_hz * 0.001);
return true;
}

/*
Expand Down Expand Up @@ -669,21 +664,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
Copy link
Collaborator

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

{
// constrain the gyro rate to be at least the loop rate
uint8_t loop_limit = 1;
if (get_loop_rate_hz() > base_odr) {
loop_limit = 2;
uint8_t min_base_rate_multiplier = 1;
if (get_loop_rate_hz() > base_backend_rate) {
min_base_rate_multiplier = 2;
}
if (get_loop_rate_hz() > base_odr * 2) {
loop_limit = 4;
if (get_loop_rate_hz() > base_backend_rate * 2) {
min_base_rate_multiplier = 4;
}
// constrain the gyro rate to be a 2^N multiple
uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 8);
uint8_t fast_sampling_rate_multiplier = constrain_int16(get_fast_sampling_rate(), min_base_rate_multiplier, 8);

// calculate rate we will be giving samples to the backend
return constrain_int16(base_odr * fast_sampling_rate, base_odr, max_odr);
return constrain_int16(base_backend_rate * fast_sampling_rate_multiplier, base_backend_rate, max_backend_rate);
}

/*
Expand Down Expand Up @@ -806,6 +801,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
}
}
}
sampling_rate_hz = backend_rate_hz; // sampling rate and backend rate are the same

// disable gyro and accel as per 12.9 in the ICM-42688 docs
register_write(INV3REG_PWR_MGMT0, 0x00);
Expand Down Expand Up @@ -862,6 +858,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void)
{
backend_rate_hz = 1600;
sampling_rate_hz = 1600;
// use low-noise mode
register_write(INV3REG_70_PWR_MGMT0, 0x0f);
hal.scheduler->delay_microseconds(300);
Expand All @@ -886,27 +883,24 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void)
*/
void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void)
{
uint8_t odr_config = 4;
backend_rate_hz = 1600;
// always fast sampling
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
uint8_t odr_config;
Copy link
Collaborator

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?

backend_rate_hz = 800;
Copy link
Collaborator

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.

Copy link
Member Author

@bugobliterator bugobliterator Jul 24, 2024 •
edited
Loading

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.

Copy link
Collaborator

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.

Copy link
Collaborator

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

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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


if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() > 1) {
backend_rate_hz = calculate_fast_sampling_backend_rate(backend_rate_hz, backend_rate_hz * 4);
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
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);
Copy link
Collaborator

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

} else {
fast_sampling = false;
}

// this sensor actually only supports 2 speeds
backend_rate_hz = constrain_int16(backend_rate_hz, 3200, 6400);

switch (backend_rate_hz) {
case 6400: // 6.4Khz
odr_config = 3;
break;
case 3200: // 3.2Khz
odr_config = 4;
break;
default:
break;
if (backend_rate_hz <= 3200) {
odr_config = 0x04; // 3200Hz
sampling_rate_hz = 3200;
} else {
odr_config = 0x03; // 6400Hz
sampling_rate_hz = 6400;
}

// Disable FIFO first
Expand Down
3 changes: 2 additions & 1 deletion libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend
void set_filter_and_scaling_icm42670(void);
void set_filter_and_scaling_icm456xy(void);
void fifo_reset();
uint16_t calculate_fast_sampling_backend_rate(uint16_t base_odr, uint16_t max_odr) const;
uint16_t calculate_fast_sampling_backend_rate(uint16_t base_backend_rate, uint16_t max_backend_rate) const;

/* Read samples from FIFO */
void read_fifo();
Expand Down Expand Up @@ -141,4 +141,5 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend

float temp_filtered;
LowPassFilter2pFloat temp_filter;
uint32_t sampling_rate_hz;
Copy link
Collaborator

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?

Copy link
Member Author

@bugobliterator bugobliterator Jul 24, 2024 •
edited
Loading

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

Copy link
Collaborator

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

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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);

};
Loading