From 5f6265838349fc9df4390479c563696943b3fc75 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 20 Jul 2022 18:33:43 +1000 Subject: [PATCH] AP_InertialSensor: re-enable notch filter on 42688 thanks to Andy for pointing out this is for an internal resonance --- libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index 485f14c097..93025c9d79 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -544,8 +544,7 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void) register_write(INV3REG_70_INTF_CONFIG0, 0x40, true); } if (inv3_type == Invensensev3_Type::ICM42688) { - // setup anti-alias filters for 488Hz on gyro and accel, notch disabled - register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC2, 0xA1); + // setup anti-alias filters for 488Hz on gyro and accel, notch left at default register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC3, 11); // GYRO_AAF_DELT register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC4, 122); // GYRO_AAF_DELTSQR register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC5, 0x80); // GYRO_AAF_BITSHIFT&GYRO_AAF_DELTSQR