From f960d9838de357be47d51d1c4e86b938da92b6c4 Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Sat, 12 Sep 2026 15:03:42 +0200 Subject: [PATCH 1/7] gyro: honour the zeroCalibrationVector_t passed to gyroUpdateAndCalibrate() The function takes the calibration state to operate on as a parameter, but one line wrote to gyroCalibration[0] directly instead of the argument. There is currently a single call site and it passes &gyroCalibration[0], so this has no effect today. It becomes wrong the moment a second call site exists with a different calibration: the wrong one is marked ZERO_CALIBRATION_DONE. --- src/main/sensors/gyro.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 8544b45dac1..75f75f2482a 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -430,7 +430,7 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC #ifndef USE_IMU_FAKE // fixes Test Unit compilation error if (!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; // pass the calibration values gyroDev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; gyroDev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y]; From 7a8d8342d2403b2972942f2a408c3755a5e6385a Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Sat, 12 Sep 2026 15:03:43 +0200 Subject: [PATCH 2/7] blackbox: optionally log the second gyro on dual-IMU boards On boards with two IMUs, gyro_to_use selects one of them and only gyroDev[0] is ever sampled. The second sensor is powered but never read. Add gyro_secondary_enabled, which samples the other IMU purely as an instrumentation channel, and the GYRO_2 blackbox include flag, which logs it as gyroRaw2. Both default to off. The two are separate switches because the costs differ: one is an extra SPI transaction per gyro cycle, the other is log bandwidth. The secondary never reaches attitude estimation or the PID loops. It is read before the primary's early return, so a stalled or uncalibrated secondary cannot suppress the sample that flies the aircraft. Calibration ownership had to be made explicit. gyroConfig()->gyro_zero_cal[] is a single shared triple that performGyroCalibration() rewrites on completion, so a second calibrating gyro could persist its own bias over the primary's. performGyroCalibration() now takes persist and only the primary writes the config; gyroUpdateAndCalibrate() takes isPrimary and the secondary always measures its own zero, ignoring init_gyro_cal, whose stored value belongs to the other sensor. Everything is under USE_DUAL_GYRO, including the field definitions, the condition enum entry and the CLI flag name. On single-IMU targets isPrimary is a compile-time constant and LTO removes the parameters: AIKONF7, at 93% flash, does not grow. The condition enum now ends at exactly 64 entries, so this consumes the last bit of the uint64_t condition cache. The STATIC_ASSERT guarding that compared with >= and would have let the next addition through as 1ULL << 64; tightened to >. --- docs/Blackbox.md | 6 +++ docs/Settings.md | 10 ++++ src/main/blackbox/blackbox.c | 30 +++++++++++- src/main/blackbox/blackbox.h | 1 + src/main/blackbox/blackbox_fielddefs.h | 3 ++ src/main/fc/cli.c | 3 ++ src/main/fc/settings.yaml | 6 +++ src/main/sensors/gyro.c | 65 +++++++++++++++++++++++--- src/main/sensors/gyro.h | 16 +++++++ 9 files changed, 133 insertions(+), 7 deletions(-) diff --git a/docs/Blackbox.md b/docs/Blackbox.md index 45900a4c08d..a95d6c41fc6 100644 --- a/docs/Blackbox.md +++ b/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: diff --git a/docs/Settings.md b/docs/Settings.md index 3d7351dc371..87c0c0cb223 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1932,6 +1932,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 diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f5225ca3e1e..4d216604fd7 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -295,6 +295,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}, @@ -498,6 +503,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]; @@ -600,7 +608,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; @@ -779,6 +787,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); @@ -956,6 +969,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]); @@ -1229,6 +1248,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); } @@ -1644,6 +1669,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++) { diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 1901201fa22..19b864087c7 100644 --- a/src/main/blackbox/blackbox.h +++ b/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 { diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 17595157dd2..8dc8f5defdd 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/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, diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 9bb2c776883..8d50677e167 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -192,6 +192,9 @@ static const char * const blackboxIncludeFlagNames[] = { "PEAKS_P", "PEAKS_Y", "SERVOS", +#ifdef USE_DUAL_GYRO + "GYRO_2", +#endif NULL }; #endif diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4e8affb0221..1fb8dd31eba 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -323,6 +323,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 diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 75f75f2482a..3ac712a1a8f 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -77,7 +77,11 @@ FASTRAM gyro_t gyro; // gyro sensor object +#ifdef USE_DUAL_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]; @@ -107,6 +111,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, @@ -335,6 +340,25 @@ 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) { + gyroDev[1].imuSensorToUse = (gyroConfig()->gyro_to_use == 0) ? 1 : 0; + if (gyroDetect(&gyroDev[1], GYRO_AUTODETECT) != GYRO_NONE) { + 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; + } + } +#endif + #ifdef USE_DYNAMIC_FILTERS // Dynamic notch running at PID frequency dynamicGyroNotchFiltersInit(&dynamicGyroNotchState); @@ -356,6 +380,17 @@ 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) { + zeroCalibrationStartV(&gyroCalibration[1], CALIBRATING_GYRO_TIME_MS, CALIBRATING_GYRO_MORON_THRESHOLD, false); + } +#endif + #ifndef USE_IMU_FAKE // fixes Test Unit compilation error if (!gyroConfig()->init_gyro_cal_enabled) { return; @@ -380,7 +415,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; @@ -399,7 +434,11 @@ 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); + } #endif LOG_DEBUG(GYRO, "Gyro calibration complete (%d, %d, %d)", (int16_t) dev->gyroZero[X], (int16_t) dev->gyroZero[Y], (int16_t) dev->gyroZero[Z]); @@ -421,14 +460,14 @@ 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 gyroCal->params.state = ZERO_CALIBRATION_DONE; // pass the calibration values @@ -453,7 +492,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; @@ -558,7 +597,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; } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 18fe5d9e517..38abc070b08 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -64,6 +64,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; @@ -106,6 +112,16 @@ 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, and then memcpy()s + * MIN(stored, current) bytes. A field added here is therefore purely + * additive: every pre-existing setting keeps its offset, the new one keeps + * the default installed by pgReset(), and no version bump is needed - so + * upgrading does not discard the user's gyro configuration. Inserting it + * mid-struct would silently shift every following field. */ + bool gyro_secondary_enabled; +#endif } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); From 5b49c628c47d54c32353cae516a9eaf80d339ceb Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Sat, 12 Sep 2026 16:43:41 +0200 Subject: [PATCH 3/7] gyro: address review findings on the secondary-gyro path Three issues raised in review on the original commit: 1. Secondary sensor selection assumed the two IMU positions are always tagged 0 and 1. They are not: AETH743Basic registers them as 0 and 2, and some targets register both with tag 0, where not even gyro_to_use can reach the second one. Probe the candidate tags and skip the primary's instead of inverting arithmetically, so the feature works on the 0-and-2 layout and stays cleanly disabled where there is nothing to find. 2. performGyroCalibration()'s new persist parameter was referenced only inside #ifndef USE_IMU_FAKE. SITL defines USE_IMU_FAKE and builds with -Wall -Wextra -Werror, so -Wunused-parameter broke that target. Verified by building SITL with WARNINGS_AS_ERRORS=ON before and after. 3. The secondary's zero calibration was started with allowFailure = false. Nothing gates arming on that sensor, so under sustained vibration the calibration restarts forever and every logged sample stays zero for the whole flight. Allow it to fail once and then log the sensor with a zero offset: a constant bias can be removed in post-processing, a column of zeroes cannot. --- src/main/sensors/gyro.c | 36 +++++++++++++++++++++++++++++++++--- 1 file changed, 33 insertions(+), 3 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 3ac712a1a8f..fd5d418a023 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -79,6 +79,9 @@ 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 @@ -348,13 +351,30 @@ bool gyroInit(void) */ gyro.secondaryInitialized = false; if (gyroConfig()->gyro_secondary_enabled) { - gyroDev[1].imuSensorToUse = (gyroConfig()->gyro_to_use == 0) ? 1 : 0; - if (gyroDetect(&gyroDev[1], GYRO_AUTODETECT) != GYRO_NONE) { + /* + * 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 @@ -387,7 +407,15 @@ void gyroStartCalibration(void) * sensor and applying it here would bias the logged samples. */ if (gyro.secondaryInitialized) { - zeroCalibrationStartV(&gyroCalibration[1], CALIBRATING_GYRO_TIME_MS, CALIBRATING_GYRO_MORON_THRESHOLD, false); + /* + * 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, CALIBRATING_GYRO_MORON_THRESHOLD, true); } #endif @@ -439,6 +467,8 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe if (persist) { setGyroCalibration(dev->gyroZero); } +#else + UNUSED(persist); #endif LOG_DEBUG(GYRO, "Gyro calibration complete (%d, %d, %d)", (int16_t) dev->gyroZero[X], (int16_t) dev->gyroZero[Y], (int16_t) dev->gyroZero[Z]); From 4517c9eb7aee37a72b969f676d733ebce42d8e00 Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Sun, 13 Sep 2026 22:12:33 +0200 Subject: [PATCH 4/7] gyro: scale the secondary calibration threshold to its own sensor The movement 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. Passing the constant unchanged asked the secondary for the same number rather than the same physical stillness, so a more sensitive secondary could fail a calibration the primary passes. Converting through both scales asks for the same stillness. Where the two sensors are the same part the scales cancel and the value is exactly the old constant, so nothing changes on the boards this has been tested on. This is the same class of bug as #11905, which fixes it for the primary by moving the constant into dps. That change and this one both edit gyroStartCalibration(); once it lands, both call sites become the same expression over each sensor's own scale. --- src/main/sensors/gyro.c | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index fd5d418a023..9b4dce55efe 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -408,14 +408,25 @@ void gyroStartCalibration(void) */ if (gyro.secondaryInitialized) { /* - * 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. + * 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. */ - zeroCalibrationStartV(&gyroCalibration[1], CALIBRATING_GYRO_TIME_MS, CALIBRATING_GYRO_MORON_THRESHOLD, true); + 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 From f6493a3a3710778f483d525a29dd99edd466e4c4 Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Mon, 14 Sep 2026 11:44:46 +0200 Subject: [PATCH 5/7] gyro: say where appending to a parameter group stops being free The comment claimed a field appended to gyroConfig_t keeps the default pgReset() installed. That is not unconditionally true, and the exception is invisible: pgLoad() copies MIN(stored, current) bytes over the defaults, so a new field that lands inside the old struct's tail padding is still within what an older configuration stored, and comes back as the zero that padding holds - pgResetInstance() copies the reset template whole, padding included. Nothing changes here, because this field defaults to OFF and zero is therefore the right answer either way. The comment now says that, rather than stating a rule that happens to hold for this field and would mislead anyone appending one whose default is not zero. No functional change. SITL builds clean. --- src/main/sensors/gyro.h | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 38abc070b08..1404a002ffd 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -114,12 +114,20 @@ typedef struct gyroConfig_s { bool gyroLuluEnabled; #ifdef USE_DUAL_GYRO /* Deliberately appended at the end of the struct. pgLoad() only compares the - * parameter group version, never the size, and then memcpy()s - * MIN(stored, current) bytes. A field added here is therefore purely - * additive: every pre-existing setting keeps its offset, the new one keeps - * the default installed by pgReset(), and no version bump is needed - so - * upgrading does not discard the user's gyro configuration. Inserting it - * mid-struct would silently shift every following field. */ + * 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; From e5a43019ce8c1f570760114903fabfc7517d3d6b Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Wed, 16 Sep 2026 16:30:10 +0200 Subject: [PATCH 6/7] Drop a line-ending-only change to an unrelated target SYNERDUINOH7/CMakeLists.txt is committed upstream with a CRLF ending while .gitattributes marks it as text, so git reports it modified on every Windows checkout and `git add -A` carries it along. It has nothing to do with the second gyro; put the upstream blob back. --- src/main/target/SYNERDUINOH7/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/SYNERDUINOH7/CMakeLists.txt b/src/main/target/SYNERDUINOH7/CMakeLists.txt index cbe6094b6bf..2b73ef8d8ea 100644 --- a/src/main/target/SYNERDUINOH7/CMakeLists.txt +++ b/src/main/target/SYNERDUINOH7/CMakeLists.txt @@ -1 +1 @@ -target_stm32h743xi(SYNERDUINOH7 HSE_MHZ 25 SKIP_RELEASES) +target_stm32h743xi(SYNERDUINOH7 HSE_MHZ 25 SKIP_RELEASES) From a5240434521136c55a8142bfd9f7ad412855e06d Mon Sep 17 00:00:00 2001 From: MrScothh <167884257+MrScothh@users.noreply.github.com> Date: Thu, 17 Sep 2026 17:59:44 +0200 Subject: [PATCH 7/7] Give the secondary gyro its own calibration state, and one switch instead of two All of this lives in the path that only exists once the second IMU is enabled. The cached "calibration complete" flag was a single bool shared by both sensors. The secondary starts its window first and is read first, so it always reached the end of the window first and raised the flag; the primary then skipped its own final sample, kept the zero it had been handed while still calibrating, and left gyroCalibration[0] in progress for good. That is what gyroIsCalibrationComplete() reports, and what areSensorsCalibrating() blocks arming on, so a board with the second gyro enabled would never arm. There is now one flag per sensor. Reproduced in SITL: with the setting off the arming flags clear after a second, with it on the sensors-calibrating bit used to stay up for as long as you cared to watch, and now clears in the same second. The feature also had two switches, gyro_secondary_enabled and a GYRO_2 Blackbox include flag. With the first on and the second off the board detected, calibrated and read the second sensor every gyro cycle and threw the numbers away. The include flag is gone and the setting decides both: on a board where the sensor is not sampled there is nothing to include, and while it is off the second IMU is not initialised at all. Finally, the secondary's calibration is asked to succeed rather than allowed to fail, so a window that ends on a moving aircraft restarts exactly as the primary's does and the two finish together on a still one. A channel meant to be compared against the primary should have its zero measured the same way. Nothing arms on this sensor, so the retry cannot keep the aircraft on the ground; what it costs is that a model which never sits still logs zeroes rather than a biased column. --- docs/Blackbox.md | 12 ++--- docs/Settings.md | 2 +- src/main/blackbox/blackbox.c | 2 +- src/main/blackbox/blackbox.h | 1 - src/main/fc/cli.c | 3 -- src/main/fc/settings.yaml | 2 +- src/main/sensors/gyro.c | 87 +++++++++++------------------------- src/main/sensors/gyro.h | 21 ++------- 8 files changed, 40 insertions(+), 90 deletions(-) diff --git a/docs/Blackbox.md b/docs/Blackbox.md index fd13d194249..5cafdd3d08a 100644 --- a/docs/Blackbox.md +++ b/docs/Blackbox.md @@ -167,12 +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. + +On a board with two IMUs, `gyro_secondary_enabled` adds `gyroRaw2[0..2]`, the second +sensor's rates in deg/s, in the same body frame as `gyroRaw` and with that sensor's +own alignment already applied. It has no flag of its own here: the setting is what +makes the flight controller sample the second gyro at all, so there is nothing to log +unless it is on. Usage: diff --git a/docs/Settings.md b/docs/Settings.md index 6bcfd299415..49193d362fd 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2178,7 +2178,7 @@ 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. +On a board with two IMUs, also sample the one `gyro_to_use` did not select, and log it to Blackbox as `gyroRaw2`. Instrumentation only: it never reaches attitude estimation or the PID loops. While this is off the second IMU is not initialised at all, so nothing else about the board changes. While it is on it costs one extra SPI transaction per gyro cycle. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index fe746ee57e8..01a37c06a6c 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -863,7 +863,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) #ifdef USE_DUAL_GYRO case FLIGHT_LOG_FIELD_CONDITION_GYRO_SECONDARY: - return gyro.secondaryInitialized && blackboxIncludeFlag(BLACKBOX_FEATURE_GYRO_SECONDARY); + return gyro.secondaryInitialized; #endif case FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL: diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 18823f1ba46..97829df9cca 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -36,7 +36,6 @@ 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 { diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index c6e847452c3..f2e446077c0 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -199,9 +199,6 @@ static const char * const blackboxIncludeFlagNames[] = { "PEAKS_P", "PEAKS_Y", "SERVOS", -#ifdef USE_DUAL_GYRO - "GYRO_2", -#endif NULL }; #endif diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 804a3c95680..a683a7c0fba 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -335,7 +335,7 @@ groups: 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." + description: "On a board with two IMUs, also sample the one `gyro_to_use` did not select, and log it to Blackbox as `gyroRaw2`. Instrumentation only: it never reaches attitude estimation or the PID loops. While this is off the second IMU is not initialised at all, so nothing else about the board changes. While it is on it costs one extra SPI transaction per gyro cycle." default_value: OFF condition: USE_DUAL_GYRO field: gyro_secondary_enabled diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 568465a8f72..98ad8fdd721 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -80,8 +80,7 @@ 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. */ +// Highest IMU tag any in-tree target uses: most are 0 and 1, AETH743Basic is 0 and 2 #define MAX_GYRO_SENSOR_TAG 2 #else #define MAX_GYRO_COUNT 1 @@ -97,8 +96,8 @@ STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT]; STATIC_FASTRAM filterApplyFnPtr gyroLpf2ApplyFn; STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT]; -// Cached calibration status to eliminate function call in hot path -STATIC_FASTRAM bool gyroCalibrationComplete; +// Cached calibration status to eliminate function call in hot path, one entry per sensor +STATIC_FASTRAM bool gyroCalibrationComplete[MAX_GYRO_COUNT]; STATIC_FASTRAM filterApplyFnPtr gyroLuluApplyFn; STATIC_FASTRAM filter_t gyroLuluState[XYZ_AXIS_COUNT]; @@ -324,7 +323,9 @@ static void gyroInitFilters(void) bool gyroInit(void) { memset(&gyro, 0, sizeof(gyro)); - gyroCalibrationComplete = false; + for (int i = 0; i < MAX_GYRO_COUNT; i++) { + gyroCalibrationComplete[i] = false; + } // Set inertial sensor tag (for dual-gyro selection) #ifdef USE_DUAL_GYRO @@ -358,21 +359,10 @@ 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. - */ + // Optional secondary IMU, logged as gyroRaw2 and read by nothing else 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. - */ + // Targets disagree on how they tag the second IMU, so probe rather than assume tag 1 for (uint8_t tag = 0; tag <= MAX_GYRO_SENSOR_TAG; tag++) { if (tag == gyroConfig()->gyro_to_use) { continue; @@ -415,32 +405,16 @@ void gyroStartCalibration(void) } #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. - */ + // The secondary measures its own zero and ignores init_gyro_cal: gyro_zero_cal is a + // single stored value, and it belongs to the gyro that flies 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. - */ + // The threshold is in raw counts, so convert it into the secondary's own scale 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); + // Asked to succeed like the primary: a window ending on a moving aircraft restarts + gyroCalibrationComplete[1] = false; + zeroCalibrationStartV(&gyroCalibration[1], CALIBRATING_GYRO_TIME_MS, secondaryThreshold, false); } #endif @@ -450,7 +424,7 @@ void gyroStartCalibration(void) } #endif - gyroCalibrationComplete = false; + gyroCalibrationComplete[0] = false; zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, CALIBRATING_GYRO_MORON_THRESHOLD, false); } @@ -469,7 +443,7 @@ bool gyroIsCalibrationComplete(void) return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]); } -STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration, bool persist) +STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration, uint8_t index) { fpVector3_t v; @@ -488,17 +462,14 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[Z] = v.v[Z]; #ifndef USE_IMU_FAKE // fixes Test Unit compilation error - /* gyro_zero_cal is a single shared value: only the gyro that actually - * flies the aircraft is allowed to write it. */ - if (persist) { + // gyro_zero_cal is a single shared value: only the gyro that flies may write it + if (index == 0) { setGyroCalibration(dev->gyroZero); } -#else - UNUSED(persist); #endif // Cache completion status to avoid function call in hot path - gyroCalibrationComplete = true; + gyroCalibrationComplete[index] = true; LOG_DEBUG(GYRO, "Gyro calibration complete (%d, %d, %d)", (int16_t) dev->gyroZero[X], (int16_t) dev->gyroZero[Y], (int16_t) dev->gyroZero[Z]); schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics @@ -519,17 +490,16 @@ void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate) } } -static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroCalibrationVector_t * gyroCal, float * gyroADCf, bool isPrimary) +static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroCalibrationVector_t * gyroCal, float * gyroADCf, uint8_t index) { - // range: +/- 8192; +/- 2000 deg/sec if (gyroDev->readFn(gyroDev)) { #ifndef USE_IMU_FAKE // fixes Test Unit compilation error - if (isPrimary && !gyroConfig()->init_gyro_cal_enabled) { + if (index == 0 && !gyroConfig()->init_gyro_cal_enabled) { // marks that the gyro calibration has ended gyroCal->params.state = ZERO_CALIBRATION_DONE; - gyroCalibrationComplete = true; + gyroCalibrationComplete[0] = true; // pass the calibration values gyroDev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; gyroDev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y]; @@ -538,7 +508,7 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC #endif // Use cached status to avoid function call in hot path - if (gyroCalibrationComplete) { + if (gyroCalibrationComplete[index]) { float gyroADCtmp[XYZ_AXIS_COUNT]; //Apply zero calibration with CMSIS DSP @@ -553,7 +523,7 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC return true; } else { - performGyroCalibration(gyroDev, gyroCal, isPrimary); + performGyroCalibration(gyroDev, gyroCal, index); // Reset gyro values to zero to prevent other code from using uncalibrated data gyroADCf[X] = 0.0f; @@ -659,12 +629,9 @@ void FAST_CODE NOINLINE gyroUpdate(void) } #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. - */ + // Read the secondary first: the primary's path returns early on a failed read if (gyro.secondaryInitialized) { - if (!gyroUpdateAndCalibrate(&gyroDev[1], &gyroCalibration[1], gyro.gyroRaw2, false)) { + if (!gyroUpdateAndCalibrate(&gyroDev[1], &gyroCalibration[1], gyro.gyroRaw2, 1)) { gyro.gyroRaw2[X] = 0.0f; gyro.gyroRaw2[Y] = 0.0f; gyro.gyroRaw2[Z] = 0.0f; @@ -672,7 +639,7 @@ void FAST_CODE NOINLINE gyroUpdate(void) } #endif - if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf, true)) { + if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf, 0)) { return; } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index cafc91b56db..98578589587 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -66,8 +66,7 @@ typedef struct gyro_s { 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. */ + // Secondary IMU, for logging only: never feeds attitude estimation or the PID loops float gyroRaw2[XYZ_AXIS_COUNT]; bool secondaryInitialized; #endif @@ -114,21 +113,9 @@ 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. */ + // Appended at the end on purpose: pgLoad() copies MIN(stored, current) bytes over the + // defaults, so appending leaves existing settings at their offset and needs no version + // bump. A field added here must default to zero: it can land in the old struct's padding bool gyro_secondary_enabled; #endif } gyroConfig_t;