Fixed gyro detection for multi gyro setups.

src/main/drivers/accgyro/accgyro_mpu.c

@@ -222,7 +222,8 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
 static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
 {
     SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(config->spiBus));
-    if (!instance) {
+
+    if (!instance || !config->csnTag) {
         return false;
     }
     spiBusSetInstance(&gyro->bus, instance);
@@ -248,7 +249,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyro
 
     spiPreinitByTag(config->csnTag);
 
-    return false;
+    return true;
 }
 #endif
 
@@ -261,13 +262,13 @@ void mpuPreInit(const struct gyroDeviceConfig_s *config)
 #endif
 }
 
-void mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
+bool mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
 {
     // MPU datasheet specifies 30ms.
     delay(35);
 
     if (config->bustype == BUSTYPE_NONE) {
-        return;
+        return false;
     }
 
     if (config->bustype == BUSTYPE_GYRO_AUTO) {
@@ -291,7 +292,7 @@ void mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
             inquiryResult &= MPU_INQUIRY_MASK;
             if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
                 gyro->mpuDetectionResult.sensor = MPU_3050;
-                return;
+                return true;
             }
 
             sig &= MPU_INQUIRY_MASK;
@@ -301,14 +302,17 @@ void mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
             } else if (sig == MPU6500_WHO_AM_I_CONST) {
                 gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
             }
-            return;
+            return true;
         }
     }
 #endif
 
 #ifdef USE_SPI_GYRO
     gyro->bus.bustype = BUSTYPE_SPI;
-    detectSPISensorsAndUpdateDetectionResult(gyro, config);
+
+    return detectSPISensorsAndUpdateDetectionResult(gyro, config);
+#else
+    return false;
 #endif
 }
 

src/main/drivers/accgyro/accgyro_mpu.h

@@ -218,7 +218,7 @@ void mpuGyroInit(struct gyroDev_s *gyro);
 bool mpuGyroRead(struct gyroDev_s *gyro);
 bool mpuGyroReadSPI(struct gyroDev_s *gyro);
 void mpuPreInit(const struct gyroDeviceConfig_s *config);
-void mpuDetect(struct gyroDev_s *gyro, const struct gyroDeviceConfig_s *config);
+bool mpuDetect(struct gyroDev_s *gyro, const struct gyroDeviceConfig_s *config);
 uint8_t mpuGyroDLPF(struct gyroDev_s *gyro);
 uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg);
 

src/main/msp/msp.c

@@ -1357,13 +1357,12 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, compassConfig()->mag_align);
 
         // API 1.41 - Add multi-gyro indicator, selected gyro, and support for separate gyro 1 & 2 alignment
+        sbufWriteU8(dst, getGyroDetectionFlags());
 #ifdef USE_MULTI_GYRO
-        sbufWriteU8(dst, 1);    // USE_MULTI_GYRO
         sbufWriteU8(dst, gyroConfig()->gyro_to_use);
         sbufWriteU8(dst, gyroDeviceConfig(0)->align);
         sbufWriteU8(dst, gyroDeviceConfig(1)->align);
 #else
-        sbufWriteU8(dst, 0);
         sbufWriteU8(dst, GYRO_CONFIG_USE_GYRO_1);
         sbufWriteU8(dst, gyroDeviceConfig(0)->align);
         sbufWriteU8(dst, ALIGN_DEFAULT);

src/main/sensors/gyro.c

@@ -86,7 +86,7 @@
 FAST_RAM_ZERO_INIT gyro_t gyro;
 static FAST_RAM_ZERO_INIT uint8_t gyroDebugMode;
 
-static uint8_t gyroToUse = 0;
+static FAST_RAM_ZERO_INIT uint8_t gyroToUse;
 static FAST_RAM_ZERO_INIT bool overflowDetected;
 
 #ifdef USE_GYRO_OVERFLOW_CHECK
@@ -167,6 +167,8 @@ STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor2;
 #endif
 
+static gyroDetectionFlags_t gyroDetectionFlags = NO_GYROS_DETECTED;
+
 #ifdef UNIT_TEST
 STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyroSensor1;
 STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
@@ -393,7 +395,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
     }
 
     if (gyroHardware != GYRO_NONE) {
-        detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
         sensorsSet(SENSOR_GYRO);
     }
 
@@ -411,24 +412,30 @@ static void gyroPreInitSensor(const gyroDeviceConfig_t *config)
 #endif
 }
 
-static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
+static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
 {
-    gyroSensor->gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
-    gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
-    gyroSensor->gyroDev.gyroAlign = config->align;
-    gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;
-
 #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
  || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20)
 
-    mpuDetect(&gyroSensor->gyroDev, config);
+    if (!mpuDetect(&gyroSensor->gyroDev, config)) {
+        return false;
+    }
+#else
+    UNUSED(config);
 #endif
 
     const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
     gyroSensor->gyroDev.gyroHardware = gyroHardware;
-    if (gyroHardware == GYRO_NONE) {
-        return false;
-    }
+
+    return gyroHardware != GYRO_NONE;
+}
+
+static void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
+{
+    gyroSensor->gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
+    gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
+    gyroSensor->gyroDev.gyroAlign = config->align;
+    gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;
 
     // Must set gyro targetLooptime before gyroDev.init and initialisation of filters
     gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_hardware_lpf, gyroConfig()->gyro_sync_denom);
@@ -437,7 +444,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c
 
     // As new gyros are supported, be sure to add them below based on whether they are subject to the overflow/inversion bug
     // Any gyro not explicitly defined will default to not having built-in overflow protection as a safe alternative.
-    switch (gyroHardware) {
+    switch (gyroSensor->gyroDev.gyroHardware) {
     case GYRO_NONE:    // Won't ever actually get here, but included to account for all gyro types
     case GYRO_DEFAULT:
     case GYRO_FAKE:
@@ -471,8 +478,6 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c
     gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime);
 
 #endif
-
-    return true;
 }
 
 void gyroPreInit(void)
@@ -516,64 +521,65 @@ bool gyroInit(void)
     }
     firstArmingCalibrationWasStarted = false;
 
-    enum {
-        NO_GYROS_DETECTED = 0,
-        DETECTED_GYRO_1 = (1 << 0),
-        DETECTED_GYRO_2 = (1 << 1),
-    };
-
-    uint8_t detectionFlags = NO_GYROS_DETECTED;
+    gyroDetectionFlags = NO_GYROS_DETECTED;
 
     gyroToUse = gyroConfig()->gyro_to_use;
 
-    if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
-        if (gyroInitSensor(&gyroSensor1, gyroDeviceConfig(0))) {
-            gyroHasOverflowProtection =  gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection;
-            detectionFlags |= DETECTED_GYRO_1;
-        }
+    if (gyroDetectSensor(&gyroSensor1, gyroDeviceConfig(0))) {
+        gyroDetectionFlags |= DETECTED_GYRO_1;
     }
 
-#ifdef USE_MULTI_GYRO
-    if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
-        if (gyroInitSensor(&gyroSensor2, gyroDeviceConfig(1))) {
-            gyroHasOverflowProtection =  gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection;
-            detectionFlags |= DETECTED_GYRO_2;
-        }
+#if defined(USE_MULTI_GYRO)
+    if (gyroDetectSensor(&gyroSensor2, gyroDeviceConfig(1))) {
+        gyroDetectionFlags |= DETECTED_GYRO_2;
     }
+#endif
 
-    if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && detectionFlags != (DETECTED_GYRO_1 | DETECTED_GYRO_2)) {
-        // at least one gyro is missing.
+    if (gyroDetectionFlags == NO_GYROS_DETECTED) {
+        return false;
+    }
 
-        if (detectionFlags & DETECTED_GYRO_1) {
+#if defined(USE_MULTI_GYRO)
+    if ((gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !(gyroDetectionFlags & DETECTED_BOTH_GYROS))
+        || (gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & DETECTED_GYRO_1))
+        || (gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & DETECTED_GYRO_2))) {
+        if (gyroDetectionFlags & DETECTED_GYRO_1) {
             gyroToUse = GYRO_CONFIG_USE_GYRO_1;
-            gyroConfigMutable()->gyro_to_use = GYRO_CONFIG_USE_GYRO_1;
-            detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware;
-            sensorsSet(SENSOR_GYRO);
-            return true;
-        }
-
-        if (detectionFlags & DETECTED_GYRO_2) {
+        } else {
             gyroToUse = GYRO_CONFIG_USE_GYRO_2;
-            gyroConfigMutable()->gyro_to_use = GYRO_CONFIG_USE_GYRO_2;
-            detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor2.gyroDev.gyroHardware;
-            sensorsSet(SENSOR_GYRO);
-            return true;
         }
+
+        gyroConfigMutable()->gyro_to_use = gyroToUse;
     }
 
     // Only allow using both gyros simultaneously if they are the same hardware type.
-    // If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
-    if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
-        if (gyroSensor1.gyroDev.gyroHardware != gyroSensor2.gyroDev.gyroHardware) {
-            gyroToUse = GYRO_CONFIG_USE_GYRO_1;
-            gyroConfigMutable()->gyro_to_use = GYRO_CONFIG_USE_GYRO_1;
-            detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware;
-            sensorsSet(SENSOR_GYRO);
-        }
+    if ((gyroDetectionFlags & DETECTED_BOTH_GYROS) && gyroSensor1.gyroDev.gyroHardware == gyroSensor2.gyroDev.gyroHardware) {
+        gyroDetectionFlags |= DETECTED_DUAL_GYROS;
+    } else if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
+        // If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
+        gyroToUse = GYRO_CONFIG_USE_GYRO_1;
+        gyroConfigMutable()->gyro_to_use = gyroToUse;
+    }
+
+    if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
+        gyroInitSensor(&gyroSensor2, gyroDeviceConfig(1));
+        gyroHasOverflowProtection =  gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection;
+        detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor2.gyroDev.gyroHardware;
     }
 #endif
 
-    return detectionFlags != NO_GYROS_DETECTED;
+    if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
+        gyroInitSensor(&gyroSensor1, gyroDeviceConfig(0));
+        gyroHasOverflowProtection =  gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection;
+        detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware;
+    }
+
+    return true;
+}
+
+gyroDetectionFlags_t getGyroDetectionFlags(void)
+{
+    return gyroDetectionFlags;
 }
 
 #ifdef USE_DYN_LPF

src/main/sensors/gyro.h

@@ -36,18 +36,18 @@ typedef struct gyro_s {
 
 extern gyro_t gyro;
 
-typedef enum {
+enum {
     GYRO_OVERFLOW_CHECK_NONE = 0,
     GYRO_OVERFLOW_CHECK_YAW,
     GYRO_OVERFLOW_CHECK_ALL_AXES
-} gyroOverflowCheck_e;
+};
 
 enum {
     DYN_NOTCH_RANGE_HIGH = 0,
     DYN_NOTCH_RANGE_MEDIUM,
     DYN_NOTCH_RANGE_LOW,
     DYN_NOTCH_RANGE_AUTO
-} ;
+};
 
 #define DYN_NOTCH_RANGE_HZ_HIGH 2000
 #define DYN_NOTCH_RANGE_HZ_MEDIUM 1333
@@ -63,10 +63,20 @@ enum {
 #define GYRO_CONFIG_USE_GYRO_2      1
 #define GYRO_CONFIG_USE_GYRO_BOTH   2
 
-typedef enum {
+enum {
     FILTER_LOWPASS = 0,
     FILTER_LOWPASS2
-} filterSlots;
+};
+
+typedef enum gyroDetectionFlags_e {
+    NO_GYROS_DETECTED = 0,
+    DETECTED_GYRO_1 = (1 << 0),
+#if defined(USE_MULTI_GYRO)
+    DETECTED_GYRO_2 = (1 << 1),
+    DETECTED_BOTH_GYROS = (DETECTED_GYRO_1 | DETECTED_GYRO_2),
+    DETECTED_DUAL_GYROS = (1 << 7), // All gyros are of the same hardware type
+#endif
+} gyroDetectionFlags_t;
 
 typedef struct gyroConfig_s {
     uint8_t  gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
@@ -125,6 +135,7 @@ bool gyroOverflowDetected(void);
 bool gyroYawSpinDetected(void);
 uint16_t gyroAbsRateDps(int axis);
 uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg);
+gyroDetectionFlags_t getGyroDetectionFlags(void);
 #ifdef USE_DYN_LPF
 float dynThrottle(float throttle);
 void dynLpfGyroUpdate(float throttle);

src/main/target/common_defaults_post.h

@@ -293,14 +293,6 @@
 #endif
 #endif
 
-// F4 and F7 single gyro boards
-#if defined(USE_MULTI_GYRO) && !defined(GYRO_2_SPI_INSTANCE)
-#define GYRO_2_SPI_INSTANCE     GYRO_1_SPI_INSTANCE
-#define GYRO_2_CS_PIN           NONE
-#define GYRO_2_ALIGN            ALIGN_DEFAULT
-#define GYRO_2_EXTI_PIN         NONE
-#endif
-
 #if !defined(GYRO_1_SPI_INSTANCE)
 #define GYRO_1_SPI_INSTANCE     NULL
 #endif
@@ -317,6 +309,14 @@
 #define GYRO_1_ALIGN            ALIGN_DEFAULT
 #endif
 
+// F4 and F7 single gyro boards
+#if defined(USE_MULTI_GYRO) && !defined(GYRO_2_SPI_INSTANCE)
+#define GYRO_2_SPI_INSTANCE     GYRO_1_SPI_INSTANCE
+#define GYRO_2_CS_PIN           NONE
+#define GYRO_2_ALIGN            ALIGN_DEFAULT
+#define GYRO_2_EXTI_PIN         NONE
+#endif
+
 #if defined(MPU_ADDRESS)
 #define GYRO_I2C_ADDRESS MPU_ADDRESS
 #else

src/test/unit/sensor_gyro_unittest.cc

@@ -59,7 +59,6 @@ TEST(SensorGyro, Detect)
 {
     const gyroHardware_e detected = gyroDetect(gyroDevPtr);
     EXPECT_EQ(GYRO_FAKE, detected);
-    EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
 }
 
 TEST(SensorGyro, Init)