Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Merge pull request #5240 from iNavFlight/dzikuvx-dterm-filters-refactor

Browse source 
Dterm filters refactoring and enhancement
This commit is contained in:
Paweł Spychalski 2019-12-29 12:34:19 +01:00 • committed by GitHub
commit c89469216e
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 74 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

9
src/main/blackbox/blackbox.c
View file

@ -1682,12 +1682,21 @@ static bool blackboxWriteSysinfo(void)
pidBank()->pid[PID_VEL_Z].D);
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz", "%d", pidProfile()->yaw_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz", "%d", pidProfile()->dterm_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_type", "%d", pidProfile()->dterm_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf2_hz", "%d", pidProfile()->dterm_lpf2_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf2_type", "%d", pidProfile()->dterm_lpf2_type);
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", pidProfile()->dterm_soft_notch_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff", "%d", pidProfile()->dterm_soft_notch_cutoff);
BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_soft_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_soft_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf2_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_width_percent", "%d", gyroConfig()->dyn_notch_width_percent);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_range", "%d", gyroConfig()->dyn_notch_range);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
gyroConfig()->gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,

2
src/main/fc/fc_msp.c
View file

@ -1991,7 +1991,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_FILTER_CONFIG :
if (dataSize >= 5) {
gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 255);
pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 500);
pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
if (dataSize >= 9) {
gyroConfigMutable()->gyro_soft_notch_hz_1 = constrain(sbufReadU16(src), 0, 500);

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

@ -1080,7 +1080,16 @@ groups:
max: 900
- name: dterm_lpf_hz
min: 0
max: 200
max: 500
- name: dterm_lpf_type
field: dterm_lpf_type
table: filter_type
- name: dterm_lpf2_hz
min: 0
max: 500
- name: dterm_lpf2_type
field: dterm_lpf2_type
table: filter_type
- name: use_dterm_fir_filter
field: use_dterm_fir_filter
type: bool

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

@ -81,7 +81,8 @@ typedef struct {
// Rate filtering
rateLimitFilter_t axisAccelFilter;
pt1Filter_t ptermLpfState;
biquadFilter_t deltaLpfState;
filter_t dtermLpfState;
filter_t dtermLpf2State;
// Dterm notch filtering
biquadFilter_t deltaNotchFilter;
float stickPosition;
@ -145,8 +146,9 @@ static EXTENDED_FASTRAM uint8_t usedPidControllerType;
typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT);
static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11);
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 12);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = {
@ -217,7 +219,10 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.dterm_soft_notch_hz = 0,
.dterm_soft_notch_cutoff = 1,
.dterm_lpf_type = 1, //Default to BIQUAD
.dterm_lpf_hz = 40,
.dterm_lpf2_type = 1, //Default to BIQUAD
.dterm_lpf2_hz = 0, // Off by default
.yaw_lpf_hz = 0,
.dterm_setpoint_weight = 1.0f,
.use_dterm_fir_filter = 1,
@ -294,8 +299,25 @@ bool pidInitFilters(void)
}
// Init other filters
for (int axis = 0; axis < 3; ++ axis) {
biquadFilterInitLPF(&pidState[axis].deltaLpfState, pidProfile()->dterm_lpf_hz, refreshRate);
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 i = 0; i < XYZ_AXIS_COUNT; i++) {
@ -690,7 +712,8 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
#endif
// Apply additional lowpass
deltaFiltered = dTermLpfFilterApplyFn(&pidState->deltaLpfState, deltaFiltered);
deltaFiltered = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, deltaFiltered);
deltaFiltered = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, deltaFiltered);
firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
newDTerm = firFilterApply(&pidState->gyroRateFilter);
@ -1058,10 +1081,22 @@ void pidInit(void)
usedPidControllerType = pidProfile()->pidControllerType;
}
dTermLpfFilterApplyFn = nullFilterApply;
if (pidProfile()->dterm_lpf_hz) {
dTermLpfFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
} else {
dTermLpfFilterApplyFn = nullFilterApply;
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;
}
}
if (usedPidControllerType == PID_TYPE_PIFF) {

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

@ -105,7 +105,13 @@ typedef struct pidProfile_s {
uint16_t dterm_soft_notch_hz; // Dterm Notch frequency
uint16_t dterm_soft_notch_cutoff; // Dterm Notch Cutoff frequency
uint8_t dterm_lpf_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
uint8_t dterm_lpf_type; // Dterm LPF type: PT1, BIQUAD
uint16_t dterm_lpf_hz;
uint8_t dterm_lpf2_type; // Dterm LPF type: PT1, BIQUAD
uint16_t dterm_lpf2_hz;
uint8_t use_dterm_fir_filter; // Use classical INAV FIR differentiator. Very noise robust, can be quite slowish
uint8_t yaw_lpf_hz;

8
src/main/sensors/acceleration.c
View file

@ -78,11 +78,11 @@ STATIC_FASTRAM filter_t accFilter[XYZ_AXIS_COUNT];
STATIC_FASTRAM filterApplyFnPtr accSoftLpfFilterApplyFn;
STATIC_FASTRAM void *accSoftLpfFilter[XYZ_AXIS_COUNT];
STATIC_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT];
STATIC_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
STATIC_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
STATIC_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
static EXTENDED_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 3);