diff --git a/src/main/common/filter.c b/src/main/common/filter.c index afa986e8df..71b420eccc 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -381,4 +381,37 @@ FAST_CODE float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float return filter->xk; } -#endif \ No newline at end of file +#endif + +FUNCTION_COMPILE_FOR_SIZE +void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFrequency, const uint32_t refreshRate) { + const float dT = refreshRate * 1e-6f; + + if (cutoffFrequency) { + if (filterType == FILTER_PT1) { + pt1FilterInit(&filter->pt1, cutoffFrequency, dT); + } if (filterType == FILTER_PT2) { + pt2FilterInit(&filter->pt2, pt2FilterGain(cutoffFrequency, dT)); + } if (filterType == FILTER_PT3) { + pt3FilterInit(&filter->pt3, pt3FilterGain(cutoffFrequency, dT)); + } else { + biquadFilterInitLPF(&filter->biquad, cutoffFrequency, refreshRate); + } + } +} + +FUNCTION_COMPILE_FOR_SIZE +void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn) { + *applyFn = nullFilterApply; + if (cutoffFrequency) { + if (filterType == FILTER_PT1) { + *applyFn = (filterApplyFnPtr) pt1FilterApply; + } if (filterType == FILTER_PT2) { + *applyFn = (filterApplyFnPtr) pt2FilterApply; + } if (filterType == FILTER_PT3) { + *applyFn = (filterApplyFnPtr) pt3FilterApply; + } else { + *applyFn = (filterApplyFnPtr) biquadFilterApply; + } + } +} \ No newline at end of file diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 7e6a579efe..c6cedc8649 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -47,12 +47,16 @@ typedef struct biquadFilter_s { typedef union { biquadFilter_t biquad; - pt1Filter_t pt1; + pt1Filter_t pt1; + pt2Filter_t pt2; + pt3Filter_t pt3; } filter_t; typedef enum { FILTER_PT1 = 0, - FILTER_BIQUAD + FILTER_BIQUAD, + FILTER_PT2, + FILTER_PT3 } filterType_e; typedef enum { @@ -127,4 +131,7 @@ float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz); void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT); -float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input); \ No newline at end of file +float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input); + +void initFilter(uint8_t filterType, filter_t *filter, float cutoffFrequency, uint32_t refreshRate); +void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn); \ No newline at end of file diff --git a/src/main/drivers/compass/compass_vcm5883.c b/src/main/drivers/compass/compass_vcm5883.c index b91dccba36..9eb312929b 100644 --- a/src/main/drivers/compass/compass_vcm5883.c +++ b/src/main/drivers/compass/compass_vcm5883.c @@ -107,7 +107,6 @@ bool vcm5883Detect(magDev_t * mag) } if (!deviceDetect(mag)) { - DEBUG_SET(DEBUG_ALWAYS, 0, 7); busDeviceDeInit(mag->busDev); return false; } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0b3ad1cb70..b68dede5b6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -129,6 +129,8 @@ tables: enum: vtxLowerPowerDisarm_e - name: filter_type values: ["PT1", "BIQUAD"] + - name: filter_type_full + values: ["PT1", "BIQUAD", "PT2", "PT3"] - name: log_level values: ["ERROR", "WARNING", "INFO", "VERBOSE", "DEBUG"] - name: iterm_relax @@ -1884,24 +1886,24 @@ groups: max: 900 - name: dterm_lpf_hz description: "Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value" - default_value: 40 + default_value: 110 min: 0 max: 500 - name: dterm_lpf_type - description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation." - default_value: "BIQUAD" + description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, `PT2`, `PT3`." + default_value: "PT2" field: dterm_lpf_type - table: filter_type + table: filter_type_full - name: dterm_lpf2_hz description: "Cutoff frequency for stage 2 D-term low pass filter" default_value: 0 min: 0 max: 500 - name: dterm_lpf2_type - description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation." - default_value: "BIQUAD" + description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, `PT2`, `PT3`." + default_value: "PT1" field: dterm_lpf2_type - table: filter_type + table: filter_type_full - name: yaw_lpf_hz description: "Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below)" default_value: 0 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e2f86d2ee2..b95f6973ac 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -309,6 +309,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, #endif ); +FUNCTION_COMPILE_FOR_SIZE bool pidInitFilters(void) { const uint32_t refreshRate = getLooptime(); @@ -317,26 +318,9 @@ bool pidInitFilters(void) return false; } - // Init other filters - if (pidProfile()->dterm_lpf_hz) { - for (int axis = 0; axis < 3; ++ axis) { - if (pidProfile()->dterm_lpf_type == FILTER_PT1) { - pt1FilterInit(&pidState[axis].dtermLpfState.pt1, pidProfile()->dterm_lpf_hz, refreshRate * 1e-6f); - } else { - biquadFilterInitLPF(&pidState[axis].dtermLpfState.biquad, pidProfile()->dterm_lpf_hz, refreshRate); - } - } - } - - // Init other filters - if (pidProfile()->dterm_lpf2_hz) { - for (int axis = 0; axis < 3; ++ axis) { - if (pidProfile()->dterm_lpf2_type == FILTER_PT1) { - pt1FilterInit(&pidState[axis].dtermLpf2State.pt1, pidProfile()->dterm_lpf2_hz, refreshRate * 1e-6f); - } else { - biquadFilterInitLPF(&pidState[axis].dtermLpf2State.biquad, pidProfile()->dterm_lpf2_hz, refreshRate); - } - } + for (int axis = 0; axis < 3; ++ axis) { + initFilter(pidProfile()->dterm_lpf_type, &pidState[axis].dtermLpfState, pidProfile()->dterm_lpf_hz, refreshRate); + initFilter(pidProfile()->dterm_lpf2_type, &pidState[axis].dtermLpf2State, pidProfile()->dterm_lpf2_hz, refreshRate); } for (int i = 0; i < XYZ_AXIS_COUNT; i++) { @@ -713,8 +697,6 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) { dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT); dBoost = constrainf(dBoost, dBoostMin, dBoostMax); - DEBUG_SET(DEBUG_ALWAYS, pidState->axis, dBoost * 100); - return dBoost; } #else @@ -733,10 +715,23 @@ static float dTermProcess(pidState_t *pidState, float dT) { newDTerm = 0; } else { float delta = pidState->previousRateGyro - pidState->gyroRate; + + if (pidState->axis == FD_ROLL) { + DEBUG_SET(DEBUG_ALWAYS, 0, delta); + } delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta); + + if (pidState->axis == FD_ROLL) { + DEBUG_SET(DEBUG_ALWAYS, 1, delta); + } + delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta); + if (pidState->axis == FD_ROLL) { + DEBUG_SET(DEBUG_ALWAYS, 2, delta); + } + // Calculate derivative newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, dT); } @@ -1230,23 +1225,8 @@ void pidInit(void) usedPidControllerType = pidProfile()->pidControllerType; } - dTermLpfFilterApplyFn = nullFilterApply; - if (pidProfile()->dterm_lpf_hz) { - if (pidProfile()->dterm_lpf_type == FILTER_PT1) { - dTermLpfFilterApplyFn = (filterApplyFnPtr) pt1FilterApply; - } else { - dTermLpfFilterApplyFn = (filterApplyFnPtr) biquadFilterApply; - } - } - - dTermLpf2FilterApplyFn = nullFilterApply; - if (pidProfile()->dterm_lpf2_hz) { - if (pidProfile()->dterm_lpf2_type == FILTER_PT1) { - dTermLpf2FilterApplyFn = (filterApplyFnPtr) pt1FilterApply; - } else { - dTermLpf2FilterApplyFn = (filterApplyFnPtr) biquadFilterApply; - } - } + assignFilterApplyFn(pidProfile()->dterm_lpf_type, pidProfile()->dterm_lpf_hz, &dTermLpfFilterApplyFn); + assignFilterApplyFn(pidProfile()->dterm_lpf2_type, pidProfile()->dterm_lpf2_hz, &dTermLpf2FilterApplyFn); if (usedPidControllerType == PID_TYPE_PIFF) { pidControllerApplyFn = pidApplyFixedWingRateController;