From 86626acbdf118f783188e8ae42fdfea985d53e52 Mon Sep 17 00:00:00 2001 From: rav-rav Date: Sun, 3 Jul 2016 23:54:01 +0200 Subject: [PATCH] add notch filter configurable with gyro_notch_hz and gyro_notch_q --- src/main/blackbox/blackbox.c | 2 ++ src/main/common/filter.c | 39 +++++++++++++++++++++++---------- src/main/common/filter.h | 7 +++++- src/main/config/config.c | 4 +++- src/main/config/config_master.h | 6 +++-- src/main/debug.h | 1 + src/main/flight/mixer.c | 2 +- src/main/io/serial_cli.c | 3 +++ src/main/sensors/acceleration.c | 2 +- src/main/sensors/battery.c | 2 +- src/main/sensors/gyro.c | 25 +++++++++++++++++---- src/main/sensors/gyro.h | 2 +- 12 files changed, 71 insertions(+), 24 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 46bae1958..35c1ba8fc 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1218,6 +1218,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); diff --git a/src/main/common/filter.c b/src/main/common/filter.c index d059fe882..98d9dc3c5 100644 --- a/src/main/common/filter.c +++ b/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; - const float b1 = 1 - cs; - const float b2 = (1 - cs) / 2; - const float a0 = 1 + alpha; - const float a1 = -2 * cs; - const float a2 = 1 - alpha; + float b0, b1, b2, a0, a1, a2; + + switch (filterType) { + case FILTER_LPF: + b0 = (1 - cs) / 2; + 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; diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 66fcb53bb..d97becc01 100644 --- a/src/main/common/filter.h +++ b/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); diff --git a/src/main/config/config.c b/src/main/config/config.c index 003b7c32a..8726a2340 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -455,6 +455,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; @@ -715,7 +717,7 @@ void activateConfig(void) ¤tProfile->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); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index c6f01b402..9d68c0b26 100644 --- a/src/main/config/config_master.h +++ b/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) diff --git a/src/main/debug.h b/src/main/debug.h index 4f6250b75..19124266d 100644 --- a/src/main/debug.h +++ b/src/main/debug.h @@ -50,5 +50,6 @@ typedef enum { DEBUG_MIXER, DEBUG_AIRMODE, DEBUG_PIDLOOP, + DEBUG_NOTCH, DEBUG_COUNT } debugType_e; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b5da4a017..942d63789 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -955,7 +955,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; } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index e160869df..35784d94a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -451,6 +451,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "MIXER", "AIRMODE", "PIDLOOP", + "NOTCH", }; #ifdef OSD static const char * const lookupTableOsdType[] = { @@ -696,6 +697,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 } }, diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index da86d37eb..b5e71f308 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -192,7 +192,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; } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 23159d87c..30c03f130 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -72,7 +72,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); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 4f425dc1d..f5996e87a 100644 --- a/src/main/sensors/gyro.c +++ b/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 "sensors/sensors.h" #include "io/beeper.h" @@ -43,21 +44,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); } } } @@ -157,7 +164,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 { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 2239eb681..4d2153fa0 100644 --- a/src/main/sensors/gyro.h +++ b/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);