Multi gyro support

src/main/fc/fc_msp.c

@@ -1131,7 +1131,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
 
     case MSP_SENSOR_ALIGNMENT:
-        sbufWriteU8(dst, gyroConfig()->gyro_align);
+        sbufWriteU8(dst, gyroConfig()->gyro_align[0]);
         sbufWriteU8(dst, accelerometerConfig()->acc_align);
         sbufWriteU8(dst, compassConfig()->mag_align);
 #ifdef USE_OPFLOW
@@ -2003,7 +2003,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
     case MSP_SET_SENSOR_ALIGNMENT:
         if (dataSize >= 4) {
-            gyroConfigMutable()->gyro_align = sbufReadU8(src);
+            gyroConfigMutable()->gyro_align[0] = sbufReadU8(src);
             accelerometerConfigMutable()->acc_align = sbufReadU8(src);
 #ifdef USE_MAG
             compassConfigMutable()->mag_align = sbufReadU8(src);

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
@@ -155,8 +158,12 @@ groups:
       - name: gyro_sync
         field: gyroSync
         type: bool
-      - name: align_gyro
-        field: gyro_align
+      - name: align_gyro1
+        field: gyro_align[0]
+        type: uint8_t
+        table: alignment
+      - name: align_gyro2
+        field: gyro_align[1]
         type: uint8_t
         table: alignment
       - name: gyro_hardware_lpf
@@ -204,9 +211,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
@@ -244,6 +252,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,11 +286,18 @@ 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
+    // FIXME: Dual acc support
+    if (gyroConfig()->gyro_to_use == IMU_TO_USE_BOTH) {
+        acc.dev.imuSensorToUse = IMU_TO_USE_FIRST;
+    }
+#endif
 #else
-    acc.dev.imuSensorToUse = 0;
+    acc.dev.imuSensorToUse = IMU_TO_USE_FIRST;
 #endif
 
     if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
@@ -310,8 +317,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