Add more filter freedom

src/main/common/filter.c

@@ -60,7 +60,7 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr
 {
     biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
 }
-void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType)
+void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
 {
     // setup variables
     const float sampleRate = 1 / ((float)refreshRate * 0.000001f);

src/main/common/filter.h

@@ -30,12 +30,17 @@ typedef struct biquadFilter_s {
 } biquadFilter_t;
 
 typedef enum {
-    FILTER_LPF,
+    FILTER_PT1 = 0,
-    FILTER_NOTCH
+    FILTER_BIQUAD,
 } filterType_e;
 
+typedef enum {
+    FILTER_LPF,
+    FILTER_NOTCH
+} biquadFilterType_e;
+
 void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
-void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType);
+void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 float biquadFilterApply(biquadFilter_t *filter, float input);
 
 void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);

src/main/config/config.c

@@ -20,6 +20,7 @@
 #include <string.h>
 
 #include "platform.h"
+#include "debug.h"
 
 #include "build_config.h"
 
@@ -171,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 141;
+static const uint8_t EEPROM_CONF_VERSION = 142;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -233,7 +234,10 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->yaw_lpf_hz = 80;
     pidProfile->rollPitchItermIgnoreRate = 200;
     pidProfile->yawItermIgnoreRate = 50;
+    pidProfile->dterm_filter_type = FILTER_PT1;
     pidProfile->dterm_lpf_hz = 100;    // filtering ON by default
+    pidProfile->dterm_notch_hz = 0;
+    pidProfile->dterm_notch_q = 5;
     pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
     pidProfile->vbatPidCompensation = 0;
     pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF;
@@ -475,7 +479,7 @@ static void resetConf(void)
     masterConfig.gyro_sync_denom = 4;
     masterConfig.pid_process_denom = 2;
 #endif
-    masterConfig.gyro_soft_type = GYRO_FILTER_PT1;
+    masterConfig.gyro_soft_type = FILTER_PT1;
     masterConfig.gyro_soft_lpf_hz = 80;
     masterConfig.gyro_soft_notch_hz = 0;
     masterConfig.gyro_soft_notch_q = 5;

src/main/flight/pid.c

@@ -66,7 +66,6 @@ uint8_t PIDweight[3];
 static int32_t errorGyroI[3];
 static float errorGyroIf[3];
 
-
 static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
         const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
 #ifdef SKIP_PID_FLOAT
@@ -107,6 +106,23 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
 static pt1Filter_t deltaFilter[3];
 static pt1Filter_t yawFilter;
+static biquadFilter_t dtermFilterLpf[3];
+static biquadFilter_t dtermFilterNotch[3];
+static bool dtermNotchInitialised, dtermBiquadLpfInitialised;
+
+void initFilters(const pidProfile_t *pidProfile) {
+    int axis;
+
+    if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) {
+        for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, ((float) pidProfile->dterm_notch_q) / 10, FILTER_NOTCH);
+    }
+
+    if (pidProfile->dterm_filter_type == FILTER_BIQUAD) {
+        if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) {
+            for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, gyro.targetLooptime);
+        }
+    }
+}
 
 #ifndef SKIP_PID_FLOAT
 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab)
@@ -126,6 +142,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
 
     float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
 
+    initFilters(pidProfile);
+
     if (FLIGHT_MODE(HORIZON_MODE)) {
         // Figure out the raw stick positions
         const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
@@ -265,7 +283,15 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
             if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd * delta * dynReduction;
 
             // Filter delta
-            if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
+            if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta);
+
+            if (pidProfile->dterm_lpf_hz) {
+                if (dtermBiquadLpfInitialised) {
+                    delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
+                } else {
+                    delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
+                }
+            }
 
             DTerm = constrainf(Kd * delta * dynReduction, -300.0f, 300.0f);
 
@@ -317,6 +343,8 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
 
     int8_t horizonLevelStrength = 100;
 
+    initFilters(pidProfile);
+
     if (FLIGHT_MODE(HORIZON_MODE)) {
         // Figure out the raw stick positions
         const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
@@ -417,7 +445,15 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
             if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
 
             // Filter delta
-            if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT());
+            if (pidProfile->dterm_lpf_hz) {
+                float deltaf = delta;  // single conversion
+                if (dtermBiquadLpfInitialised) {
+                    delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
+                } else {
+                    delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
+                }
+                delta = lrintf(deltaf);
+            }
 
             DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
 

src/main/flight/pid.h

@@ -80,8 +80,11 @@ typedef struct pidProfile_s {
     uint8_t I8[PID_ITEM_COUNT];
     uint8_t D8[PID_ITEM_COUNT];
 
+    uint8_t dterm_filter_type;              // Filter selection for dterm
     uint16_t dterm_lpf_hz;                  // Delta Filter in hz
     uint16_t yaw_lpf_hz;                    // Additional yaw filter when yaw axis too noisy
+    uint16_t dterm_notch_hz;                // Biquad dterm notch hz
+    uint8_t dterm_notch_q;                  // Biquad dterm notch quality
     uint8_t deltaMethod;                    // Alternative delta Calculation
     uint16_t rollPitchItermIgnoreRate;      // Experimental threshold for resetting iterm for pitch and roll on certain rates
     uint16_t yawItermIgnoreRate;            // Experimental threshold for resetting iterm for yaw on certain rates

src/main/io/serial_cli.c

@@ -497,7 +497,7 @@ static const char * const lookupTableRcSmoothing[] = {
     "OFF", "DEFAULT", "AUTO", "MANUAL"
 };
 
-static const char * const lookupTableGyroSoftLowpassType[] = {
+static const char * const lookupTableLowpassType[] = {
     "NORMAL", "HIGH"
 };
 
@@ -538,7 +538,7 @@ typedef enum {
     TABLE_MOTOR_PWM_PROTOCOL,
     TABLE_DELTA_METHOD,
     TABLE_RC_SMOOTHING,
-    TABLE_GYRO_LOWPASS_TYPE,
+    TABLE_LOWPASS_TYPE,
 #ifdef OSD
     TABLE_OSD,
 #endif
@@ -576,7 +576,7 @@ static const lookupTableEntry_t lookupTables[] = {
     { lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) },
     { lookupTableDeltaMethod, sizeof(lookupTableDeltaMethod) / sizeof(char *) },
     { lookupTableRcSmoothing, sizeof(lookupTableRcSmoothing) / sizeof(char *) },
-    { lookupTableGyroSoftLowpassType, sizeof(lookupTableGyroSoftLowpassType) / sizeof(char *) },
+    { lookupTableLowpassType, sizeof(lookupTableLowpassType) / sizeof(char *) },
 #ifdef OSD
     { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) },
 #endif
@@ -748,7 +748,7 @@ const clivalue_t valueTable[] = {
     { "pid_delta_method",           VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
     { "gyro_lpf",                   VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
     { "gyro_sync_denom",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyro_sync_denom, .config.minmax = { 1,  8 } },
-    { "gyro_lowpass_type",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.gyro_soft_type, .config.lookup = { TABLE_GYRO_LOWPASS_TYPE } },
+    { "gyro_lowpass_type",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
     { "gyro_lowpass",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0,  255 } },
     { "gyro_notch_hz",              VAR_UINT16 | MASTER_VALUE,  &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0,  500 } },
     { "gyro_notch_q",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyro_soft_notch_q, .config.minmax = { 1,  100 } },
@@ -821,7 +821,10 @@ const clivalue_t valueTable[] = {
     { "mag_hardware",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
     { "mag_declination",            VAR_INT16  | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000,  18000 } },
 #endif
+    { "dterm_filter_type",          VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
     { "dterm_lowpass",              VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
+    { "dterm_notch_hz",             VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0,  500 } },
+    { "dterm_notch_q",              VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_q, .config.minmax = { 1,  100 } },
     { "vbat_pid_compensation",      VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } },
     { "zero_throttle_stabilisation",VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } },
     { "pid_tolerance_band",         VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } },

src/main/sensors/gyro.c

@@ -69,7 +69,7 @@ void gyroInit(void)
     if (gyroSoftLpfHz && gyro.targetLooptime) {  // Initialisation needs to happen once samplingrate is known
         for (int axis = 0; axis < 3; axis++) {
             biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH);
-            if (gyroSoftLpfType == GYRO_FILTER_BIQUAD)
+            if (gyroSoftLpfType == FILTER_BIQUAD)
                 biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
             else
                 gyroDt = gyro.targetLooptime / 1000.0f;
@@ -182,7 +182,7 @@ void gyroUpdate(void)
                 debug[axis*2 + 1] = lrintf(sample);
             }
 
-            if (gyroSoftLpfType == GYRO_FILTER_BIQUAD) {
+            if (gyroSoftLpfType == FILTER_BIQUAD) {
                 gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);
             } else {
                 gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], sample, gyroSoftLpfHz, gyroDt);

src/main/sensors/gyro.h

@@ -31,11 +31,6 @@ typedef enum {
     GYRO_MAX = GYRO_FAKE
 } gyroSensor_e;
 
-typedef enum {
-    GYRO_FILTER_PT1 = 0,
-    GYRO_FILTER_BIQUAD,
-} gyroFilter_e;
-
 extern gyro_t gyro;
 
 extern int32_t gyroADC[XYZ_AXIS_COUNT];