Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 11:59:58 +03:00
Code Issues Wiki Activity

add PT2 and PT3 lowpass filter options

Browse source
This commit is contained in:
ctzsnooze 2021-05-18 09:59:27 +10:00
parent 636d563abe
commit 3b62b2e5d4
8 changed files with 123 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/cli/settings.c
View file

@ -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[] = {

18
src/main/flight/pid.c
View file

@ -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;
}
}
}

2
src/main/flight/pid.h
View file

@ -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 {

58
src/main/flight/pid_init.c
View file

@ -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;

22
src/main/sensors/gyro.c
View file

@ -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;
}
}
}

8
src/main/sensors/gyro.h
View file

@ -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 {

32
src/main/sensors/gyro_init.c
View file

@ -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;

8
src/test/unit/pid_unittest.cc
View file

@ -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());
}