diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 6fc30d4066..e9da396c5b 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -294,11 +294,15 @@ static const char * const lookupTablePwmProtocol[] = { static const char * const lookupTableLowpassType[] = { "PT1", "BIQUAD", + "PT2", + "PT3", }; static const char * const lookupTableDtermLowpassType[] = { "PT1", "BIQUAD", + "PT2", + "PT3", }; static const char * const lookupTableAntiGravityMode[] = { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7e86dbfccb..a76452305a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1231,15 +1231,27 @@ void dynLpfDTermUpdate(float throttle) } else { cutoffFreq = fmax(dynThrottle(throttle) * pidRuntime.dynLpfMax, pidRuntime.dynLpfMin); } - - if (pidRuntime.dynLpfFilter == DYN_LPF_PT1) { + switch (pidRuntime.dynLpfFilter) { + case DYN_LPF_PT1: for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { pt1FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, pidRuntime.dT)); } - } else if (pidRuntime.dynLpfFilter == DYN_LPF_BIQUAD) { + break; + case DYN_LPF_BIQUAD: for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { biquadFilterUpdateLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime); } + break; + case DYN_LPF_PT2: + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + pt2FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt2Filter, pt2FilterGain(cutoffFreq, pidRuntime.dT)); + } + break; + case DYN_LPF_PT3: + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + pt3FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt3Filter, pt3FilterGain(cutoffFreq, pidRuntime.dT)); + } + break; } } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index fa699fdecd..009b289300 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -253,6 +253,8 @@ typedef struct pidAxisData_s { typedef union dtermLowpass_u { pt1Filter_t pt1Filter; biquadFilter_t biquadFilter; + pt2Filter_t pt2Filter; + pt3Filter_t pt3Filter; } dtermLowpass_t; typedef struct pidCoefficient_s { diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index ae0c65df13..d623b0dcd3 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -110,7 +110,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) } #endif - if (dterm_lowpass_hz > 0 && dterm_lowpass_hz < pidFrequencyNyquist) { + if (dterm_lowpass_hz > 0) { switch (pidProfile->dterm_filter_type) { case FILTER_PT1: pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply; @@ -119,13 +119,29 @@ void pidInitFilters(const pidProfile_t *pidProfile) } break; case FILTER_BIQUAD: + if (pidProfile->dterm_lowpass_hz < pidFrequencyNyquist) { #ifdef USE_DYN_LPF - pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; + pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; #else - pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply; + pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply; #endif + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime); + } + } else { + pidRuntime.dtermLowpassApplyFn = nullFilterApply; + } + break; + case FILTER_PT2: + pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt2FilterApply; for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime); + pt2FilterInit(&pidRuntime.dtermLowpass[axis].pt2Filter, pt2FilterGain(dterm_lowpass_hz, pidRuntime.dT)); + } + break; + case FILTER_PT3: + pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt3FilterApply; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + pt3FilterInit(&pidRuntime.dtermLowpass[axis].pt3Filter, pt3FilterGain(dterm_lowpass_hz, pidRuntime.dT)); } break; default: @@ -137,9 +153,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) } //2nd Dterm Lowpass Filter - if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) { - pidRuntime.dtermLowpass2ApplyFn = nullFilterApply; - } else { + if (pidProfile->dterm_lowpass2_hz > 0) { switch (pidProfile->dterm_filter2_type) { case FILTER_PT1: pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; @@ -148,18 +162,36 @@ void pidInitFilters(const pidProfile_t *pidProfile) } break; case FILTER_BIQUAD: - pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply; + if (pidProfile->dterm_lowpass2_hz < pidFrequencyNyquist) { + pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime); + } + } else { + pidRuntime.dtermLowpassApplyFn = nullFilterApply; + } + break; + case FILTER_PT2: + pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt2FilterApply; for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime); + pt2FilterInit(&pidRuntime.dtermLowpass2[axis].pt2Filter, pt2FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT)); + } + break; + case FILTER_PT3: + pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt3FilterApply; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + pt3FilterInit(&pidRuntime.dtermLowpass2[axis].pt3Filter, pt3FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT)); } break; default: pidRuntime.dtermLowpass2ApplyFn = nullFilterApply; break; } + } else { + pidRuntime.dtermLowpass2ApplyFn = nullFilterApply; } - if (pidProfile->yaw_lowpass_hz == 0 || pidProfile->yaw_lowpass_hz > pidFrequencyNyquist) { + if (pidProfile->yaw_lowpass_hz == 0) { pidRuntime.ptermYawLowpassApplyFn = nullFilterApply; } else { pidRuntime.ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply; @@ -333,6 +365,12 @@ void pidInitConfig(const pidProfile_t *pidProfile) case FILTER_BIQUAD: pidRuntime.dynLpfFilter = DYN_LPF_BIQUAD; break; + case FILTER_PT2: + pidRuntime.dynLpfFilter = DYN_LPF_PT2; + break; + case FILTER_PT3: + pidRuntime.dynLpfFilter = DYN_LPF_PT3; + break; default: pidRuntime.dynLpfFilter = DYN_LPF_NONE; break; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 93ecbab10f..051b4a7ab9 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -635,17 +635,29 @@ void dynLpfGyroUpdate(float throttle) } else { cutoffFreq = fmax(dynThrottle(throttle) * gyro.dynLpfMax, gyro.dynLpfMin); } - if (gyro.dynLpfFilter == DYN_LPF_PT1) { - DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); - const float gyroDt = gyro.targetLooptime * 1e-6f; + DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); + const float gyroDt = gyro.targetLooptime * 1e-6f; + switch (gyro.dynLpfFilter) { + case DYN_LPF_PT1: for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { pt1FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt)); } - } else if (gyro.dynLpfFilter == DYN_LPF_BIQUAD) { - DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq); + break; + case DYN_LPF_BIQUAD: for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { biquadFilterUpdateLPF(&gyro.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime); } + break; + case DYN_LPF_PT2: + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + pt2FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt2FilterState, pt2FilterGain(cutoffFreq, gyroDt)); + } + break; + case DYN_LPF_PT3: + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + pt3FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt3FilterState, pt3FilterGain(cutoffFreq, gyroDt)); + } + break; } } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b18094edf0..b588ba7a1d 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -36,7 +36,7 @@ #include "pg/pg.h" -#define FILTER_FREQUENCY_MAX 4000 // maximum frequency for filter cutoffs (nyquist limit of 8K max sampling) +#define FILTER_FREQUENCY_MAX 1000 // so little filtering above 1000hz that if the user wants less delay, they must disable the filter #define DYN_LPF_FILTER_FREQUENCY_MAX 1000 #define DYN_LPF_GYRO_MIN_HZ_DEFAULT 200 @@ -51,6 +51,8 @@ typedef union gyroLowpassFilter_u { pt1Filter_t pt1FilterState; biquadFilter_t biquadFilterState; + pt2Filter_t pt2FilterState; + pt3Filter_t pt3FilterState; } gyroLowpassFilter_t; typedef enum gyroDetectionFlags_e { @@ -147,7 +149,9 @@ enum { enum { DYN_LPF_NONE = 0, DYN_LPF_PT1, - DYN_LPF_BIQUAD + DYN_LPF_BIQUAD, + DYN_LPF_PT2, + DYN_LPF_PT3, }; typedef enum { diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index c41ab1c710..5b55d62822 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -181,8 +181,8 @@ static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_ // type. It will be overridden for positive cases. *lowpassFilterApplyFn = nullFilterApply; - // If lowpass cutoff has been specified and is less than the Nyquist frequency - if (lpfHz && lpfHz <= gyroFrequencyNyquist) { + // If lowpass cutoff has been specified + if (lpfHz) { switch (type) { case FILTER_PT1: *lowpassFilterApplyFn = (filterApplyFnPtr) pt1FilterApply; @@ -192,13 +192,29 @@ static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_ ret = true; break; case FILTER_BIQUAD: + if (lpfHz <= gyroFrequencyNyquist) { #ifdef USE_DYN_LPF - *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1; + *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1; #else - *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply; + *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply; #endif + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, looptime); + } + ret = true; + } + break; + case FILTER_PT2: + *lowpassFilterApplyFn = (filterApplyFnPtr) pt2FilterApply; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, looptime); + pt2FilterInit(&lowpassFilter[axis].pt2FilterState, gain); + } + ret = true; + break; + case FILTER_PT3: + *lowpassFilterApplyFn = (filterApplyFnPtr) pt3FilterApply; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + pt3FilterInit(&lowpassFilter[axis].pt3FilterState, gain); } ret = true; break; @@ -218,6 +234,12 @@ static void dynLpfFilterInit() case FILTER_BIQUAD: gyro.dynLpfFilter = DYN_LPF_BIQUAD; break; + case FILTER_PT2: + gyro.dynLpfFilter = DYN_LPF_PT2; + break; + case FILTER_PT3: + gyro.dynLpfFilter = DYN_LPF_PT3; + break; default: gyro.dynLpfFilter = DYN_LPF_NONE; break; diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 9fa44af118..8e6af4caff 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -548,11 +548,11 @@ TEST(pidControllerTest, testFeedForward) { EXPECT_NEAR(515.26, pidData[FD_PITCH].F, calculateTolerance(515.26)); EXPECT_NEAR(515.26, pidData[FD_YAW].F, calculateTolerance(515.26)); - setStickPosition(FD_ROLL, 0.0f); - setStickPosition(FD_PITCH, 0.0f); - setStickPosition(FD_YAW, 0.0f); + setStickPosition(FD_ROLL, 0.0f); + setStickPosition(FD_PITCH, 0.0f); + setStickPosition(FD_YAW, 0.0f); - for (int loop = 0; loop <= 15; loop++) { + for (int loop = 0; loop <= 15; loop++) { gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL]; pidController(pidProfile, currentTestTime()); }