diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 30f9fdc665..6e76818fee 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -956,6 +956,9 @@ groups: - name: dterm_lpf_hz min: 0 max: 200 + - name: use_dterm_fir_filter + field: use_dterm_fir_filter + type: bool - name: yaw_lpf_hz min: 0 max: 200 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5181f32291..774e230955 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -183,6 +183,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .dterm_lpf_hz = 40, .yaw_lpf_hz = 30, .dterm_setpoint_weight = 1.0f, + .use_dterm_fir_filter = 1, .itermWindupPointPercent = 50, // Percent @@ -211,8 +212,10 @@ void pidInit(void) // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ // h[0] = 5/8, h[-1] = 1/4, h[-2] = -1, h[-3] = -1/4, h[-4] = 3/8 static const float dtermCoeffs[PID_GYRO_RATE_BUF_LENGTH] = {5.0f/8, 2.0f/8, -8.0f/8, -2.0f/8, 3.0f/8}; - for (int axis = 0; axis < 3; ++ axis) { - firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs); + if (pidProfile()->use_dterm_fir_filter) { + for (int axis = 0; axis < 3; ++ axis) { + firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs); + } } pidResetTPAFilter(); @@ -566,9 +569,19 @@ static void pidApplyMulticopterRateController(pidState_t *pidState, flight_dynam deltaFiltered = biquadFilterApply(&pidState->deltaLpfState, deltaFiltered); } + /* + * Apply "Classical" INAV noise robust differentiator if configured to do so + * This filter is not configurable in any way, it only can be enabled or disabled + */ + if (pidProfile()->use_dterm_fir_filter) { + firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered); + newDTerm = firFilterApply(&pidState->gyroRateFilter); + } else { + newDTerm = deltaFiltered; + } + // Calculate derivative - firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered); - newDTerm = firFilterApply(&pidState->gyroRateFilter) * (pidState->kD / dT); + newDTerm = newDTerm * (pidState->kD / dT); // Additionally constrain D newDTerm = constrainf(newDTerm, -300.0f, 300.0f); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index cf5c05c283..2d491759b0 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -83,6 +83,7 @@ typedef struct pidProfile_s { uint16_t dterm_soft_notch_hz; // Dterm Notch frequency uint16_t dterm_soft_notch_cutoff; // Dterm Notch Cutoff frequency uint8_t dterm_lpf_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5 + uint8_t use_dterm_fir_filter; // Use classical INAV FIR differentiator. Very noise robust, can be quite slowish uint8_t yaw_pterm_lpf_hz; // Used for filering Pterm noise on noisy frames uint8_t yaw_lpf_hz;