Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 16:55:29 +03:00
Code Issues Wiki Activity

Allow PT2 and PT3 filters on Dterm

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2021-07-26 18:03:52 +02:00
parent 78c4a1967b
commit 70c8f07c73
5 changed files with 72 additions and 51 deletions
Download patch file Download diff file Expand all files Collapse all files

35
src/main/common/filter.c
View file

@ -381,4 +381,37 @@ FAST_CODE float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float
return filter->xk;
}
#endif
#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;
}
}
}

13
src/main/common/filter.h
View file

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

1
src/main/drivers/compass/compass_vcm5883.c
View file

@ -107,7 +107,6 @@ bool vcm5883Detect(magDev_t * mag)
}
if (!deviceDetect(mag)) {
DEBUG_SET(DEBUG_ALWAYS, 0, 7);
busDeviceDeInit(mag->busDev);
return false;
}

16
src/main/fc/settings.yaml
View file

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

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

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