Cleanup debugs and USE_ defines that are always on

src/main/build/debug.h

@@ -51,29 +51,19 @@ extern timeUs_t sectionTimes[2][4];
 typedef enum {
     DEBUG_NONE,
     DEBUG_GYRO,
-    DEBUG_NOTCH,
-    DEBUG_NAV_LANDING_DETECTOR,
-    DEBUG_FW_CLIMB_RATE_TO_ALTITUDE,
     DEBUG_AGL,
     DEBUG_FLOW_RAW,
     DEBUG_FLOW,
     DEBUG_SBUS,
     DEBUG_FPORT,
     DEBUG_ALWAYS,
-    DEBUG_STAGE2,
     DEBUG_SAG_COMP_VOLTAGE,
     DEBUG_VIBE,
     DEBUG_CRUISE,
     DEBUG_REM_FLIGHT_TIME,
     DEBUG_SMARTAUDIO,
     DEBUG_ACC,
-    DEBUG_GENERIC,
     DEBUG_ITERM_RELAX,
-    DEBUG_D_BOOST,
-    DEBUG_ANTIGRAVITY,
-    DEBUG_FFT,
-    DEBUG_FFT_TIME,
-    DEBUG_FFT_FREQ,
     DEBUG_ERPM,
     DEBUG_COUNT
 } debugType_e;

src/main/fc/config.c

@@ -167,27 +167,18 @@ uint32_t getLooptime(void) {
 
 void validateAndFixConfig(void)
 {
-#ifdef USE_GYRO_NOTCH_1
     if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
         gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
     }
-#endif
-#ifdef USE_GYRO_NOTCH_2
     if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
         gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
     }
-#endif
-#ifdef USE_DTERM_NOTCH
     if (pidProfile()->dterm_soft_notch_cutoff >= pidProfile()->dterm_soft_notch_hz) {
         pidProfileMutable()->dterm_soft_notch_hz = 0;
     }
-#endif
-
-#ifdef USE_ACC_NOTCH
     if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
         accelerometerConfigMutable()->acc_notch_hz = 0;
     }
-#endif
 
     // Disable unused features
     featureClear(FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10);

src/main/fc/fc_msp.c

@@ -1101,43 +1101,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz);
         sbufWriteU16(dst, pidProfile()->dterm_lpf_hz);
         sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);
-#ifdef USE_GYRO_NOTCH_1
         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1); //masterConfig.gyro_soft_notch_hz_1
         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1); //BF: masterConfig.gyro_soft_notch_cutoff_1
-#else
-        sbufWriteU16(dst, 0); //masterConfig.gyro_soft_notch_hz_1
-        sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_1
-#endif
 
-#ifdef USE_DTERM_NOTCH
         sbufWriteU16(dst, pidProfile()->dterm_soft_notch_hz); //BF: pidProfile()->dterm_notch_hz
         sbufWriteU16(dst, pidProfile()->dterm_soft_notch_cutoff); //pidProfile()->dterm_notch_cutoff
-#else
-        sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz
-        sbufWriteU16(dst, 1); //pidProfile()->dterm_notch_cutoff
-#endif
 
-#ifdef USE_GYRO_NOTCH_2
         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2); //BF: masterConfig.gyro_soft_notch_hz_2
         sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2); //BF: masterConfig.gyro_soft_notch_cutoff_2
-#else
-        sbufWriteU16(dst, 0); //BF: masterConfig.gyro_soft_notch_hz_2
-        sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_2
-#endif
 
-#ifdef USE_ACC_NOTCH
         sbufWriteU16(dst, accelerometerConfig()->acc_notch_hz);
         sbufWriteU16(dst, accelerometerConfig()->acc_notch_cutoff);
-#else
-        sbufWriteU16(dst, 0);
-        sbufWriteU16(dst, 1);
-#endif
 
-#ifdef USE_GYRO_BIQUAD_RC_FIR2
         sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz);
-#else
-        sbufWriteU16(dst, 0);
-#endif
         break;
 
     case MSP_PID_ADVANCED:
@@ -2017,43 +1993,38 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
             pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 255);
             pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
-#ifdef USE_GYRO_NOTCH_1
             if (dataSize >= 9) {
                 gyroConfigMutable()->gyro_soft_notch_hz_1 = constrain(sbufReadU16(src), 0, 500);
                 gyroConfigMutable()->gyro_soft_notch_cutoff_1 = constrain(sbufReadU16(src), 1, 500);
-            } else
+            } else {
                 return MSP_RESULT_ERROR;
-#endif
+            }
-#ifdef USE_DTERM_NOTCH
             if (dataSize >= 13) {
                 pidProfileMutable()->dterm_soft_notch_hz = constrain(sbufReadU16(src), 0, 500);
                 pidProfileMutable()->dterm_soft_notch_cutoff = constrain(sbufReadU16(src), 1, 500);
                 pidInitFilters();
-            } else
+            } else {
                 return MSP_RESULT_ERROR;
-#endif
+            }
-#ifdef USE_GYRO_NOTCH_2
             if (dataSize >= 17) {
                 gyroConfigMutable()->gyro_soft_notch_hz_2 = constrain(sbufReadU16(src), 0, 500);
                 gyroConfigMutable()->gyro_soft_notch_cutoff_2 = constrain(sbufReadU16(src), 1, 500);
-            } else
+            } else {
                 return MSP_RESULT_ERROR;
-#endif
+            }
 
-#ifdef USE_ACC_NOTCH
             if (dataSize >= 21) {
                 accelerometerConfigMutable()->acc_notch_hz = constrain(sbufReadU16(src), 0, 255);
                 accelerometerConfigMutable()->acc_notch_cutoff = constrain(sbufReadU16(src), 1, 255);
-            } else
+            } else {
                 return MSP_RESULT_ERROR;
-#endif
+            }
 
-#ifdef USE_GYRO_BIQUAD_RC_FIR2
             if (dataSize >= 22) {
                 gyroConfigMutable()->gyro_stage2_lowpass_hz = constrain(sbufReadU16(src), 0, 500);
-            } else
+            } else {
                 return MSP_RESULT_ERROR;
-#endif
+            }
         } else
             return MSP_RESULT_ERROR;
         break;

src/main/fc/settings.yaml

@@ -78,10 +78,10 @@ tables:
   - name: i2c_speed
     values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
   - name: debug_modes
-    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW",
+    values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
-      "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE",
+      "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
-      "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX", 
+      "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX", 
-      "D_BOOST", "ANTIGRAVITY", "FFT", "FFT_TIME", "FFT_FREQ", "ERPM"]
+      "ERPM"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -164,25 +164,20 @@ groups:
         max: 128
       - name: gyro_notch1_hz
         field: gyro_soft_notch_hz_1
-        condition: USE_GYRO_NOTCH_1
         max: 500
       - name: gyro_notch1_cutoff
         field: gyro_soft_notch_cutoff_1
-        condition: USE_GYRO_NOTCH_1
         min: 1
         max: 500
       - name: gyro_notch2_hz
         field: gyro_soft_notch_hz_2
-        condition: USE_GYRO_NOTCH_2
         max: 500
       - name: gyro_notch2_cutoff
         field: gyro_soft_notch_cutoff_2
-        condition: USE_GYRO_NOTCH_2
         min: 1
         max: 500
       - name: gyro_stage2_lowpass_hz
         field: gyro_stage2_lowpass_hz
-        condition: USE_GYRO_BIQUAD_RC_FIR2
         min: 0
         max: 500
       - name: dyn_notch_width_percent
@@ -236,11 +231,9 @@ groups:
     headers: ["sensors/acceleration.h"]
     members:
       - name: acc_notch_hz
-        condition: USE_ACC_NOTCH
         min: 0
         max: 255
       - name: acc_notch_cutoff
-        condition: USE_ACC_NOTCH
         min: 1
         max: 255
       - name: align_acc
@@ -1076,12 +1069,10 @@ groups:
         max: 1
       - name: dterm_notch_hz
         field: dterm_soft_notch_hz
-        condition: USE_DTERM_NOTCH
         min: 0
         max: 500
       - name: dterm_notch_cutoff
         field: dterm_soft_notch_cutoff
-        condition: USE_DTERM_NOTCH
         min: 1
         max: 500
       - name: pidsum_limit

src/main/flight/gyroanalyse.c

@@ -157,9 +157,6 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn,
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
             state->downsampledGyroData[axis][state->circularBufferIdx] = sample;
-            if (axis == 0) {
-                DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
-            }
 
             state->oversampledGyroAccumulator[axis] = 0;
         }
@@ -201,12 +198,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
 
     arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint);
 
-    uint32_t startTime = 0;
-    if (debugMode == (DEBUG_FFT_TIME)) {
-        startTime = micros();
-    }
-
-    DEBUG_SET(DEBUG_FFT_TIME, 0, state->updateStep);
     switch (state->updateStep) {
         case STEP_ARM_CFFT_F32:
         {
@@ -224,14 +215,12 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
                 arm_radix8_butterfly_f32(state->fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1);
                 break;
             }
-            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
             break;
         }
         case STEP_BITREVERSAL:
         {
             // 6us
             arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable);
-            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
             state->updateStep++;
             FALLTHROUGH;
         }
@@ -240,14 +229,12 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
             // 14us
             // this does not work in place => fftData AND rfftData needed
             stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData);
-            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
             break;
         }
         case STEP_ARM_CMPLX_MAG_F32:
         {
             // 8us
             arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT);
-            DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
             state->updateStep++;
             FALLTHROUGH;
         }
@@ -312,15 +299,7 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
 
             dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
 
-            if (state->updateAxis == 0) {
-                DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
-                DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
-            }
-            if (state->updateAxis == 1) {
-                DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
-            }
             // Debug FFT_Freq carries raw gyro, gyro after first filter set, FFT centre for roll and for pitch
-            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
             break;
         }
         case STEP_UPDATE_FILTERS:
@@ -329,8 +308,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
             // calculate cutoffFreq and notch Q, update notch filter  =1.8+((A2-150)*0.004)
             if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
                 
-                DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis + 5, state->centerFreq[state->updateAxis]);
-
                 if (dualNotch) {
                     biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
                     biquadFilterUpdate(&notchFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
@@ -338,7 +315,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
                     biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], getLooptime(), dynNotchQ, FILTER_NOTCH);
                 }
             }
-            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 
             state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
             state->updateStep++;
@@ -354,8 +330,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
             if (state->circularBufferIdx > 0) {
                 arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx);
             }
-
-            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
         }
     }
 

src/main/flight/pid.c

@@ -81,11 +81,8 @@ typedef struct {
     rateLimitFilter_t axisAccelFilter;
     pt1Filter_t ptermLpfState;
     biquadFilter_t deltaLpfState;
-
     // Dterm notch filtering
-#ifdef USE_DTERM_NOTCH
     biquadFilter_t deltaNotchFilter;
-#endif
     float stickPosition;
 
 #ifdef USE_D_BOOST
@@ -96,10 +93,7 @@ typedef struct {
 #endif
 } pidState_t;
 
-#ifdef USE_DTERM_NOTCH
 STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn;
-#endif
-
 STATIC_FASTRAM bool pidFiltersConfigured = false;
 FASTRAM float headingHoldCosZLimit;
 FASTRAM int16_t headingHoldTarget;
@@ -310,7 +304,6 @@ bool pidInitFilters(void)
         firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs);
     }
 
-#ifdef USE_DTERM_NOTCH
     notchFilterApplyFn = nullFilterApply;
     if (pidProfile()->dterm_soft_notch_hz != 0) {
         notchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
@@ -318,7 +311,6 @@ bool pidInitFilters(void)
             biquadFilterInitNotch(&pidState[axis].deltaNotchFilter, refreshRate, pidProfile()->dterm_soft_notch_hz, pidProfile()->dterm_soft_notch_cutoff);
         }
     }
-#endif
 
     // Init other filters
     for (int axis = 0; axis < 3; ++ axis) {
@@ -682,14 +674,6 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t
         dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
         dBoost = constrainf(dBoost, 1.0f, dBoostFactor);
 
-        if (axis == FD_ROLL) {
-            DEBUG_SET(DEBUG_D_BOOST, 0, dBoostGyroAcceleration);
-            DEBUG_SET(DEBUG_D_BOOST, 1, dBoostRateAcceleration);
-            DEBUG_SET(DEBUG_D_BOOST, 2, dBoost * 100);
-        } else if (axis == FD_PITCH) {
-            DEBUG_SET(DEBUG_D_BOOST, 3, dBoost * 100);
-        }
-
         pidState->previousRateTarget = pidState->rateTarget;
         pidState->previousRateGyro = pidState->gyroRate;
     } 
@@ -719,10 +703,8 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
         // Calculate delta for Dterm calculation. Apply filters before derivative to minimize effects of dterm kick
         float deltaFiltered = pidProfile()->dterm_setpoint_weight * pidState->rateTarget - pidState->gyroRate;
 
-#ifdef USE_DTERM_NOTCH
         // Apply D-term notch
         deltaFiltered = notchFilterApplyFn(&pidState->deltaNotchFilter, deltaFiltered);
-#endif
 
         // Apply additional lowpass
         if (pidProfile()->dterm_lpf_hz) {
@@ -752,8 +734,6 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
 
 #ifdef USE_ANTIGRAVITY
     const float iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);    
-    DEBUG_SET(DEBUG_ANTIGRAVITY, 0, iTermAntigravityGain * 100);
-    DEBUG_SET(DEBUG_ANTIGRAVITY, 1, antigravityThrottleHpf);
     itermErrorRate *= iTermAntigravityGain;
 #endif
 

src/main/flight/pid.h

@@ -162,11 +162,7 @@ extern int16_t axisPID[];
 extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[];
 
 void pidInit(void);
-
-#ifdef USE_DTERM_NOTCH
 bool pidInitFilters(void);
-#endif
-
 void pidResetErrorAccumulators(void);
 void pidResetTPAFilter(void);
 

src/main/navigation/navigation.c

@@ -2391,9 +2391,6 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
             // Fixed wing climb rate controller is open-loop. We simply move the known altitude target
             float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
 
-            DEBUG_SET(DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, 0, desiredClimbRate);
-            DEBUG_SET(DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, 1, timeDelta * 1000);
-
             if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
                 posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
                 posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500);    // FIXME: calculate sanity limits in a smarter way

src/main/navigation/navigation_multicopter.c

@@ -660,10 +660,6 @@ bool isMulticopterLandingDetected(void)
 
     bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement;
 
-    DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 0, isAtMinimalThrust * 100 + !verticalMovement * 10 + !horizontalMovement * 1);
-    DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 1, (landingThrSamples == 0) ? 0 : (rcCommandAdjustedThrottle - (landingThrSum / landingThrSamples)));
-    DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 2, (currentTimeUs - landingTimer) / 1000);
-
     // If we have surface sensor (for example sonar) - use it to detect touchdown
     if ((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z >= 0)) {
         // TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety.

src/main/sensors/acceleration.c

@@ -81,10 +81,8 @@ STATIC_FASTRAM void *accSoftLpfFilter[XYZ_AXIS_COUNT];
 STATIC_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT];
 STATIC_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
 
-#ifdef USE_ACC_NOTCH
 STATIC_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
 STATIC_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
-#endif
 
 PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 3);
 
@@ -578,13 +576,11 @@ void accUpdate(void)
         acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
     }
 
-#ifdef USE_ACC_NOTCH
     if (accelerometerConfig()->acc_notch_hz) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             acc.accADCf[axis] = accNotchFilterApplyFn(accNotchFilter[axis], acc.accADCf[axis]);
         }
     }
-#endif
 
 }
 
@@ -665,7 +661,6 @@ void accInitFilters(void)
         pt1FilterInit(&accVibeFilter[axis], ACC_VIBE_FILT_HZ, accDt);
     }
 
-#ifdef USE_ACC_NOTCH
     STATIC_FASTRAM biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT];
     accNotchFilterApplyFn = nullFilterApply;
 
@@ -676,7 +671,6 @@ void accInitFilters(void)
             biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, accelerometerConfig()->acc_notch_hz, accelerometerConfig()->acc_notch_cutoff);
         }
     }
-#endif
 
 }
 

src/main/sensors/gyro.c

@@ -84,20 +84,13 @@ STATIC_FASTRAM int32_t gyroADC[XYZ_AXIS_COUNT];
 STATIC_FASTRAM filterApplyFnPtr softLpfFilterApplyFn;
 STATIC_FASTRAM void *softLpfFilter[XYZ_AXIS_COUNT];
 
-#ifdef USE_GYRO_NOTCH_1
 STATIC_FASTRAM filterApplyFnPtr notchFilter1ApplyFn;
 STATIC_FASTRAM void *notchFilter1[XYZ_AXIS_COUNT];
-#endif
-#ifdef USE_GYRO_NOTCH_2
 STATIC_FASTRAM filterApplyFnPtr notchFilter2ApplyFn;
 STATIC_FASTRAM void *notchFilter2[XYZ_AXIS_COUNT];
-#endif
 
-#if defined(USE_GYRO_BIQUAD_RC_FIR2)
-// gyro biquad RC FIR2 filter
 STATIC_FASTRAM filterApplyFnPtr gyroFilterStage2ApplyFn;
 STATIC_FASTRAM void *stage2Filter[XYZ_AXIS_COUNT];
-#endif
 
 #ifdef USE_DYNAMIC_FILTERS
 
@@ -337,16 +330,13 @@ void gyroInitFilters(void)
 {
     STATIC_FASTRAM filter_t gyroFilterLPF[XYZ_AXIS_COUNT];
     softLpfFilterApplyFn = nullFilterApply;
-#ifdef USE_GYRO_NOTCH_1
+
     STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
     notchFilter1ApplyFn = nullFilterApply;
-#endif
+    
-#ifdef USE_GYRO_NOTCH_2
     STATIC_FASTRAM biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
     notchFilter2ApplyFn = nullFilterApply;
-#endif
+    
-
-#ifdef USE_GYRO_BIQUAD_RC_FIR2
     STATIC_FASTRAM biquadFilter_t gyroFilterStage2[XYZ_AXIS_COUNT];
     gyroFilterStage2ApplyFn = nullFilterApply;
 
@@ -357,7 +347,6 @@ void gyroInitFilters(void)
             biquadRCFIR2FilterInit(stage2Filter[axis], gyroConfig()->gyro_stage2_lowpass_hz, getLooptime());
         }
     }
-#endif
 
     if (gyroConfig()->gyro_soft_lpf_hz) {
         
@@ -380,7 +369,6 @@ void gyroInitFilters(void)
         }
     }
 
-#ifdef USE_GYRO_NOTCH_1
     if (gyroConfig()->gyro_soft_notch_hz_1) {
         notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
         for (int axis = 0; axis < 3; axis++) {
@@ -388,9 +376,7 @@ void gyroInitFilters(void)
             biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
         }
     }
-#endif
 
-#ifdef USE_GYRO_NOTCH_2
     if (gyroConfig()->gyro_soft_notch_hz_2) {
         notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
         for (int axis = 0; axis < 3; axis++) {
@@ -398,7 +384,6 @@ void gyroInitFilters(void)
             biquadFilterInitNotch(notchFilter2[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
         }
     }
-#endif
 }
 
 void gyroStartCalibration(void)
@@ -479,44 +464,13 @@ void FAST_CODE NOINLINE gyroUpdate()
 
         DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
 
-#ifdef USE_DYNAMIC_FILTERS
-        if (isDynamicFilterActive()) {
-            if (axis == FD_ROLL) {
-                DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
-                DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
-            }
-        }
-#endif
-
-        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));
-
-#ifdef USE_GYRO_NOTCH_1
         gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
-#endif
-#ifdef USE_GYRO_NOTCH_2
         gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
-#endif
 
 #ifdef USE_DYNAMIC_FILTERS
         if (isDynamicFilterActive()) {
-            if (axis == FD_ROLL) {
-                DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
-                DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf));
-            }
             gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
             gyroADCf = notchFilterDynApplyFn((filter_t *)&notchFilterDyn[axis], gyroADCf);
             gyroADCf = notchFilterDynApplyFn2((filter_t *)&notchFilterDyn2[axis], gyroADCf);

src/main/target/common.h

@@ -54,7 +54,6 @@
 #define USE_TELEMETRY_LTM
 #define USE_TELEMETRY_FRSKY
 
-#define USE_GYRO_BIQUAD_RC_FIR2
 #define USE_MR_BRAKING_MODE
 
 #if defined(STM_FAST_TARGET)
@@ -121,10 +120,6 @@
 #define USE_AUTOTUNE_FIXED_WING
 #define USE_LOG
 #define USE_STATS
-#define USE_GYRO_NOTCH_1
-#define USE_GYRO_NOTCH_2
-#define USE_DTERM_NOTCH
-#define USE_ACC_NOTCH
 #define USE_CMS
 #define CMS_MENU_OSD
 #define USE_GPS_PROTO_NMEA