diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 77aa3f7836..3fbcc19633 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -57,6 +57,7 @@ typedef enum { DEBUG_SBUS, DEBUG_FPORT, DEBUG_ALWAYS, + DEBUG_STAGE2, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6af7d2ce53..20499fada2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -69,7 +69,7 @@ tables: - name: i2c_speed values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"] - name: debug_modes - values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS"] + values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 162bb355b2..c7d0b6843f 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -441,10 +441,18 @@ void gyroUpdate(timeDelta_t gyroUpdateDeltaUs) DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); + if (axis < 2) { + DEBUG_SET(DEBUG_STAGE2, axis, lrintf(gyroADCf)); + } + #ifdef USE_GYRO_BIQUAD_RC_FIR2 gyroADCf = gyroFilterStage2ApplyFn(stage2Filter[axis], gyroADCf); #endif + if (axis < 2) { + DEBUG_SET(DEBUG_STAGE2, axis + 2, lrintf(gyroADCf)); + } + gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf); DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));