blackbox: optionally log the second gyro on dual-IMU boards

docs/Blackbox.md

@@ -167,6 +167,12 @@ The CLI command `blackbox` allows setting which Blackbox fields are recorded to
 * `PEAKS_P` - Pitch axis noise peak
 * `PEAKS_Y` - Yaw axis noise peak
 * `SERVOS` - Servo outputs (for planes, tris, etc.)
+* `GYRO_2` - Raw Gyro data from the secondary IMU, on boards that carry two of them.
+  Requires `set gyro_secondary_enabled = ON`, which is what actually makes the flight
+  controller sample the second gyro; this flag only selects whether the samples are
+  logged. The secondary is instrumentation only: it never feeds attitude estimation
+  or the PID loops. Logged as `gyroRaw2[0..2]`, in deg/s, in the same body frame as
+  `gyroRaw`, with each sensor's own alignment already applied.
 
 Usage:
 

docs/Settings.md

@@ -2176,6 +2176,16 @@ Software based gyro main lowpass filter. Value is cutoff frequency (Hz)
 
 ---
 
+### gyro_secondary_enabled
+
+On multi-gyro targets, additionally sample the gyro NOT selected by `gyro_to_use`. The extra sample is exposed to Blackbox as `gyroRaw2` and is never used for attitude estimation or flight control. Intended for filter and estimator analysis. Costs one additional SPI transaction per gyro cycle.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### gyro_to_use
 
 On multi-gyro targets, allows to choose which gyro to use. 0 = first gyro, 1 = second gyro

src/main/blackbox/blackbox.c

@@ -353,6 +353,11 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
     {"gyroRaw",     0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
     {"gyroRaw",     1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
     {"gyroRaw",     2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
+#ifdef USE_DUAL_GYRO
+    {"gyroRaw2",    0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_SECONDARY},
+    {"gyroRaw2",    1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_SECONDARY},
+    {"gyroRaw2",    2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_SECONDARY},
+#endif
 
     {"gyroPeakRoll",    0, UNSIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL},
     {"gyroPeakRoll",    1, UNSIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL},
@@ -565,6 +570,9 @@ typedef struct blackboxMainState_s {
     int16_t rcCommand[4];
     int16_t gyroADC[XYZ_AXIS_COUNT];
     int16_t gyroRaw[XYZ_AXIS_COUNT];
+#ifdef USE_DUAL_GYRO
+    int16_t gyroRaw2[XYZ_AXIS_COUNT];
+#endif
 
     int16_t gyroPeaksRoll[DYN_NOTCH_PEAK_COUNT];
     int16_t gyroPeaksPitch[DYN_NOTCH_PEAK_COUNT];
@@ -674,7 +682,7 @@ static struct {
 // Cache for FLIGHT_LOG_FIELD_CONDITION_* test results:
 static uint64_t blackboxConditionCache;
 
-STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_LAST, too_many_flight_log_conditions);
+STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) > FLIGHT_LOG_FIELD_CONDITION_LAST, too_many_flight_log_conditions);
 
 static uint32_t blackboxIFrameInterval;
 static uint32_t blackboxIteration;
@@ -853,6 +861,11 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
     case FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW:
         return blackboxIncludeFlag(BLACKBOX_FEATURE_GYRO_RAW);
 
+#ifdef USE_DUAL_GYRO
+    case FLIGHT_LOG_FIELD_CONDITION_GYRO_SECONDARY:
+        return gyro.secondaryInitialized && blackboxIncludeFlag(BLACKBOX_FEATURE_GYRO_SECONDARY);
+#endif
+
     case FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL:
         return blackboxIncludeFlag(BLACKBOX_FEATURE_GYRO_PEAKS_ROLL);
 
@@ -1032,6 +1045,12 @@ static void writeIntraframe(void)
         blackboxWriteSigned16VBArray(blackboxCurrent->gyroRaw, XYZ_AXIS_COUNT);
     }
 
+#ifdef USE_DUAL_GYRO
+    if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_SECONDARY)) {
+        blackboxWriteSigned16VBArray(blackboxCurrent->gyroRaw2, XYZ_AXIS_COUNT);
+    }
+#endif
+
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL)) {
         blackboxWriteUnsignedVB(blackboxCurrent->gyroPeaksRoll[0]);
         blackboxWriteUnsignedVB(blackboxCurrent->gyroPeaksRoll[1]);
@@ -1306,6 +1325,12 @@ static void writeInterframe(void)
         blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, gyroRaw), XYZ_AXIS_COUNT);
     }
 
+#ifdef USE_DUAL_GYRO
+    if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_SECONDARY)) {
+        blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, gyroRaw2), XYZ_AXIS_COUNT);
+    }
+#endif
+
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL)) {
         blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, gyroPeaksRoll), DYN_NOTCH_PEAK_COUNT);
     }
@@ -1759,6 +1784,9 @@ static void loadMainState(timeUs_t currentTimeUs)
         blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
         blackboxCurrent->accADC[i] = constrain(lrintf(acc.accADCf[i] * acc.dev.acc_1G), -32678, 32767);
         blackboxCurrent->gyroRaw[i] = lrintf(gyro.gyroRaw[i]);
+#ifdef USE_DUAL_GYRO
+        blackboxCurrent->gyroRaw2[i] = lrintf(gyro.gyroRaw2[i]);
+#endif
 
 #ifdef USE_DYNAMIC_FILTERS
         for (uint8_t i = 0; i < DYN_NOTCH_PEAK_COUNT ; i++) {

src/main/blackbox/blackbox.h

@@ -36,6 +36,7 @@ typedef enum {
     BLACKBOX_FEATURE_GYRO_PEAKS_PITCH   = 1 << 11,
     BLACKBOX_FEATURE_GYRO_PEAKS_YAW     = 1 << 12,
     BLACKBOX_FEATURE_SERVOS             = 1 << 13,
+    BLACKBOX_FEATURE_GYRO_SECONDARY     = 1 << 14,
 } blackboxFeatureMask_e;
 
 typedef enum BlackboxState {

src/main/blackbox/blackbox_fielddefs.h

@@ -98,6 +98,9 @@ typedef enum FlightLogFieldCondition {
     FLIGHT_LOG_FIELD_CONDITION_RC_DATA,
     FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND,
     FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW,
+#ifdef USE_DUAL_GYRO
+    FLIGHT_LOG_FIELD_CONDITION_GYRO_SECONDARY,
+#endif
 
     FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL,
     FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH,

src/main/fc/cli.c

@@ -199,6 +199,9 @@ static const char * const blackboxIncludeFlagNames[] = {
     "PEAKS_P",
     "PEAKS_Y",
     "SERVOS",
+#ifdef USE_DUAL_GYRO
+    "GYRO_2",
+#endif
     NULL
 };
 #endif

src/main/fc/settings.yaml

@@ -334,6 +334,12 @@ groups:
         min: 0
         max: 2
         default_value: 0
+      - name: gyro_secondary_enabled
+        description: "On multi-gyro targets, additionally sample the gyro NOT selected by `gyro_to_use`. The extra sample is exposed to Blackbox as `gyroRaw2` and is never used for attitude estimation or flight control. Intended for filter and estimator analysis. Costs one additional SPI transaction per gyro cycle."
+        default_value: OFF
+        condition: USE_DUAL_GYRO
+        field: gyro_secondary_enabled
+        type: bool
       - name: setpoint_kalman_enabled
         description: "Enable Kalman filter on the gyro data"
         default_value: ON

src/main/sensors/gyro.c

@@ -78,7 +78,14 @@
 
 FASTRAM gyro_t gyro; // gyro sensor object
 
+#ifdef USE_DUAL_GYRO
+#define MAX_GYRO_COUNT 2
+/* Highest bus tag any in-tree target gives an IMU position. Targets disagree:
+ * most use 0 and 1, AETH743Basic uses 0 and 2. */
+#define MAX_GYRO_SENSOR_TAG 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];
@@ -111,6 +118,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .looptime = SETTING_LOOPTIME_DEFAULT,
 #ifdef USE_DUAL_GYRO
     .gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT,
+    .gyro_secondary_enabled = SETTING_GYRO_SECONDARY_ENABLED_DEFAULT,
 #endif
     .gyro_main_lpf_hz = SETTING_GYRO_MAIN_LPF_HZ_DEFAULT,
     .gyroDynamicLpfMinHz = SETTING_GYRO_DYN_LPF_MIN_HZ_DEFAULT,
@@ -349,6 +357,42 @@ bool gyroInit(void)
 
     gyroInitFilters();
 
+#ifdef USE_DUAL_GYRO
+    /*
+     * Optional secondary IMU, sampled purely as an instrumentation channel.
+     * Its output reaches Blackbox as gyroRaw2 and nothing else: attitude
+     * estimation and the PID loops keep using gyroDev[0] exclusively.
+     */
+    gyro.secondaryInitialized = false;
+    if (gyroConfig()->gyro_secondary_enabled) {
+        /*
+         * Do not assume the two IMU positions are tagged 0 and 1. Most targets
+         * do, but AETH743Basic registers them as 0 and 2, and a few register
+         * both positions with tag 0 - where not even gyro_to_use can reach the
+         * second one. Probe the candidate tags instead, skipping the one the
+         * primary already claimed, and leave the feature disabled if nothing
+         * else answers.
+         */
+        for (uint8_t tag = 0; tag <= MAX_GYRO_SENSOR_TAG; tag++) {
+            if (tag == gyroConfig()->gyro_to_use) {
+                continue;
+            }
+
+            gyroDev[1].imuSensorToUse = tag;
+            if (gyroDetect(&gyroDev[1], GYRO_AUTODETECT) == GYRO_NONE) {
+                continue;
+            }
+
+            gyroDev[1].lpf = GYRO_LPF_256HZ;
+            gyroDev[1].requestedSampleIntervalUs = TASK_GYRO_LOOPTIME;
+            gyroDev[1].sampleRateIntervalUs = TASK_GYRO_LOOPTIME;
+            gyroDev[1].initFn(&gyroDev[1]);
+            gyro.secondaryInitialized = true;
+            break;
+        }
+    }
+#endif
+
 #ifdef USE_DYNAMIC_FILTERS
     // Dynamic notch running at PID frequency
     dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
@@ -370,6 +414,36 @@ void gyroStartCalibration(void)
         return;
     }
 
+#ifdef USE_DUAL_GYRO
+    /*
+     * The secondary always measures its own zero offset. It deliberately
+     * ignores init_gyro_cal: the stored calibration belongs to the primary
+     * sensor and applying it here would bias the logged samples.
+     */
+    if (gyro.secondaryInitialized) {
+        /*
+         * The threshold is a number of raw counts, and raw counts mean different
+         * rotation rates on different parts - a dual-IMU board is free to pair two
+         * unrelated sensors. Converting through both scales asks the secondary for
+         * the same physical stillness the primary is asked for, rather than for the
+         * same number. Where the two sensors are the same part the scales cancel and
+         * this is exactly the old constant.
+         */
+        const float secondaryThreshold =
+            CALIBRATING_GYRO_MORON_THRESHOLD * gyroDev[0].scale / gyroDev[1].scale;
+
+        /*
+         * allowFailure is true here, unlike for the primary. Nothing gates arming on
+         * this sensor, so a calibration that keeps restarting on vibration would
+         * never finish and every logged sample would stay zero for the whole flight.
+         * Failing once and then logging the sensor with a zero offset keeps the
+         * channel useful: a constant bias can be removed in post-processing, a
+         * column of zeroes cannot.
+         */
+        zeroCalibrationStartV(&gyroCalibration[1], CALIBRATING_GYRO_TIME_MS, secondaryThreshold, true);
+    }
+#endif
+
 #ifndef USE_IMU_FAKE // fixes Test Unit compilation error
     if (!gyroConfig()->init_gyro_cal_enabled) {
         return;
@@ -395,7 +469,7 @@ bool gyroIsCalibrationComplete(void)
     return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]);
 }
 
-STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration)
+STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration, bool persist)
 {
     fpVector3_t v;
 
@@ -414,7 +488,13 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe
         dev->gyroZero[Z] = v.v[Z];
 
 #ifndef USE_IMU_FAKE // fixes Test Unit compilation error
-        setGyroCalibration(dev->gyroZero);
+        /* gyro_zero_cal is a single shared value: only the gyro that actually
+         * flies the aircraft is allowed to write it. */
+        if (persist) {
+            setGyroCalibration(dev->gyroZero);
+        }
+#else
+        UNUSED(persist);
 #endif
 
         // Cache completion status to avoid function call in hot path
@@ -439,16 +519,16 @@ void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate)
     }
 }
 
-static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroCalibrationVector_t * gyroCal, float * gyroADCf)
+static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroCalibrationVector_t * gyroCal, float * gyroADCf, bool isPrimary)
 {
 
     // range: +/- 8192; +/- 2000 deg/sec
     if (gyroDev->readFn(gyroDev)) {
 
 #ifndef USE_IMU_FAKE // fixes Test Unit compilation error
-    if (!gyroConfig()->init_gyro_cal_enabled) {
+    if (isPrimary && !gyroConfig()->init_gyro_cal_enabled) {
         // marks that the gyro calibration has ended
-        gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE;
+        gyroCal->params.state = ZERO_CALIBRATION_DONE;
         gyroCalibrationComplete = true;
         // pass the calibration values
         gyroDev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X];
@@ -473,7 +553,7 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC
 
             return true;
         } else {
-            performGyroCalibration(gyroDev, gyroCal);
+            performGyroCalibration(gyroDev, gyroCal, isPrimary);
 
             // Reset gyro values to zero to prevent other code from using uncalibrated data
             gyroADCf[X] = 0.0f;
@@ -578,7 +658,21 @@ void FAST_CODE NOINLINE gyroUpdate(void)
         return;
     }
 
-    if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) {
+#ifdef USE_DUAL_GYRO
+    /*
+     * Read the secondary before the primary's early return, so that a stalled
+     * or uncalibrated secondary can never suppress the primary sample.
+     */
+    if (gyro.secondaryInitialized) {
+        if (!gyroUpdateAndCalibrate(&gyroDev[1], &gyroCalibration[1], gyro.gyroRaw2, false)) {
+            gyro.gyroRaw2[X] = 0.0f;
+            gyro.gyroRaw2[Y] = 0.0f;
+            gyro.gyroRaw2[Z] = 0.0f;
+        }
+    }
+#endif
+
+    if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf, true)) {
         return;
     }
 

src/main/sensors/gyro.h

@@ -65,6 +65,12 @@ typedef struct gyro_s {
     uint32_t targetLooptime;
     float gyroADCf[XYZ_AXIS_COUNT];
     float gyroRaw[XYZ_AXIS_COUNT];
+#ifdef USE_DUAL_GYRO
+    /* Secondary IMU. Sampled for logging and analysis only:
+     * never feeds attitude estimation or the PID loops. */
+    float gyroRaw2[XYZ_AXIS_COUNT];
+    bool  secondaryInitialized;
+#endif
 } gyro_t;
 
 extern gyro_t gyro;
@@ -107,6 +113,24 @@ typedef struct gyroConfig_s {
 
     uint8_t gyroLuluSampleCount;
     bool gyroLuluEnabled;
+#ifdef USE_DUAL_GYRO
+    /* Deliberately appended at the end of the struct. pgLoad() only compares the
+     * parameter group version, never the size: it installs the defaults and
+     * then copies MIN(stored, current) bytes over them. Appending therefore
+     * leaves every pre-existing setting at its offset and needs no version
+     * bump, so upgrading does not discard the user's gyro configuration, while
+     * inserting mid-struct would silently shift every following field.
+     *
+     * Appending is not unconditionally free, though, and the exception is worth
+     * stating because it is invisible: if the new field lands inside the old
+     * struct's tail padding it is still within what an older configuration
+     * stored, and it is overwritten with the zero that padding holds -
+     * pgResetInstance() copies the reset template whole, padding included. This
+     * field defaults to OFF, so zero is the default and the overlap cannot
+     * matter. A field whose default were non-zero would need a filler byte
+     * ahead of it. */
+    bool     gyro_secondary_enabled;
+#endif
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);