diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 527ac4d5d9..468b944d99 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -24,30 +24,23 @@ #include "common/filter.h" #include "common/maths.h" -#include "drivers/gyro_sync.h" - - #define M_LN2_FLOAT 0.69314718055994530942f #define M_PI_FLOAT 3.14159265358979323846f #define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */ /* sets up a biquad Filter */ -void BiQuadNewLpf(uint8_t filterCutFreq, biquad_t *newState, float refreshRate) +void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate) { - float samplingRate; + float sampleRate; - if (!refreshRate) { - samplingRate = 1 / (targetLooptime * 0.000001f); - } else { - samplingRate = refreshRate; - } + sampleRate = 1 / ((float)refreshRate * 0.000001f); float omega, sn, cs, alpha; float a0, a1, a2, b0, b1, b2; /* setup variables */ - omega = 2 * M_PI_FLOAT * (float) filterCutFreq / samplingRate; + omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate; sn = sinf(omega); cs = cosf(omega); alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn); @@ -60,11 +53,11 @@ void BiQuadNewLpf(uint8_t filterCutFreq, biquad_t *newState, float refreshRate) a2 = 1 - alpha; /* precompute the coefficients */ - newState->a0 = b0 /a0; - newState->a1 = b1 /a0; - newState->a2 = b2 /a0; - newState->a3 = a1 /a0; - newState->a4 = a2 /a0; + newState->b0 = b0 /a0; + newState->b1 = b1 /a0; + newState->b2 = b2 /a0; + newState->a1 = a1 /a0; + newState->a2 = a2 /a0; /* zero initial samples */ newState->x1 = newState->x2 = 0; @@ -77,8 +70,8 @@ float applyBiQuadFilter(float sample, biquad_t *state) float result; /* compute result */ - result = state->a0 * sample + state->a1 * state->x1 + state->a2 * state->x2 - - state->a3 * state->y1 - state->a4 * state->y2; + result = state->b0 * sample + state->b1 * state->x1 + state->b2 * state->x2 - + state->a1 * state->y1 - state->a2 * state->y2; /* shift x1 to x2, sample to x1 */ state->x2 = state->x1; diff --git a/src/main/common/filter.h b/src/main/common/filter.h index c6b7918ce2..11ea51c9d9 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -17,9 +17,9 @@ /* this holds the data required to update samples thru a filter */ typedef struct biquad_s { - float a0, a1, a2, a3, a4; + float b0, b1, b2, a1, a2; float x1, x2, y1, y2; } biquad_t; float applyBiQuadFilter(float sample, biquad_t *state); -void BiQuadNewLpf(uint8_t filterCutFreq, biquad_t *newState, float refreshRate); +void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate); diff --git a/src/main/config/config.c b/src/main/config/config.c index ea5ce2cb1a..58f626410f 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 119; +static const uint8_t EEPROM_CONF_VERSION = 120; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -176,7 +176,6 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->I8[PIDVEL] = 45; pidProfile->D8[PIDVEL] = 1; - pidProfile->gyro_lpf_hz = 60; // filtering ON by default pidProfile->dterm_lpf_hz = 0; // filtering ON by default pidProfile->deltaFromGyro = 1; pidProfile->airModeInsaneAcrobilityFactor = 0; @@ -406,6 +405,7 @@ static void resetConf(void) masterConfig.dcm_kp = 2500; // 1.0 * 10000 masterConfig.dcm_ki = 0; // 0.003 * 10000 masterConfig.gyro_lpf = 1; // 188HZ + masterConfig.gyro_soft_lpf_hz = 60; resetAccelerometerTrims(&masterConfig.accZero); @@ -770,7 +770,7 @@ void activateConfig(void) ¤tProfile->pidProfile ); - useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_lpf_hz); + useGyroConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz); #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 037fb5dbf7..0293c0ec69 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -51,6 +51,7 @@ typedef struct master_t { 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. uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. + float gyro_soft_lpf_hz; // Biqyad gyro lpf hz 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/flight/pid.c b/src/main/flight/pid.c index 7c1e1d0bec..dadc564919 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -142,7 +142,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa float horizonLevelStrength = 1; if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0); + for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime); deltaStateIsSet = true; } @@ -284,7 +284,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat int8_t horizonLevelStrength = 100; if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0); + for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime); deltaStateIsSet = true; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 053dc0406d..f9c6f5d3f5 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -60,8 +60,7 @@ typedef struct pidProfile_s { uint8_t H_sensitivity; uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor - uint8_t gyro_lpf_hz; // Gyro Soft filter in hz - uint8_t dterm_lpf_hz; // Delta Filter in hz + float dterm_lpf_hz; // Delta Filter in hz uint8_t deltaFromGyro; // Alternative delta Calculation #ifdef GTUNE diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 20749fcec8..e04f90a852 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -601,6 +601,7 @@ const clivalue_t valueTable[] = { { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, + { "gyro_soft_lpf", VAR_FLOAT | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } }, { "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 } }, @@ -705,8 +706,7 @@ const clivalue_t valueTable[] = { { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } }, - { "soft_gyro_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_lpf_hz, .config.minmax = {0, 255 } }, - { "dterm_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 255 } }, + { "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "acro_plus_factor", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 100 } }, #ifdef BLACKBOX diff --git a/src/main/mw.c b/src/main/mw.c index f3a30c6571..a8711b4b22 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -184,7 +184,7 @@ void filterRc(void){ /* Initialize cycletime filter */ if (!filterIsSet) { - BiQuadNewLpf(1, &filteredCycleTimeState, 0); + BiQuadNewLpf(0.5f, &filteredCycleTimeState, targetLooptime); filterIsSet = true; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index daf11fee6a..6f1ac06e91 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -41,15 +41,15 @@ int16_t gyroADC[XYZ_AXIS_COUNT]; int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; static gyroConfig_t *gyroConfig; -static biquad_t gyroBiQuadState[3]; +static biquad_t gyroFilterState[3]; static bool gyroFilterStateIsSet; -static uint8_t gyroLpfCutFreq; +static float gyroLpfCutFreq; int axis; gyro_t gyro; // gyro access functions sensor_align_e gyroAlign = 0; -void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz) +void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz) { gyroConfig = gyroConfigToUse; gyroLpfCutFreq = gyro_lpf_hz; @@ -57,7 +57,7 @@ void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz) void initGyroFilterCoefficients(void) { if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */ - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroBiQuadState[axis], 0); + for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], targetLooptime); gyroFilterStateIsSet = true; } } @@ -140,10 +140,10 @@ void gyroUpdate(void) alignSensors(gyroADC, gyroADC, gyroAlign); if (gyroLpfCutFreq) { - if (!gyroFilterStateIsSet) { - initGyroFilterCoefficients(); - } else { - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis])); + if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */ + + if (gyroFilterStateIsSet) { + for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis])); } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 2daaeffbe7..8131089e52 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -39,7 +39,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 useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz); +void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz); void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); void gyroUpdate(void); bool isGyroCalibrationComplete(void);