Speed up gyro device detection; fix MPU6000 initialization
The detection logic in the MPU6000 and ICM20689 drivers had unnecessary "retry" logic that would try to read the `WHOAMI` register 5 times for MPU6000 and 20(!) times for ICM20689 - with a 150ms delay for each iteration. So the problem is that all the enabled drivers need to be probed in order so on a board that had a LSM6DSO for example (or any sensor further down the list) it would introduce about 4 seconds of initialization delay (dual gyro board would take 8 seconds!). Revised logic completes in about 0.4 seconds in the worst case. After removing the unnecessary retry/repeat logic for the MPU6000 an initialization problem became visible. The driver was not following the datasheet guidelines and clearing the signal path after resetting the device. This lead to detection issues on warm reboots that were accidentally masked by the excessive delays with the retry logic. The improvement in boot speed depends on the device's position in the list. So any device with a MPU6000 won't see any difference. All other devices will boot about 1 second faster (like ICM20602, ICM20689). Devices after the ICM20689 (like BMI160/270, LSM6DSO) will boot about 4 seconds faster.
This commit is contained in:
parent
968e72fae8
commit
140471adc6
|
@ -210,6 +210,9 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
|
|||
#ifdef USE_GYRO_SPI_ICM20689
|
||||
icm20689SpiDetect, // icm20689SpiDetect detects ICM20602 and ICM20689
|
||||
#endif
|
||||
#ifdef USE_ACCGYRO_LSM6DSO
|
||||
lsm6dsoDetect,
|
||||
#endif
|
||||
#ifdef USE_ACCGYRO_BMI160
|
||||
bmi160Detect,
|
||||
#endif
|
||||
|
@ -224,9 +227,6 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
|
|||
#endif
|
||||
#ifdef USE_GYRO_L3GD20
|
||||
l3gd20Detect,
|
||||
#endif
|
||||
#ifdef USE_ACCGYRO_LSM6DSO
|
||||
lsm6dsoDetect,
|
||||
#endif
|
||||
NULL // Avoid an empty array
|
||||
};
|
||||
|
|
|
@ -57,37 +57,35 @@ uint8_t icm20689SpiDetect(const busDevice_t *bus)
|
|||
|
||||
spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); //low speed
|
||||
|
||||
// reset the device configuration
|
||||
spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
|
||||
delay(100);
|
||||
|
||||
uint8_t icmDetected = MPU_NONE;
|
||||
uint8_t attemptsRemaining = 20;
|
||||
do {
|
||||
delay(150);
|
||||
const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
|
||||
switch (whoAmI) {
|
||||
case ICM20601_WHO_AM_I_CONST:
|
||||
icmDetected = ICM_20601_SPI;
|
||||
break;
|
||||
case ICM20602_WHO_AM_I_CONST:
|
||||
icmDetected = ICM_20602_SPI;
|
||||
break;
|
||||
case ICM20608G_WHO_AM_I_CONST:
|
||||
icmDetected = ICM_20608_SPI;
|
||||
break;
|
||||
case ICM20689_WHO_AM_I_CONST:
|
||||
icmDetected = ICM_20689_SPI;
|
||||
break;
|
||||
default:
|
||||
icmDetected = MPU_NONE;
|
||||
break;
|
||||
}
|
||||
if (icmDetected != MPU_NONE) {
|
||||
break;
|
||||
}
|
||||
if (!attemptsRemaining) {
|
||||
return MPU_NONE;
|
||||
}
|
||||
} while (attemptsRemaining--);
|
||||
// reset the device signal paths
|
||||
spiBusWriteRegister(bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
|
||||
delay(100);
|
||||
|
||||
uint8_t icmDetected;
|
||||
|
||||
const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
|
||||
|
||||
switch (whoAmI) {
|
||||
case ICM20601_WHO_AM_I_CONST:
|
||||
icmDetected = ICM_20601_SPI;
|
||||
break;
|
||||
case ICM20602_WHO_AM_I_CONST:
|
||||
icmDetected = ICM_20602_SPI;
|
||||
break;
|
||||
case ICM20608G_WHO_AM_I_CONST:
|
||||
icmDetected = ICM_20608_SPI;
|
||||
break;
|
||||
case ICM20689_WHO_AM_I_CONST:
|
||||
icmDetected = ICM_20689_SPI;
|
||||
break;
|
||||
default:
|
||||
icmDetected = MPU_NONE;
|
||||
break;
|
||||
}
|
||||
|
||||
spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
|
||||
|
||||
|
@ -121,12 +119,8 @@ void icm20689GyroInit(gyroDev_t *gyro)
|
|||
|
||||
spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION);
|
||||
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
|
||||
delay(100);
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
|
||||
delay(100);
|
||||
// spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
|
||||
// delay(100);
|
||||
// Device was already reset during detection so proceed with configuration
|
||||
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||
delay(15);
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
|
||||
|
|
|
@ -129,55 +129,50 @@ uint8_t mpu6000SpiDetect(const busDevice_t *bus)
|
|||
|
||||
spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION);
|
||||
|
||||
// reset the device configuration
|
||||
spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
|
||||
delay(100); // datasheet specifies a 100ms delay after reset
|
||||
|
||||
uint8_t attemptsRemaining = 5;
|
||||
do {
|
||||
delay(150);
|
||||
// reset the device signal paths
|
||||
spiBusWriteRegister(bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
|
||||
delay(100); // datasheet specifies a 100ms delay after signal path reset
|
||||
|
||||
const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
|
||||
if (whoAmI == MPU6000_WHO_AM_I_CONST) {
|
||||
break;
|
||||
|
||||
const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
|
||||
uint8_t detectedSensor = MPU_NONE;
|
||||
|
||||
if (whoAmI == MPU6000_WHO_AM_I_CONST) {
|
||||
const uint8_t productID = spiBusReadRegister(bus, MPU_RA_PRODUCT_ID);
|
||||
|
||||
/* look for a product ID we recognise */
|
||||
|
||||
// verify product revision
|
||||
switch (productID) {
|
||||
case MPU6000ES_REV_C4:
|
||||
case MPU6000ES_REV_C5:
|
||||
case MPU6000_REV_C4:
|
||||
case MPU6000_REV_C5:
|
||||
case MPU6000ES_REV_D6:
|
||||
case MPU6000ES_REV_D7:
|
||||
case MPU6000ES_REV_D8:
|
||||
case MPU6000_REV_D6:
|
||||
case MPU6000_REV_D7:
|
||||
case MPU6000_REV_D8:
|
||||
case MPU6000_REV_D9:
|
||||
case MPU6000_REV_D10:
|
||||
detectedSensor = MPU_60x0_SPI;
|
||||
}
|
||||
if (!attemptsRemaining) {
|
||||
return MPU_NONE;
|
||||
}
|
||||
} while (attemptsRemaining--);
|
||||
|
||||
const uint8_t productID = spiBusReadRegister(bus, MPU_RA_PRODUCT_ID);
|
||||
|
||||
/* look for a product ID we recognise */
|
||||
|
||||
// verify product revision
|
||||
switch (productID) {
|
||||
case MPU6000ES_REV_C4:
|
||||
case MPU6000ES_REV_C5:
|
||||
case MPU6000_REV_C4:
|
||||
case MPU6000_REV_C5:
|
||||
case MPU6000ES_REV_D6:
|
||||
case MPU6000ES_REV_D7:
|
||||
case MPU6000ES_REV_D8:
|
||||
case MPU6000_REV_D6:
|
||||
case MPU6000_REV_D7:
|
||||
case MPU6000_REV_D8:
|
||||
case MPU6000_REV_D9:
|
||||
case MPU6000_REV_D10:
|
||||
return MPU_60x0_SPI;
|
||||
}
|
||||
|
||||
return MPU_NONE;
|
||||
spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
|
||||
return detectedSensor;
|
||||
}
|
||||
|
||||
static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION);
|
||||
|
||||
// Device Reset
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
|
||||
delay(150);
|
||||
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
|
||||
delay(150);
|
||||
// Device was already reset during detection so proceed with configuration
|
||||
|
||||
// Clock Source PPL with Z axis gyro reference
|
||||
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
|
||||
|
|
Loading…
Reference in New Issue