Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-31 13:08:34 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: do not read FIFO faster than requested rate for ICM45686

Browse Source
This commit is contained in:
Siddharth Purohit 2024-12-09 21:10:01 +11:00 committed by Randy Mackay
parent 10661c41a4
commit cd08d30e8f
2 changed files with 36 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -335,15 +335,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);
@ -373,16 +369,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;
}
/*
@ -649,21 +644,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
{
// 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);
}
/*
@ -786,6 +781,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);
@ -841,6 +837,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);
@ -865,27 +862,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

3
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
View File

@ -57,7 +57,7 @@ private:
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();
@ -140,4 +140,5 @@ private:
float temp_filtered;
LowPassFilter2pFloat temp_filter;
uint32_t sampling_rate_hz;
};