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

AP_InertialSensor: constrain ICM45686 to 800Hz-6.4KHz #28682

Closed
wants to merge 1 commit into from
Closed

AP_InertialSensor: constrain ICM45686 to 800Hz-6.4KHz #28682

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
19 changes: 12 additions & 7 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 @@ -887,24 +887,29 @@ 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;
backend_rate_hz = 800;
// 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) {
backend_rate_hz = calculate_fast_sampling_backend_rate(backend_rate_hz, backend_rate_hz * 4);
backend_rate_hz = calculate_fast_sampling_backend_rate(backend_rate_hz, backend_rate_hz * 8);
}

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

switch (backend_rate_hz) {
case 6400: // 6.4Khz
odr_config = 3;
break;
case 3200: // 3.2Khz
case 3200: // 3.2Khz / ODR 6.4KHz
odr_config = 3;
break;
case 1600: // 1.6Khz / ODR 3.2KHz
odr_config = 4;
break;
case 800: // 800hz / ODR 1.6KHz
odr_config = 5;
break;
default:
break;
}
Expand All @@ -913,10 +918,10 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void)
register_write(INV3REG_456_FIFO_CONFIG3, 0x00);
register_write(INV3REG_456_FIFO_CONFIG0, 0x00);

// setup gyro for 1.6-6.4kHz, 4000dps range
// setup gyro for 800-6.4kHz, 4000dps range
register_write(INV3REG_456_GYRO_CONFIG0, (0x0 << 4) | odr_config); // GYRO_UI_FS_SEL b4-7, GYRO_ODR b0-3

// setup accel for 1.6-6.4kHz, 32g range
// setup accel for 800-6.4kHz, 32g range
register_write(INV3REG_456_ACCEL_CONFIG0, (0x0 << 4) | odr_config); // ACCEL_UI_FS_SEL b4-6, ACCEL_ODR b0-3

// enable timestamps on FIFO data
Expand Down
Loading