Skip to content

Commit

Permalink
AP_InertialSensor: do not read FIFO faster than requested rate for IC…
Browse files Browse the repository at this point in the history
…M45686
  • Loading branch information
bugobliterator committed Nov 19, 2024
1 parent 37d1e8d commit 801aae0
Show file tree
Hide file tree
Showing 2 changed files with 25 additions and 26 deletions.
50 changes: 24 additions & 26 deletions libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -381,16 +381,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 LR:%.1fkHz SR:%.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 @@ -806,6 +805,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 +862,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 +887,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;
backend_rate_hz = 800;

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);
} 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
Original file line number Diff line number Diff line change
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;
};

0 comments on commit 801aae0

Please sign in to comment.