Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 23:35:34 +03:00
Code Issues Wiki Activity

Cleanup Denoise Code

Browse source
This commit is contained in:
borisbstyle 2016-10-08 01:49:10 +02:00
parent 8dd8e3969f
commit b168b33448
5 changed files with 30 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -108,7 +108,7 @@ static pt1Filter_t deltaFilter[3];
static pt1Filter_t yawFilter;
static biquadFilter_t dtermFilterLpf[3];
static biquadFilter_t dtermFilterNotch[3];
static float dtermFilterDenoise[XYZ_AXIS_COUNT][MAX_DENOISE_WINDOW_SIZE];
static denoisingState_t dtermDenoisingState[3];
static bool dtermNotchInitialised, dtermLpfInitialised;
void initFilters(const pidProfile_t *pidProfile) {
@ -126,6 +126,13 @@ void initFilters(const pidProfile_t *pidProfile) {
dtermLpfInitialised = true;
}
}
if (pidProfile->dterm_filter_type == FILTER_DENOISE) {
if (pidProfile->dterm_lpf_hz && !dtermLpfInitialised) {
for (axis = 0; axis < 3; axis++) initDenoisingFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
dtermLpfInitialised = true;
}
}
}
#ifndef SKIP_PID_FLOAT
@ -277,7 +284,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
else if (pidProfile->dterm_filter_type == FILTER_PT1)
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
else
delta = denoisingFilterUpdate(delta, 3, dtermFilterDenoise[axis]);
delta = denoisingFilterUpdate(&dtermDenoisingState[axis], delta);
}
DTerm = Kd[axis] * delta * tpaFactor;
@ -417,7 +424,7 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
else if (pidProfile->dterm_filter_type == FILTER_PT1)
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
else
delta = denoisingFilterUpdate(delta, 3, dtermFilterDenoise[axis]);
delta = denoisingFilterUpdate(&dtermDenoisingState[axis], delta);
delta = lrintf(deltaf);
}