Merge pull request #684 from rav-rav/notch

Smoother filtering (additional notch filter)

src/main/blackbox/blackbox.c

@@ -1219,6 +1219,8 @@ static bool blackboxWriteSysinfo()
         BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d",                     masterConfig.rcControlsConfig.yaw_deadband);
         BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d",                         masterConfig.gyro_lpf);
         BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d",            (int)(masterConfig.gyro_soft_lpf_hz * 100.0f));
+        BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d",              (int)(masterConfig.gyro_soft_notch_hz * 100.0f));
+        BLACKBOX_PRINT_HEADER_LINE("gyro_notch_q:%d",               (int)(masterConfig.gyro_soft_notch_q * 10.0f));
         BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d",                 (int)(masterConfig.acc_lpf_hz * 100.0f));
         BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d",                     masterConfig.acc_hardware);
         BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d",                    masterConfig.baro_hardware);

src/main/common/filter.c

@@ -56,24 +56,39 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
 }
 
 /* sets up a biquad Filter */
-void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate)
+void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
+{
+    biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
+}
+void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType)
 {
-    const float sampleRate = 1 / ((float)refreshRate * 0.000001f);
-
     // setup variables
-    const float omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
+    const float sampleRate = 1 / ((float)refreshRate * 0.000001f);
+    const float omega = 2 * M_PI_FLOAT * filterFreq / sampleRate;
     const float sn = sinf(omega);
     const float cs = cosf(omega);
-    //this is wrong, should be hyperbolic sine
-    //alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
     const float alpha = sn / (2 * BIQUAD_Q);
 
-    const float b0 = (1 - cs) / 2;
+    float b0, b1, b2, a0, a1, a2;
-    const float b1 = 1 - cs;
+
-    const float b2 = (1 - cs) / 2;
+    switch (filterType) {
-    const float a0 = 1 + alpha;
+        case FILTER_LPF:
-    const float a1 = -2 * cs;
+            b0 = (1 - cs) / 2;
-    const float a2 = 1 - alpha;
+            b1 = 1 - cs;
+            b2 = (1 - cs) / 2;
+            a0 = 1 + alpha;
+            a1 = -2 * cs;
+            a2 = 1 - alpha;
+            break;
+        case FILTER_NOTCH:
+            b0 =  1;
+            b1 = -2 * cs;
+            b2 =  1;
+            a0 =  1 + alpha;
+            a1 = -2 * cs;
+            a2 =  1 - alpha;
+            break;
+    }
 
     // precompute the coefficients
     filter->b0 = b0 / a0;

src/main/common/filter.h

@@ -29,8 +29,13 @@ typedef struct biquadFilter_s {
     float d1, d2;
 } biquadFilter_t;
 
+typedef enum {
+    FILTER_LPF,
+    FILTER_NOTCH
+} filterType_e;
 
-void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate);
+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);
 float biquadFilterApply(biquadFilter_t *filter, float input);
 
 void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);

src/main/config/config.c

@@ -456,6 +456,8 @@ static void resetConf(void)
     masterConfig.gyro_sync_denom = 4;
 #endif
     masterConfig.gyro_soft_lpf_hz = 100;
+    masterConfig.gyro_soft_notch_hz = 0;
+    masterConfig.gyro_soft_notch_q = 5;
 
     masterConfig.pid_process_denom = 2;
 
@@ -717,7 +719,7 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-    gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
+    gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_q);
 
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);

src/main/config/config_master.h

@@ -56,9 +56,11 @@ typedef struct master_t {
     int8_t yaw_control_direction;           // change control direction of yaw (inverted, normal)
     uint8_t acc_hardware;                   // Which acc hardware to use on boards with more than one device
     uint8_t acc_for_fast_looptime;          // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
-    uint16_t gyro_lpf;                       // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
+    uint16_t gyro_lpf;                      // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
     uint8_t gyro_sync_denom;                // Gyro sample divider
-    uint8_t gyro_soft_lpf_hz;               // Biqyad gyro lpf hz
+    uint8_t gyro_soft_lpf_hz;               // Biquad gyro lpf hz
+    uint16_t gyro_soft_notch_hz;            // Biquad gyro notch hz
+    uint8_t gyro_soft_notch_q;              // Biquad gyro notch quality
     uint16_t dcm_kp;                        // DCM filter proportional gain ( x 10000)
     uint16_t dcm_ki;                        // DCM filter integral gain ( x 10000)
 

src/main/debug.h

@@ -50,5 +50,6 @@ typedef enum {
     DEBUG_MIXER,
     DEBUG_AIRMODE,
     DEBUG_PIDLOOP,
+    DEBUG_NOTCH,
     DEBUG_COUNT
 } debugType_e;

src/main/flight/mixer.c

@@ -956,7 +956,7 @@ void filterServos(void)
     if (mixerConfig->servo_lowpass_enable) {
         for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
             if (!servoFilterIsSet) {
-                biquadFilterInit(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime);
+                biquadFilterInitLPF(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime);
                 servoFilterIsSet = true;
             }
 

src/main/io/serial_cli.c

@@ -453,6 +453,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
     "MIXER",
     "AIRMODE",
     "PIDLOOP",
+    "NOTCH",
 };
 #ifdef OSD
 static const char * const lookupTableOsdType[] = {
@@ -698,6 +699,8 @@ const clivalue_t valueTable[] = {
     { "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",               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 } },
     { "moron_threshold",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0,  128 } },
     { "imu_dcm_kp",                 VAR_UINT16 | MASTER_VALUE,  &masterConfig.dcm_kp, .config.minmax = { 0,  50000 } },
     { "imu_dcm_ki",                 VAR_UINT16 | MASTER_VALUE,  &masterConfig.dcm_ki, .config.minmax = { 0,  50000 } },

src/main/sensors/acceleration.c

@@ -190,7 +190,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
         if (!accFilterInitialised) {
             if (accTargetLooptime) {  /* Initialisation needs to happen once sample rate is known */
                 for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                    biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
+                    biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
                 }
                 accFilterInitialised = true;
             }

src/main/sensors/battery.c

@@ -70,7 +70,7 @@ static void updateBatteryVoltage(void)
     if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
 
     if (!vbatFilterIsInitialised) {
-        biquadFilterInit(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
+        biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
         vbatFilterIsInitialised = true;
     }
     vbatSample = biquadFilterApply(&vbatFilter, vbatSample);

src/main/sensors/gyro.c

@@ -27,6 +27,7 @@
 #include "common/filter.h"
 
 #include "drivers/sensor.h"
+#include "drivers/system.h"
 #include "drivers/accgyro.h"
 
 #include "io/beeper.h"
@@ -44,21 +45,27 @@ float gyroADCf[XYZ_AXIS_COUNT];
 
 static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
 static const gyroConfig_t *gyroConfig;
-static biquadFilter_t gyroFilter[XYZ_AXIS_COUNT];
+static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
+static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT];
+static uint16_t gyroSoftNotchHz;
+static uint8_t gyroSoftNotchQ;
 static uint8_t gyroSoftLpfHz;
 static uint16_t calibratingG = 0;
 
-void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz)
+void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q)
 {
     gyroConfig = gyroConfigToUse;
     gyroSoftLpfHz = gyro_soft_lpf_hz;
+    gyroSoftNotchHz = gyro_soft_notch_hz;
+    gyroSoftNotchQ = gyro_soft_notch_q;
 }
 
 void gyroInit(void)
 {
     if (gyroSoftLpfHz && gyro.targetLooptime) {  // Initialisation needs to happen once samplingrate is known
         for (int axis = 0; axis < 3; axis++) {
-            biquadFilterInit(&gyroFilter[axis], gyroSoftLpfHz, gyro.targetLooptime);
+            biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH);
+            biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
         }
     }
 }
@@ -158,7 +165,17 @@ void gyroUpdate(void)
 
     if (gyroSoftLpfHz) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            gyroADCf[axis] = biquadFilterApply(&gyroFilter[axis], (float)gyroADC[axis]);
+            float sample = (float) gyroADC[axis];
+            if (gyroSoftNotchHz) {
+                sample = biquadFilterApply(&gyroFilterNotch[axis], sample);
+            }
+
+            if (debugMode == DEBUG_NOTCH && axis < 2){
+                debug[axis*2 + 0] = gyroADC[axis];
+                debug[axis*2 + 1] = lrintf(sample);
+            }
+
+            gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);;
             gyroADC[axis] = lrintf(gyroADCf[axis]);
         }
     } else {

src/main/sensors/gyro.h

@@ -40,7 +40,7 @@ typedef struct gyroConfig_s {
     uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
 } gyroConfig_t;
 
-void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz);
+void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q);
 void gyroSetCalibrationCycles(void);
 void gyroInit(void);
 void gyroUpdate(void);