Merge pull request #10098 from etracer65/improve_gyro_detect

Speed up gyro device detection; fix MPU6000 initialization

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);
+    // Device was already reset during detection so proceed with configuration
-    delay(100);
+
-    spiBusWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
-    delay(100);
-//    spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
-//    delay(100);
     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) {
+    uint8_t detectedSensor = MPU_NONE;
-            break;
-        }
-        if (!attemptsRemaining) {
-            return MPU_NONE;
-        }
-    } while (attemptsRemaining--);
 
+    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
+    // Device was already reset during detection so proceed with configuration
-    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);
 
     // Clock Source PPL with Z axis gyro reference
     spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);