WIP: Multi gyro support

src/main/fc/settings.yaml

@@ -144,6 +144,9 @@ tables:
   - name: osd_ahi_style
     values: ["DEFAULT", "LINE"]
     enum: osd_ahi_style_e
+  - name: gyro_to_use_t
+    values: ["FIRST", "SECOND", "BOTH"]
+    enum: gyro_to_use_e
 
 groups:
   - name: PG_GYRO_CONFIG
@@ -159,6 +162,10 @@ groups:
         field: gyro_align
         type: uint8_t
         table: alignment
+      - name: align_gyro2
+        field: gyro2_align
+        type: uint8_t
+        table: alignment
       - name: gyro_hardware_lpf
         field: gyro_lpf
         table: gyro_lpf
@@ -211,9 +218,10 @@ groups:
         min: 30
         max: 1000
       - name: gyro_to_use
+        table: gyro_to_use_t
+        field: gyro_to_use
         condition: USE_DUAL_GYRO
-        min: 0
-        max: 1
+
 
   - name: PG_ADC_CHANNEL_CONFIG
     type: adcChannelConfig_t
@@ -251,6 +259,10 @@ groups:
         field: acc_align
         type: uint8_t
         table: alignment
+      - name: align_acc2
+        field: acc2_align
+        type: uint8_t
+        table: alignment
       - name: acc_hardware
         table: acc_hardware
       - name: acc_lpf_hz

src/main/sensors/acceleration.c

@@ -286,9 +286,14 @@ bool accInit(uint32_t targetLooptime)
 {
     memset(&acc, 0, sizeof(acc));
 
-    // Set inertial sensor tag (for dual-gyro selection)
+
+// Set inertial sensor tag (for dual-gyro selection)
 #ifdef USE_DUAL_GYRO
-    acc.dev.imuSensorToUse = gyroConfig()->gyro_to_use;     // Use the same selection from gyroConfig()
+    acc.dev.imuSensorToUse = gyroConfig()->gyro_to_use;
+#ifdef USE_MULTI_GYRO //TODO: Fixme to
+    if(gyroConfig()->gyro_to_use == BOTH )
+        acc.dev.imuSensorToUse = 0;
+#endif
 #else
     acc.dev.imuSensorToUse = 0;
 #endif
@@ -310,8 +315,23 @@ bool accInit(uint32_t targetLooptime)
 
     // At this poinrt acc.dev.accAlign was set up by the driver from the busDev record
     // If configuration says different - override
-    if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) {
-        acc.dev.accAlign = accelerometerConfig()->acc_align;
+    switch(gyroConfig()->gyro_to_use ){
+        case FIRST:
+        default:
+#ifdef USE_MULTI_GYRO
+        case BOTH:
+#endif
+            if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) {
+                acc.dev.accAlign = accelerometerConfig()->acc_align;
+            }
+            break;
+#ifdef USE_DUAL_GYRO
+        case SECOND:
+            if (accelerometerConfig()->acc2_align != ALIGN_DEFAULT) {
+                acc.dev.accAlign = accelerometerConfig()->acc2_align;
+            }
+            break;
+#endif
     }
     return true;
 }

src/main/sensors/acceleration.h

@@ -69,6 +69,7 @@ extern acc_t acc;
 
 typedef struct accelerometerConfig_s {
     sensor_align_e acc_align;               // acc alignment
+    sensor_align_e acc2_align;               // acc 2 alignment
     uint8_t acc_hardware;                   // Which acc hardware to use on boards with more than one device
     uint16_t acc_lpf_hz;                    // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
     flightDynamicsTrims_t accZero;          // Accelerometer offset

src/main/sensors/gyro.c

@@ -78,8 +78,12 @@ FILE_COMPILE_FOR_SPEED
 #endif
 
 FASTRAM gyro_t gyro; // gyro sensor object
-
+#ifdef USE_MULTI_GYRO
+#define MAX_GYRO_COUNT          2
+#else
 #define MAX_GYRO_COUNT          1
+#endif
+
 
 STATIC_UNIT_TESTED gyroDev_t gyroDev[MAX_GYRO_COUNT];  // Not in FASTRAM since it may hold DMA buffers
 STATIC_FASTRAM int16_t gyroTemperature[MAX_GYRO_COUNT];
@@ -113,7 +117,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyroMovementCalibrationThreshold = 32,
     .looptime = 1000,
     .gyroSync = 1,
-    .gyro_to_use = 0,
+    .gyro_to_use = FIRST,
     .gyro_soft_notch_hz_1 = 0,
     .gyro_soft_notch_cutoff_1 = 1,
     .gyro_soft_notch_hz_2 = 0,
@@ -285,40 +289,89 @@ static void gyroInitFilters(void)
 bool gyroInit(void)
 {
     memset(&gyro, 0, sizeof(gyro));
-
     // Set inertial sensor tag (for dual-gyro selection)
+    gyroSensor_e gyroHardware;
+#ifdef USE_MULTI_GYRO
+    gyroSensor_e gyro2Hardware;
+#endif
+    switch (gyroConfig()->gyro_to_use){
+        case FIRST:
+        case SECOND:
 #ifdef USE_DUAL_GYRO
-    gyroDev[0].imuSensorToUse = gyroConfig()->gyro_to_use;
+            gyroDev[0].imuSensorToUse = gyroConfig()->gyro_to_use;
 #else
-    gyroDev[0].imuSensorToUse = 0;
+            gyroDev[0].imuSensorToUse = 0;
 #endif
 
-    // Detecting gyro0
-    gyroSensor_e gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT);
-    if (gyroHardware == GYRO_NONE) {
-        gyro.initialized = false;
-        detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE;
-        return true;
-    }
+            // Detecting gyro0
+            gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT);
+            if (gyroHardware == GYRO_NONE) {
+                gyro.initialized = false;
+                detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE;
+                return true;
+            }
+            break;
 
-    // Gyro is initialized
+#ifdef USE_MULTI_GYRO
+        case BOTH:
+            gyroDev[0].imuSensorToUse = FIRST;
+            gyroDev[1].imuSensorToUse = SECOND;
+            gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT);
+            gyro2Hardware = gyroDetect(&gyroDev[1], GYRO_AUTODETECT);
+
+            if (gyroHardware == GYRO_NONE || gyro2Hardware == GYRO_NONE)  {
+                gyro.initialized = false;
+                    detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE;
+                return true;
+            }
+            break;
+
+#endif
+        default:
+            return false;
+    };
     gyro.initialized = true;
-    detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
+        detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
     sensorsSet(SENSOR_GYRO);
-
     // Driver initialisation
     gyroDev[0].lpf = gyroConfig()->gyro_lpf;
     gyroDev[0].requestedSampleIntervalUs = gyroConfig()->looptime;
     gyroDev[0].sampleRateIntervalUs = gyroConfig()->looptime;
     gyroDev[0].initFn(&gyroDev[0]);
 
     // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value
-    gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime;
+    gyro.targetLooptime = (gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime);
+#ifdef USE_MULTI_GYRO
+    if (gyroConfig()->gyro_to_use == BOTH) {
+        gyroDev[1].lpf = gyroConfig()->gyro_lpf;
+        gyroDev[1].requestedSampleIntervalUs = gyroConfig()->looptime;
+        gyroDev[1].sampleRateIntervalUs = gyroConfig()->looptime;
+        gyroDev[1].initFn(&gyroDev[1]);
+        gyro.targetLooptime = (gyroConfig()->gyroSync ? MAX(gyroDev[0].sampleRateIntervalUs, gyroDev[1].sampleRateIntervalUs) : gyroConfig()->looptime);
+    }
+#endif
 
-    // At this poinrt gyroDev[0].gyroAlign was set up by the driver from the busDev record
-    // If configuration says different - override
-    if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
-        gyroDev[0].gyroAlign = gyroConfig()->gyro_align;
+
+    switch (gyroConfig()->gyro_to_use){
+    case FIRST:
+    default:
+        if (gyroConfig()->gyro_align != ALIGN_DEFAULT)
+            gyroDev[0].gyroAlign = gyroConfig()->gyro_align;
+        break;
+#ifdef USE_DUAL_GYRO
+    case SECOND:
+        if (gyroConfig()->gyro2_align != ALIGN_DEFAULT)
+            gyroDev[0].gyroAlign = gyroConfig()->gyro2_align;
+        break;
+#endif
+#ifdef USE_MULTI_GYRO
+    case BOTH:
+        if (gyroConfig()->gyro_align != ALIGN_DEFAULT)
+            gyroDev[0].gyroAlign = gyroConfig()->gyro_align;
+        if (gyroConfig()->gyro2_align != ALIGN_DEFAULT)
+            gyroDev[1].gyroAlign = gyroConfig()->gyro2_align;
+        break;
+#endif
     }
 
     gyroInitFilters();
@@ -341,14 +394,22 @@ void gyroStartCalibration(void)
     }
 
     zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
+#ifdef USE_MULTI_GYRO
+    if(gyroConfig()->gyro_to_use == BOTH)
+        zeroCalibrationStartV(&gyroCalibration[1], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
+#endif
 }
 
 bool gyroIsCalibrationComplete(void)
 {
     if (!gyro.initialized) {
         return true;
     }
-
+#ifdef USE_MULTI_GYRO
+    if(gyroConfig()->gyro_to_use == BOTH )
+        return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]) &&
+        zeroCalibrationIsCompleteV(&gyroCalibration[1]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[1]);
+#endif
     return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]);
 }
 
@@ -437,6 +498,15 @@ void FAST_CODE NOINLINE gyroUpdate()
     if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) {
         return;
     }
+#ifdef USE_MULTI_GYRO
+    if (gyroConfig()->gyro_to_use == BOTH ) {
+        if(!gyroUpdateAndCalibrate(&gyroDev[1], &gyroCalibration[1], gyro.gyro2ADCf))
+            return;
+        gyro.gyroADCf[X] = (gyro.gyroADCf[X] + gyro.gyro2ADCf[X]) / 2.0f;
+        gyro.gyroADCf[Y] = (gyro.gyroADCf[Y] + gyro.gyro2ADCf[Y]) / 2.0f;
+        gyro.gyroADCf[Z] = (gyro.gyroADCf[Z] + gyro.gyro2ADCf[Z]) / 2.0f;
+    }
+#endif
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         // At this point gyro.gyroADCf contains unfiltered gyro value [deg/s]
@@ -489,6 +559,13 @@ bool gyroReadTemperature(void)
     }
 
     // Read gyro sensor temperature. temperatureFn returns temperature in [degC * 10]
+    // TODO: [degC * 10] is a bug in Finland. Negative temperature...
+#ifdef USE_MULTI_GYRO
+    if (gyroConfig()->gyro_to_use == BOTH && gyroDev[0].temperatureFn && gyroDev[1].temperatureFn)
+        return MAX(gyroDev[0].temperatureFn(&gyroDev[0], &gyroTemperature[0]), gyroDev[1].temperatureFn(&gyroDev[1], &gyroTemperature[1]));
+    else if (gyroConfig()->gyro_to_use == BOTH)
+        return false;
+#endif
     if (gyroDev[0].temperatureFn) {
         return gyroDev[0].temperatureFn(&gyroDev[0], &gyroTemperature[0]);
     }
@@ -501,7 +578,10 @@ int16_t gyroGetTemperature(void)
     if (!gyro.initialized) {
         return 0;
     }
-
+#ifdef USE_MULTI_GYRO
+    if (gyroConfig()->gyro_to_use == BOTH)
+        return MAX(gyroTemperature[0], gyroTemperature[1]);
+#endif
     return gyroTemperature[0];
 }
 
@@ -524,5 +604,12 @@ bool gyroSyncCheckUpdate(void)
         return false;
     }
 
+#ifdef USE_MULTI_GYRO
+    if(gyroConfig()->gyro_to_use == BOTH) {
+        if (!gyroDev[1].intStatusFn)
+            return false;
+        return gyroDev[0].intStatusFn(&gyroDev[0]) && gyroDev[1].intStatusFn(&gyroDev[1]);
+    }
+#endif
     return gyroDev[0].intStatusFn(&gyroDev[0]);
 }

src/main/sensors/gyro.h

@@ -45,6 +45,12 @@ typedef enum {
     DYN_NOTCH_RANGE_LOW
 } dynamicFilterRange_e;
 
+typedef enum {
+    FIRST = 0,
+    SECOND = 1,
+    BOTH = 2
+} gyro_to_use_e;
+
 #define DYN_NOTCH_RANGE_HZ_HIGH 2000
 #define DYN_NOTCH_RANGE_HZ_MEDIUM 1333
 #define DYN_NOTCH_RANGE_HZ_LOW 1000
@@ -53,12 +59,16 @@ typedef struct gyro_s {
     bool initialized;
     uint32_t targetLooptime;
     float gyroADCf[XYZ_AXIS_COUNT];
+#ifdef USE_MULTI_GYRO
+    float gyro2ADCf[XYZ_AXIS_COUNT];
+#endif
 } gyro_t;
 
 extern gyro_t gyro;
 
 typedef struct gyroConfig_s {
     sensor_align_e gyro_align;              // gyro alignment
+    sensor_align_e gyro2_align;              // second gyro alignment
     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.
     uint8_t  gyroSync;                      // Enable interrupt based loop
     uint16_t looptime;                      // imu loop time in us

src/main/target/IFLIGHTF7_TWING/target.c

@@ -37,11 +37,11 @@ const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
     DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
 
-    DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),
-    DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), 
+    DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),
+    DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_SERVO | TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),
 
-    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
-    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
 
     DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED,                         0, 0),
 };

src/main/target/IFLIGHTF7_TWING/target.h

@@ -36,7 +36,7 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-#define USE_DUAL_GYRO
+#define USE_MULTI_GYRO
 
 #define USE_IMU_MPU6500
 #define IMU_0_ALIGN       CW90_DEG

src/main/target/common_post.h

@@ -39,6 +39,10 @@
     #define USE_RPM_FILTER
 #endif
 
+#ifdef USE_MULTI_GYRO
+    #define USE_DUAL_GYRO
+#endif
+
 #ifdef USE_ITCM_RAM
 #define FAST_CODE                   __attribute__((section(".tcm_code")))
 #define NOINLINE                    __NOINLINE