Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: ensure that hardware AAF and notch filter are enabled on 42605 and 42609

Browse Source
This commit is contained in:
Andy Piper 2022-10-20 19:48:09 +01:00 committed by Randy Mackay
parent 2617c2de99
commit 356b2a9656
1 changed files with 3 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -578,6 +578,9 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
register_write(INV3REG_ACCEL_CONFIG0, odr_config); register_write(INV3REG_ACCEL_CONFIG0, odr_config);
// setup anti-alias filters for gyro at 1/4 ODR, notch left at default // 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_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_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 register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC5, ((aaf_bitshift<<4) & 0xF0) | ((aaf_deltsqr>>8) & 0x0F)); // GYRO_AAF_BITSHIFT | GYRO_AAF_DELTSQR