diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index d97cb39a7..9ab7e8a0b 100644 --- a/src/main/blackbox/blackbox.c +++ b/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); 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 3cdc4f82a..0caa5c62e 100755 --- a/src/main/config/config.c +++ b/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) ¤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 6632ca3af..5ce1d3683 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 b851fd355..8db1443d0 100755 --- a/src/main/flight/mixer.c +++ b/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; } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 995484389..490476425 100644 --- a/src/main/io/serial_cli.c +++ b/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 } }, diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 5de9c3fce..d8cc423c1 100644 --- a/src/main/sensors/acceleration.c +++ b/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; } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index fed53524e..53e709648 100644 --- a/src/main/sensors/battery.c +++ b/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); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 67e56b43d..40a30ebb7 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 "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 { 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);