From 24537b2b842b5f183d8de82e207b71fd5417e2d8 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 20 Oct 2022 19:48:09 +0100 Subject: [PATCH] AP_InertialSensor: ensure that hardware AAF and notch filter are enabled on 42605 and 42609 --- libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index d995db66e5..06e28d211f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -578,6 +578,9 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void) register_write(INV3REG_ACCEL_CONFIG0, odr_config); // setup anti-alias filters for gyro at 1/4 ODR, notch left at default + // The defaults for 42605 and 42609 are different to the 42688, make sure AAF and notch are enabled on all + uint8_t aaf_enable = register_read_bank(1, INV3REG_GYRO_CONFIG_STATIC2); + register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC2, aaf_enable & ~0x03); register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC3, aaf_delt); // GYRO_AAF_DELT register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC4, (aaf_deltsqr & 0xFF)); // GYRO_AAF_DELTSQR register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC5, ((aaf_bitshift<<4) & 0xF0) | ((aaf_deltsqr>>8) & 0x0F)); // GYRO_AAF_BITSHIFT | GYRO_AAF_DELTSQR