fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 12:11:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

mpu6000: work around apparent ICM20608 bug

Browse Source 
undocumented register 0x11 sometimes starts with value 0, which gives
an offset on the Y accel axis of 2.7m/s/s. It sometimes boots with
0xc9, which gives a zero offset. Force it to 0xc9 to get consistently
correct behaviour
This commit is contained in:
Andrew Tridgell
2015-11-28 09:13:15 +01:00
committed by Lorenz Meier
parent 38724d9e12
commit 46631b5c8a
1 changed files with 21 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/mpu6000/mpu6000.cpp
+21 -2
View File
@@ -151,6 +151,15 @@
#define MPU_WHOAMI_6000 0x68 #define MPU_WHOAMI_6000 0x68
#define ICM_WHOAMI_20608 0xaf #define ICM_WHOAMI_20608 0xaf
// ICM2608 specific registers
/* this is an undocumented register which
if set incorrectly results in getting a 2.7m/s/s offset
on the Y axis of the accelerometer
*/
#define MPUREG_ICM_UNDOC1 0x11
#define MPUREG_ICM_UNDOC1_VALUE 0xc9
// Product ID Description for ICM2608 // Product ID Description for ICM2608
// There is none // There is none
@@ -301,7 +310,7 @@ private:
// this is used to support runtime checking of key // this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor // configuration registers to detect SPI bus errors and sensor
// reset // reset
#define MPU6000_NUM_CHECKED_REGISTERS 9 #define MPU6000_NUM_CHECKED_REGISTERS 10
static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS];
uint8_t _checked_next; uint8_t _checked_next;
@@ -486,7 +495,8 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU
MPUREG_GYRO_CONFIG, MPUREG_GYRO_CONFIG,
MPUREG_ACCEL_CONFIG, MPUREG_ACCEL_CONFIG,
MPUREG_INT_ENABLE, MPUREG_INT_ENABLE,
MPUREG_INT_PIN_CFG MPUREG_INT_PIN_CFG,
MPUREG_ICM_UNDOC1
}; };
@@ -789,6 +799,10 @@ int MPU6000::reset()
write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
usleep(1000); usleep(1000);
if (is_icm_device()) {
write_checked_reg(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE);
}
// Oscillator set // Oscillator set
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
usleep(1000); usleep(1000);
@@ -1669,6 +1683,11 @@ MPU6000::check_registers(void)
*/ */
uint8_t v; uint8_t v;
// the MPUREG_ICM_UNDOC1 is specific to the ICM20608 (and undocumented)
if (_checked_registers[_checked_next] == MPUREG_ICM_UNDOC1 && !is_icm_device()) {
_checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS;
}
if ((v = read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != if ((v = read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
_checked_values[_checked_next]) { _checked_values[_checked_next]) {
/* /*