Merge branch 'master' into development

src/main/common/filter.c

@@ -237,15 +237,19 @@ int16_t firFilterInt16Get(const firFilter_t *filter, int index)
     return filter->buf[index];
 }
 
+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];
+    for (index = filter->targetCount-1; index > 0; index--) filter->state[index] = filter->state[index-1];
-    filter[0] = input;
+    filter->state[0] = input;
-    for (count = 0; count < count; index++) averageSum += filter[index];
+    for (int count = 0; count < filter->targetCount; index++) averageSum += filter->state[index];
 
-    return averageSum / count;
+    return averageSum / filter->targetCount;
 }
 

src/main/common/filter.h

@@ -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,
@@ -81,5 +86,6 @@ float firFilterInt16CalcPartialAverage(const firFilterInt16_t *filter, uint8_t c
 float firFilterInt16CalcAverage(const firFilterInt16_t *filter);
 int16_t firFilterInt16LastInput(const firFilterInt16_t *filter);
 int16_t firFilterInt16Get(const firFilter_t *filter, int index);
-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);
 

src/main/flight/pid.c

@@ -103,7 +103,7 @@ biquadFilter_t dtermFilterLpf[3];
 biquadFilter_t dtermFilterNotch[3];
 bool dtermNotchInitialised;
 bool dtermBiquadLpfInitialised;
-float dtermFilterDenoise[XYZ_AXIS_COUNT][MAX_DENOISE_WINDOW_SIZE];
+denoisingState_t dtermDenoisingState[3];
 bool dtermNotchInitialised, dtermLpfInitialised;
 
 void initFilters(const pidProfile_t *pidProfile) {
@@ -121,6 +121,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;
+        }
+    }
 }
 
 void pidSetController(pidControllerType_e type)

src/main/flight/pid_betaflight.c

@@ -60,7 +60,7 @@ extern pt1Filter_t yawFilter;
 extern biquadFilter_t dtermFilterLpf[3];
 extern biquadFilter_t dtermFilterNotch[3];
 extern bool dtermNotchInitialised;
-extern float dtermFilterDenoise[XYZ_AXIS_COUNT][MAX_DENOISE_WINDOW_SIZE];
+extern denoisingState_t dtermDenoisingState[3];
 
 void initFilters(const pidProfile_t *pidProfile);
 float getdT(void);
@@ -214,7 +214,7 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
                 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;

src/main/flight/pid_legacy.c

@@ -57,7 +57,7 @@ extern int32_t errorGyroI[3];
 extern pt1Filter_t deltaFilter[3];
 extern pt1Filter_t yawFilter;
 extern biquadFilter_t dtermFilterLpf[3];
-extern float dtermFilterDenoise[XYZ_AXIS_COUNT][MAX_DENOISE_WINDOW_SIZE];
+extern denoisingState_t dtermDenoisingState[3];
 
 void initFilters(const pidProfile_t *pidProfile);
 float getdT(void);
@@ -183,7 +183,7 @@ void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, c
                 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);
             }

src/main/io/serial_cli.c

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

src/main/sensors/gyro.c

@@ -49,7 +49,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;
@@ -196,7 +196,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]);
@@ -210,8 +210,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];
-        }
     }
 }