diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 21fd2948f..d059fe882 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -17,76 +17,81 @@ #include #include -#include #include -#include "common/axis.h" #include "common/filter.h" #include "common/maths.h" -#define M_LN2_FLOAT 0.69314718055994530942f -#define M_PI_FLOAT 3.14159265358979323846f +#define M_LN2_FLOAT 0.69314718055994530942f +#define M_PI_FLOAT 3.14159265358979323846f #define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */ #define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/ -// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime) -float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT) { +// PT1 Low Pass filter - // Pre calculate and store RC - if (!filter->RC) { - filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut ); - } +void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT) +{ + filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut ); + filter->dT = dT; +} - filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state); +float pt1FilterApply(pt1Filter_t *filter, float input) +{ + filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state); + return filter->state; +} + +float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT) +{ + // Pre calculate and store RC + if (!filter->RC) { + filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut ); + filter->dT = dT; + } + + filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state); return filter->state; } /* sets up a biquad Filter */ -void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate) +void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate) { - float sampleRate; + const float sampleRate = 1 / ((float)refreshRate * 0.000001f); - 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 * filterCutFreq / sampleRate; - sn = sinf(omega); - cs = cosf(omega); + // setup variables + const float omega = 2 * M_PI_FLOAT * filterCutFreq / 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); - alpha = sn / (2 * BIQUAD_Q); + const float alpha = sn / (2 * BIQUAD_Q); - b0 = (1 - cs) / 2; - b1 = 1 - cs; - b2 = (1 - cs) / 2; - a0 = 1 + alpha; - a1 = -2 * cs; - a2 = 1 - alpha; + 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; - /* precompute the coefficients */ - newState->b0 = b0 / a0; - newState->b1 = b1 / a0; - newState->b2 = b2 / a0; - newState->a1 = a1 / a0; - newState->a2 = a2 / a0; + // precompute the coefficients + filter->b0 = b0 / a0; + filter->b1 = b1 / a0; + filter->b2 = b2 / a0; + filter->a1 = a1 / a0; + filter->a2 = a2 / a0; - /* zero initial samples */ - newState->d1 = newState->d2 = 1; + // zero initial samples + filter->d1 = filter->d2 = 0; } /* Computes a biquad_t filter on a sample */ -float applyBiQuadFilter(float sample, biquad_t *state) //direct form 2 transposed +float biquadFilterApply(biquadFilter_t *filter, float input) { - float result; - - result = state->b0 * sample + state->d1; - state->d1 = state->b1 * sample - state->a1 * result + state->d2; - state->d2 = state->b2 * sample - state->a2 * result; + const float result = filter->b0 * input + filter->d1; + filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2; + filter->d2 = filter->b2 * input - filter->a2 * result; return result; } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 809063f4b..66fcb53bb 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -17,20 +17,26 @@ #define DELTA_MAX_SAMPLES 12 -typedef struct filterStatePt1_s { - float state; - float RC; - float constdT; -} filterStatePt1_t; +typedef struct pt1Filter_s { + float state; + float RC; + float dT; +} pt1Filter_t; /* this holds the data required to update samples thru a filter */ -typedef struct biquad_s { +typedef struct biquadFilter_s { float b0, b1, b2, a1, a2; float d1, d2; -} biquad_t; +} biquadFilter_t; + + +void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate); +float biquadFilterApply(biquadFilter_t *filter, float input); + +void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT); +float pt1FilterApply(pt1Filter_t *filter, float input); +float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT); -float applyBiQuadFilter(float sample, biquad_t *state); -float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt); int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]); float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]); -void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate); + diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 2e200cff1..d885d30ea 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -28,7 +28,6 @@ #include "debug.h" #include "common/axis.h" -#include "common/filter.h" #include "drivers/system.h" #include "drivers/sensor.h" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index adc0145ba..b5da4a017 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -946,7 +946,7 @@ void filterServos(void) #ifdef USE_SERVOS static int16_t servoIdx; static bool servoFilterIsSet; - static biquad_t servoFilterState[MAX_SUPPORTED_SERVOS]; + static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS]; #if defined(MIXER_DEBUG) uint32_t startTime = micros(); @@ -955,11 +955,11 @@ void filterServos(void) if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { if (!servoFilterIsSet) { - BiQuadNewLpf(mixerConfig->servo_lowpass_freq, &servoFilterState[servoIdx], targetPidLooptime); + biquadFilterInit(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime); servoFilterIsSet = true; } - servo[servoIdx] = lrintf(applyBiQuadFilter((float) servo[servoIdx], &servoFilterState[servoIdx])); + servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); // Sanity check servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 91ccbc2d8..b0dee8749 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -112,8 +112,8 @@ float getdT (void) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -static filterStatePt1_t deltaFilterState[3]; -static filterStatePt1_t yawFilterState; +static pt1Filter_t deltaFilter[3]; +static pt1Filter_t yawFilter; #ifndef SKIP_PID_LUXFLOAT static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, @@ -205,7 +205,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat //-----calculate D-term if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); axisPID[axis] = lrintf(PTerm + ITerm); @@ -230,7 +230,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat delta *= (1.0f / getdT()); // Filter delta - if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); + if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f); @@ -344,7 +344,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin //-----calculate D-term if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); axisPID[axis] = PTerm + ITerm; @@ -369,7 +369,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; // Filter delta - if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); + if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT()); DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index d79eb2e14..da86d37eb 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -47,7 +47,6 @@ uint32_t accTargetLooptime; static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. extern uint16_t InflightcalibratingA; -extern bool AccInflightCalibrationArmed; extern bool AccInflightCalibrationMeasurementDone; extern bool AccInflightCalibrationSavetoEEProm; extern bool AccInflightCalibrationActive; @@ -55,7 +54,7 @@ extern bool AccInflightCalibrationActive; static flightDynamicsTrims_t *accelerationTrims; static float accLpfCutHz = 0; -static biquad_t accFilterState[XYZ_AXIS_COUNT]; +static biquadFilter_t accFilter[XYZ_AXIS_COUNT]; static bool accFilterInitialised = false; void accSetCalibrationCycles(uint16_t calibrationCyclesRequired) @@ -87,9 +86,8 @@ void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims) void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims) { static int32_t a[3]; - int axis; - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { // Reset a[axis] at start of calibration if (isOnFirstAccelerationCalibrationCycle()) @@ -179,14 +177,13 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims) void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) { - int16_t accADCRaw[XYZ_AXIS_COUNT]; - int axis; + int16_t accADCRaw[XYZ_AXIS_COUNT]; if (!acc.read(accADCRaw)) { return; } - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis]; accSmooth[axis] = accADCRaw[axis]; } @@ -194,13 +191,17 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) if (accLpfCutHz) { if (!accFilterInitialised) { if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */ - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(accLpfCutHz, &accFilterState[axis], accTargetLooptime); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime); + } accFilterInitialised = true; } } if (accFilterInitialised) { - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accSmooth[axis] = lrintf(applyBiQuadFilter((float) accSmooth[axis], &accFilterState[axis])); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis])); + } } } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 5bcc265f1..23159d87c 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -63,20 +63,19 @@ uint16_t batteryAdcToVoltage(uint16_t src) static void updateBatteryVoltage(void) { - uint16_t vbatSample; - static biquad_t vbatFilterState; - static bool vbatFilterStateIsSet; + static biquadFilter_t vbatFilter; + static bool vbatFilterIsInitialised; // store the battery voltage with some other recent battery voltage readings - vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); + uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample; - if (!vbatFilterStateIsSet) { - BiQuadNewLpf(VBATT_LPF_FREQ, &vbatFilterState, 50000); //50HZ Update - vbatFilterStateIsSet = true; + if (!vbatFilterIsInitialised) { + biquadFilterInit(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update + vbatFilterIsInitialised = true; } - vbatSample = applyBiQuadFilter(vbatSample, &vbatFilterState); + vbatSample = biquadFilterApply(&vbatFilter, vbatSample); vbat = batteryAdcToVoltage(vbatSample); if (debugMode == DEBUG_BATTERY) debug[1] = vbat; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c79d29013..4f425dc1d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -43,7 +43,7 @@ float gyroADCf[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static const gyroConfig_t *gyroConfig; -static biquad_t gyroFilterState[XYZ_AXIS_COUNT]; +static biquadFilter_t gyroFilter[XYZ_AXIS_COUNT]; static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; @@ -57,7 +57,7 @@ void gyroInit(void) { if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { - BiQuadNewLpf(gyroSoftLpfHz, &gyroFilterState[axis], gyro.targetLooptime); + biquadFilterInit(&gyroFilter[axis], gyroSoftLpfHz, gyro.targetLooptime); } } } @@ -157,7 +157,7 @@ void gyroUpdate(void) if (gyroSoftLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]); + gyroADCf[axis] = biquadFilterApply(&gyroFilter[axis], (float)gyroADC[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); } } else {