From 84f493664dee0d3b0f6d439db0e104c0c7b2476b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 20 Jul 2022 13:26:59 +1000 Subject: [PATCH] AP_InertialSensor: enable 180Hz LPF on ICM42670 --- .../AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index c674e44062..485f14c097 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -72,7 +72,9 @@ static const float GYRO_SCALE = (0.0174532f / 16.4f); // registers for ICM-42670, multi-bank #define INV3REG_70_PWR_MGMT0 0x1F #define INV3REG_70_GYRO_CONFIG0 0x20 +#define INV3REG_70_GYRO_CONFIG1 0x23 #define INV3REG_70_ACCEL_CONFIG0 0x21 +#define INV3REG_70_ACCEL_CONFIG1 0x24 #define INV3REG_70_FIFO_COUNTH 0x3D #define INV3REG_70_FIFO_DATA 0x3F #define INV3REG_70_INTF_CONFIG0 0x35 @@ -426,11 +428,13 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void) register_write(INV3REG_70_PWR_MGMT0, 0x0f); hal.scheduler->delay_microseconds(300); - // setup gyro for 1.6kHz + // setup gyro for 1.6kHz, with 180Hz LPF register_write(INV3REG_70_GYRO_CONFIG0, 0x05); + register_write(INV3REG_70_GYRO_CONFIG1, 0x01); - // setup accel for 1.6kHz + // setup accel for 1.6kHz, with 180Hz LPF register_write(INV3REG_70_ACCEL_CONFIG0, 0x05); + register_write(INV3REG_70_ACCEL_CONFIG1, 0x01); } else { // enable gyro and accel in low-noise modes register_write(INV3REG_PWR_MGMT0, 0x0F);