From ff59c37fdb878c7896d4601df4c451cb85b553df Mon Sep 17 00:00:00 2001
From: Bruce Luckcuck <github@etracer.net>
Date: Thu, 3 May 2018 10:18:17 -0400
Subject: [PATCH] Only allow gyro_to_use = BOTH if both detected gyros are the
 same type Since we currently don't have per gyro configuration, trying to use
 two different gyro types simultaneously may not provide good results or lead
 to other unforeseen situations like attempting to initialize with settings
 not applicable to both hardware types.

This logic looks at the detected gyro types and resets to use only the first one if the user selected "BOTH" and they're different hardware types.
---
 src/main/drivers/accgyro/accgyro.h    |  3 ++-
 src/main/sensors/gyro.c               | 17 ++++++++++++++++-
 src/test/unit/rc_controls_unittest.cc |  5 ++++-
 3 files changed, 22 insertions(+), 3 deletions(-)

diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h
index 8af8f1c762..fe7560e136 100644
--- a/src/main/drivers/accgyro/accgyro.h
+++ b/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 {
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 8c903c721a..38eb04f356 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -437,6 +437,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;
     }
@@ -603,7 +604,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;
 }
 
diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc
index e498b70e33..3efcb7bbb5 100644
--- a/src/test/unit/rc_controls_unittest.cc
+++ b/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;}