Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: calculate backend sample rates on ICM45686 correctly

Browse Source
This commit is contained in:
Andy Piper 2023-02-20 09:37:48 +00:00 committed by Andrew Tridgell
parent 7f32a756bb
commit 3cca401af8
1 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -679,20 +679,24 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void)
void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void) void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void)
{ {
uint8_t odr_config = 4; uint8_t odr_config = 4;
backend_rate_hz = 3200; backend_rate_hz = 1600;
// always fast sampling
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() > 1) { if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() > 1) {
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; backend_rate_hz = calculate_fast_sampling_backend_rate(backend_rate_hz, backend_rate_hz * 4);
if (fast_sampling) {
backend_rate_hz = calculate_fast_sampling_backend_rate(backend_rate_hz, backend_rate_hz * 4);
}
} }
// this sensor actually only supports 2 speeds
backend_rate_hz = constrain_int16(backend_rate_hz, 3200, 6400);
switch (backend_rate_hz) { switch (backend_rate_hz) {
case 6400: // 6.4Khz case 6400: // 6.4Khz
odr_config = 3; odr_config = 3;
break; break;
case 3200: // 3.2Khz
odr_config = 4;
break;
default: default:
break; break;
} }