Merge pull request #5807 from etracer65/gyro_both_validation

Only allow gyro_to_use = BOTH if both detected gyros are the same type

src/main/drivers/accgyro/accgyro.h

@@ -27,6 +27,7 @@
 #include "drivers/bus.h"
 #include "drivers/sensor.h"
 #include "drivers/accgyro/accgyro_mpu.h"
+#include "sensors/gyro.h"
 #pragma GCC diagnostic push
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
 #include <pthread.h>
@@ -81,7 +82,7 @@ typedef struct gyroDev_s {
     uint8_t mpuDividerDrops;
     ioTag_t mpuIntExtiTag;
     uint8_t gyroHasOverflowProtection;
-    uint8_t filler[1];
+    gyroSensor_e gyroHardware;
 } gyroDev_t;
 
 typedef struct accDev_s {

src/main/interface/cli.c

@@ -2374,11 +2374,27 @@ static void cliGpsPassthrough(char *cmdline)
 #endif
 
 #if defined(USE_GYRO_REGISTER_DUMP) && !defined(SIMULATOR_BUILD)
+static void cliPrintGyroRegisters(uint8_t whichSensor)
+{
+    tfp_printf("# WHO_AM_I    0x%X\r\n", gyroReadRegister(whichSensor, MPU_RA_WHO_AM_I));
+    tfp_printf("# CONFIG      0x%X\r\n", gyroReadRegister(whichSensor, MPU_RA_CONFIG));
+    tfp_printf("# GYRO_CONFIG 0x%X\r\n", gyroReadRegister(whichSensor, MPU_RA_GYRO_CONFIG));
+}
+
 static void cliDumpGyroRegisters(char *cmdline)
 {
-    tfp_printf("# WHO_AM_I    0x%X\r\n", gyroReadRegister(MPU_RA_WHO_AM_I));
+#ifdef USE_DUAL_GYRO
-    tfp_printf("# CONFIG      0x%X\r\n", gyroReadRegister(MPU_RA_CONFIG));
+    if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) {
-    tfp_printf("# GYRO_CONFIG 0x%X\r\n", gyroReadRegister(MPU_RA_GYRO_CONFIG));
+        tfp_printf("\r\n# Gyro 1\r\n");
+        cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1);
+    }
+    if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) {
+        tfp_printf("\r\n# Gyro 2\r\n");
+        cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_2);
+    }
+#else
+    cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1);
+#endif // USE_DUAL_GYRO
     UNUSED(cmdline);
 }
 #endif

src/main/sensors/gyro.c

@@ -223,6 +223,22 @@ const busDevice_t *gyroSensorBus(void)
 #endif
 }
 
+#ifdef USE_GYRO_REGISTER_DUMP
+const busDevice_t *gyroSensorBusByDevice(uint8_t whichSensor)
+{
+#ifdef USE_DUAL_GYRO
+    if (whichSensor == GYRO_CONFIG_USE_GYRO_2) {
+        return &gyroSensor2.gyroDev.bus;
+    } else {
+        return &gyroSensor1.gyroDev.bus;
+    }
+#else
+    UNUSED(whichSensor);
+    return &gyroSensor1.gyroDev.bus;
+#endif
+}
+#endif // USE_GYRO_REGISTER_DUMP
+
 const mpuConfiguration_t *gyroMpuConfiguration(void)
 {
 #ifdef USE_DUAL_GYRO
@@ -433,6 +449,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
 #endif
 
     const gyroSensor_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
+    gyroSensor->gyroDev.gyroHardware = gyroHardware;
     if (gyroHardware == GYRO_NONE) {
         return false;
     }
@@ -599,7 +616,21 @@ bool gyroInit(void)
         }
         gyroHasOverflowProtection =  gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection;
     }
-#endif
+#endif // USE_DUAL_GYRO
+
+#ifdef USE_DUAL_GYRO
+    // 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);
+
+        }
+    }
+#endif // USE_DUAL_GYRO
     return ret;
 }
 
@@ -1257,8 +1288,8 @@ uint16_t gyroAbsRateDps(int axis)
 }
 
 #ifdef USE_GYRO_REGISTER_DUMP
-uint8_t gyroReadRegister(uint8_t reg)
+uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
 {
-    return mpuGyroReadRegister(gyroSensorBus(), reg);
+    return mpuGyroReadRegister(gyroSensorBusByDevice(whichSensor), reg);
 }
-#endif
+#endif // USE_GYRO_REGISTER_DUMP

src/main/sensors/gyro.h

@@ -128,4 +128,4 @@ int16_t gyroRateDps(int axis);
 bool gyroOverflowDetected(void);
 bool gyroYawSpinDetected(void);
 uint16_t gyroAbsRateDps(int axis);
-uint8_t gyroReadRegister(uint8_t reg);
+uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg);

src/test/unit/rc_controls_unittest.cc

@@ -697,7 +697,10 @@ void initRcProcessing(void) {}
 void changePidProfile(uint8_t) {}
 void pidInitConfig(const pidProfile_t *) {}
 void accSetCalibrationCycles(uint16_t) {}
-void gyroStartCalibration(void) {}
+void gyroStartCalibration(bool isFirstArmingCalibration)
+{
+    UNUSED(isFirstArmingCalibration);
+}
 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
 void handleInflightCalibrationStickPosition(void) {}
 bool feature(uint32_t) { return false;}