Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +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

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

@ -115,15 +115,19 @@ float biquadFilterApply(biquadFilter_t *filter, float input)
return result;
}
void initDenoisingFilter(denoisingState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) {
filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_DENOISE_WINDOW_SIZE);
}
/* prototype function for denoising of signal by dynamic moving average. Mainly for test purposes */
float denoisingFilterUpdate(float input, uint8_t count, float filter[MAX_DENOISE_WINDOW_SIZE]) {
float denoisingFilterUpdate(denoisingState_t *filter, float input) {
int index;
float averageSum = 0.0f;
for (index = count-1; index > 0; index--) filter[index] = filter[index-1];
filter[0] = input;
for (count = 0; count < count; index++) averageSum += filter[index];
for (index = filter->targetCount-1; index > 0; index--) filter->state[index] = filter->state[index-1];
filter->state[0] = input;
for (int count = 0; count < filter->targetCount; index++) averageSum += filter->state[index];
return averageSum / count;
return averageSum / filter->targetCount;
}

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

@ -29,6 +29,11 @@ typedef struct biquadFilter_s {
float d1, d2;
} biquadFilter_t;
typedef struct dennoisingState_s {
int targetCount;
float state[MAX_DENOISE_WINDOW_SIZE];
} denoisingState_t;
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
@ -48,6 +53,6 @@ float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);
float pt1FilterApply(pt1Filter_t *filter, float input);
float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT);
float denoisingFilterUpdate(float input, uint8_t count, float filter[MAX_DENOISE_WINDOW_SIZE]);
void initDenoisingFilter(denoisingState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime);
float denoisingFilterUpdate(denoisingState_t *filter, float input);

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

2
src/main/io/serial_cli.c
View file

@ -515,7 +515,7 @@ static const char * const lookupTableRcInterpolation[] = {
};
static const char * const lookupTableLowpassType[] = {
"NORMAL", "HIGH"
"NORMAL", "HIGH", "DENOISE"
};
static const char * const lookupTableFailsafe[] = {

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

@ -48,7 +48,7 @@ static const gyroConfig_t *gyroConfig;
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT];
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
static float gyroFilterDenoise[XYZ_AXIS_COUNT][MAX_DENOISE_WINDOW_SIZE];
static denoisingState_t gyroDenoiseState[XYZ_AXIS_COUNT];
static uint8_t gyroSoftLpfType;
static uint16_t gyroSoftNotchHz_1, gyroSoftNotchHz_2;
static float gyroSoftNotchQ_1, gyroSoftNotchQ_2;
@ -195,7 +195,7 @@ void gyroUpdate(void)
else if (gyroSoftLpfType == FILTER_PT1)
gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt);
else
gyroADCf[axis] = denoisingFilterUpdate((float) gyroADC[axis], 3, gyroFilterDenoise[axis]);
gyroADCf[axis] = denoisingFilterUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]);
if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]);
@ -209,8 +209,7 @@ void gyroUpdate(void)
gyroADC[axis] = lrintf(gyroADCf[axis]);
}
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
gyroADCf[axis] = gyroADC[axis];
}
}
}