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.

src/main/drivers/accgyro/accgyro_mpu.c

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

src/main/drivers/accgyro/accgyro_spi_icm20689.c

@@ -57,13 +57,18 @@ 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);
+
+    // reset the device signal paths
+    spiBusWriteRegister(bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
+    delay(100);
+
+    uint8_t icmDetected;
 
-    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;
@@ -81,13 +86,6 @@ uint8_t icm20689SpiDetect(const busDevice_t *bus)
         icmDetected = MPU_NONE;
         break;
     }
-        if (icmDetected != MPU_NONE) {
-            break;
-        }
-        if (!attemptsRemaining) {
-            return MPU_NONE;
-        }
-    } while (attemptsRemaining--);
 
     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);

src/main/drivers/accgyro/accgyro_spi_mpu6000.c

@@ -129,21 +129,19 @@ 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
+
+    // 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
 
-    uint8_t attemptsRemaining = 5;
-    do {
-        delay(150);
 
     const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
-        if (whoAmI == MPU6000_WHO_AM_I_CONST) {
-            break;
-        }
-        if (!attemptsRemaining) {
-            return MPU_NONE;
-        }
-    } while (attemptsRemaining--);
+    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 */
@@ -162,22 +160,19 @@ uint8_t mpu6000SpiDetect(const busDevice_t *bus)
         case MPU6000_REV_D8:
         case MPU6000_REV_D9:
         case MPU6000_REV_D10:
-        return MPU_60x0_SPI;
+            detectedSensor = 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);