diff --git a/docs/Blackbox.md b/docs/Blackbox.md index d770955497d..5cafdd3d08a 100644 --- a/docs/Blackbox.md +++ b/docs/Blackbox.md @@ -168,6 +168,12 @@ The CLI command `blackbox` allows setting which Blackbox fields are recorded to * `PEAKS_Y` - Yaw axis noise peak * `SERVOS` - Servo outputs (for planes, tris, etc.) +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: * `blackbox` currently enabled Blackbox fields diff --git a/docs/Settings.md b/docs/Settings.md index 91c406e6199..49193d362fd 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2176,6 +2176,16 @@ Software based gyro main lowpass filter. Value is cutoff frequency (Hz) --- +### gyro_secondary_enabled + +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 | +| --- | --- | --- | +| 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 6c32d4e36eb..01a37c06a6c 100644 --- a/src/main/blackbox/blackbox.c +++ b/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; +#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++) { 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/settings.yaml b/src/main/fc/settings.yaml index 0744c98403c..a683a7c0fba 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -334,6 +334,12 @@ groups: min: 0 max: 2 default_value: 0 + - name: gyro_secondary_enabled + 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 + 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 38b56b22ae0..98ad8fdd721 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -78,7 +78,13 @@ FASTRAM gyro_t gyro; // gyro sensor object +#ifdef USE_DUAL_GYRO +#define MAX_GYRO_COUNT 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 +#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]; @@ -90,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]; @@ -111,6 +117,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, @@ -316,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 @@ -349,6 +358,31 @@ bool gyroInit(void) gyroInitFilters(); +#ifdef USE_DUAL_GYRO + // Optional secondary IMU, logged as gyroRaw2 and read by nothing else + gyro.secondaryInitialized = false; + if (gyroConfig()->gyro_secondary_enabled) { + // 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; + } + + 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,13 +404,27 @@ void gyroStartCalibration(void) return; } +#ifdef USE_DUAL_GYRO + // 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 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; + + // 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 + #ifndef USE_IMU_FAKE // fixes Test Unit compilation error if (!gyroConfig()->init_gyro_cal_enabled) { return; } #endif - gyroCalibrationComplete = false; + gyroCalibrationComplete[0] = false; zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, CALIBRATING_GYRO_MORON_THRESHOLD, false); } @@ -395,7 +443,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, uint8_t index) { fpVector3_t v; @@ -414,11 +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 - setGyroCalibration(dev->gyroZero); + // gyro_zero_cal is a single shared value: only the gyro that flies may write it + if (index == 0) { + setGyroCalibration(dev->gyroZero); + } #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 @@ -439,17 +490,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, uint8_t index) { - // 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 (index == 0 && !gyroConfig()->init_gyro_cal_enabled) { // marks that the gyro calibration has ended - gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE; - gyroCalibrationComplete = true; + gyroCal->params.state = ZERO_CALIBRATION_DONE; + gyroCalibrationComplete[0] = true; // pass the calibration values gyroDev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X]; gyroDev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y]; @@ -458,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 @@ -473,7 +523,7 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC return true; } else { - performGyroCalibration(gyroDev, gyroCal); + performGyroCalibration(gyroDev, gyroCal, index); // Reset gyro values to zero to prevent other code from using uncalibrated data gyroADCf[X] = 0.0f; @@ -578,7 +628,18 @@ void FAST_CODE NOINLINE gyroUpdate(void) return; } - if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) { +#ifdef USE_DUAL_GYRO + // 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, 1)) { + gyro.gyroRaw2[X] = 0.0f; + gyro.gyroRaw2[Y] = 0.0f; + gyro.gyroRaw2[Z] = 0.0f; + } + } +#endif + + 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 e8167d7d7d4..98578589587 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -65,6 +65,11 @@ typedef struct gyro_s { uint32_t targetLooptime; float gyroADCf[XYZ_AXIS_COUNT]; float gyroRaw[XYZ_AXIS_COUNT]; +#ifdef USE_DUAL_GYRO + // Secondary IMU, for logging 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 +112,12 @@ typedef struct gyroConfig_s { uint8_t gyroLuluSampleCount; bool gyroLuluEnabled; +#ifdef USE_DUAL_GYRO + // 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; PG_DECLARE(gyroConfig_t, gyroConfig);