5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00

AP_InertialSensor: re-enable notch filter on 42688

thanks to Andy for pointing out this is for an internal resonance
This commit is contained in:
Andrew Tridgell 2022-07-20 18:33:43 +10:00
parent 8a6b8fc486
commit 2582d5fad5

View File

@ -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