aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: ensure indirect registers are not modified with sensors active on ICM-42688

Browse Source
This commit is contained in:
Andy Piper 2023-09-21 21:30:45 +01:00 committed by Andrew Tridgell
parent da0b1402d3
commit a9e0bf070c
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -776,9 +776,8 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
} }
} }
// enable gyro and accel in low-noise modes // disable gyro and accel as per 12.9 in the ICM-42688 docs
register_write(INV3REG_PWR_MGMT0, 0x0F); register_write(INV3REG_PWR_MGMT0, 0x00);
hal.scheduler->delay_microseconds(300);
// setup gyro for backend rate // setup gyro for backend rate
register_write(INV3REG_GYRO_CONFIG0, odr_config); register_write(INV3REG_GYRO_CONFIG0, odr_config);
@ -797,6 +796,10 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC2, accel_aaf_delt<<1); // ACCEL_AAF_DELT | enabled bit register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC2, accel_aaf_delt<<1); // ACCEL_AAF_DELT | enabled bit
register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC3, (accel_aaf_deltsqr & 0xFF)); // ACCEL_AAF_DELTSQR register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC3, (accel_aaf_deltsqr & 0xFF)); // ACCEL_AAF_DELTSQR
register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC4, ((accel_aaf_bitshift<<4) & 0xF0) | ((accel_aaf_deltsqr>>8) & 0x0F)); // ACCEL_AAF_BITSHIFT | ACCEL_AAF_DELTSQR register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC4, ((accel_aaf_bitshift<<4) & 0xF0) | ((accel_aaf_deltsqr>>8) & 0x0F)); // ACCEL_AAF_BITSHIFT | ACCEL_AAF_DELTSQR
// enable gyro and accel in low-noise modes
register_write(INV3REG_PWR_MGMT0, 0x0F);
hal.scheduler->delay_microseconds(300);
} }
/* /*