For discussion - use of noise-robust differentiators for DTerm in PIDs

src/main/common/filter.c

@@ -17,107 +17,172 @@
 
 #include <stdbool.h>
 #include <stdint.h>
-#include <stdlib.h>
+#include <string.h>
 #include <math.h>
 
 #include "common/axis.h"
 #include "common/filter.h"
 #include "common/maths.h"
 
-#define M_LN2_FLOAT	0.69314718055994530942f
-#define M_PI_FLOAT	3.14159265358979323846f
+#define M_LN2_FLOAT 0.69314718055994530942f
+#define M_PI_FLOAT  3.14159265358979323846f
 
 #define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
 
 // PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
-float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT) {
-
-	// Pre calculate and store RC
-	if (!filter->RC) {
-		filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
-	}
+float pt1FilterApply(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
+{
+    // Pre calculate and store RC
+    if (!filter->RC) {
+        filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
+    }
 
     filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
-
     return filter->state;
 }
 
 /* sets up a biquad Filter */
-void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate)
+void biQuadFilterInit(biquad_t *filter, float filterCutFreq, uint32_t refreshRate)
 {
-    float sampleRate;
+    // zero initial samples
+    filter->x1 = filter->x2 = 0;
+    filter->y1 = filter->y2 = 0;
+
+    const float sampleRate = 1 / ((float)refreshRate * 0.000001f);
+
+    // setup variables
+    const float omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
+    const float sn = sinf(omega);
+    const float cs = cosf(omega);
+    const float alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
+
+    const float b0 = (1 - cs) / 2;
+    const float b1 = 1 - cs;
+    const float b2 = (1 - cs) / 2;
+    const float a0 = 1 + alpha;
+    const float a1 = -2 * cs;
+    const float a2 = 1 - alpha;
+
+    // precompute the coefficients
+    filter->b0 = b0 / a0;
+    filter->b1 = b1 / a0;
+    filter->b2 = b2 / a0;
+    filter->a1 = a1 / a0;
+    filter->a2 = a2 / a0;
+}
 
-    sampleRate = 1 / ((float)refreshRate * 0.000001f);
+/* Computes a biquad_t filter on a sample */
+float biQuadFilterApply(biquad_t *filter, float input)
+{
+    // compute result
+    const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 -
+        filter->a1 * filter->y1 - filter->a2 * filter->y2;
 
-    float omega, sn, cs, alpha;
-    float a0, a1, a2, b0, b1, b2;
+    // shift x1 to x2, sample to x1
+    filter->x2 = filter->x1;
+    filter->x1 = input;
 
-    /* setup variables */
-    omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
-    sn = sinf(omega);
-    cs = cosf(omega);
-    alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
+    // shift y1 to y2, result to y1
+    filter->y2 = filter->y1;
+    filter->y1 = result;
 
-    b0 = (1 - cs) /2;
-    b1 = 1 - cs;
-    b2 = (1 - cs) /2;
-    a0 = 1 + alpha;
-    a1 = -2 * cs;
-    a2 = 1 - alpha;
+    return result;
+}
 
-    /* precompute the coefficients */
-    newState->b0 = b0 /a0;
-    newState->b1 = b1 /a0;
-    newState->b2 = b2 /a0;
-    newState->a1 = a1 /a0;
-    newState->a2 = a2 /a0;
+void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength)
+{
+    filter->buf = buf;
+    filter->bufLength = bufLength;
+    filter->coeffs = coeffs;
+    filter->coeffsLength = coeffsLength;
+    memset(filter->buf, 0, sizeof(float) * filter->bufLength);
+}
 
-    /* zero initial samples */
-    newState->x1 = newState->x2 = 0;
-    newState->y1 = newState->y2 = 0;
+void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs)
+{
+    firFilterInit2(filter, buf, bufLength, coeffs, bufLength);
 }
 
-/* Computes a biquad_t filter on a sample */
-float applyBiQuadFilter(float sample, biquad_t *state)
+void firFilterUpdate(firFilter_t *filter, float input)
+{
+    memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(float));
+    filter->buf[0] = input;
+}
+
+float firFilterApply(firFilter_t *filter)
+{
+    float ret = 0.0f;
+    for (int ii = 0; ii < filter->coeffsLength; ++ii) {
+        ret += filter->coeffs[ii] * filter->buf[ii];
+    }
+    return ret;
+}
+
+float firFilterCalcPartialAverage(firFilter_t *filter, uint8_t count)
 {
-    float result;
+    float ret = 0.0f;
+    for (int ii = 0; ii < count; ++ii) {
+        ret += filter->buf[ii];
+    }
+    return ret / count;
+}
 
-    /* compute result */
-    result = state->b0 * sample + state->b1 * state->x1 + state->b2 * state->x2 -
-        state->a1 * state->y1 - state->a2 * state->y2;
+float firFilterCalcAverage(firFilter_t *filter)
+{
+    return firFilterCalcPartialAverage(filter, filter->coeffsLength);
+}
 
-    /* shift x1 to x2, sample to x1 */
-    state->x2 = state->x1;
-    state->x1 = sample;
+float firFilterLastInput(firFilter_t *filter)
+{
+    return filter->buf[0];
+}
 
-    /* shift y1 to y2, result to y1 */
-    state->y2 = state->y1;
-    state->y1 = result;
+// integer based FIR filter
+// coefficients are multiples of 1/64
+void firFilterInt32Init2(firFilterInt32_t *filter, int32_t *buf, uint8_t bufLength, const int8_t *coeffs, uint8_t coeffsLength)
+{
+    filter->buf = buf;
+    filter->bufLength = bufLength;
+    filter->coeffs = coeffs;
+    filter->coeffsLength = coeffsLength;
+    memset(filter->buf, 0, sizeof(int32_t) * filter->bufLength);
+}
 
-    return result;
+void firFilterInt32Init(firFilterInt32_t *filter, int32_t *buf, uint8_t bufLength, const int8_t *coeffs)
+{
+    firFilterInt32Init2(filter, buf, bufLength, coeffs, bufLength);
 }
 
-int32_t filterApplyAverage(int32_t input, uint8_t count, int32_t averageState[])
+void firFilterInt32Update(firFilterInt32_t *filter, float input)
 {
-    int32_t sum = 0;
-    for (int ii = count - 1; ii > 0; --ii) {
-        averageState[ii] = averageState[ii-1];
-        sum += averageState[ii];
+    memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(int32_t));
+    filter->buf[0] = input;
+}
+
+int32_t firFilterInt32Apply(firFilterInt32_t *filter)
+{
+    int32_t ret = 0;
+    for (int ii = 0; ii < filter->coeffsLength; ++ii) {
+        ret += filter->coeffs[ii] * filter->buf[ii];
     }
-    averageState[0] = input;
-    sum += input;
-    return sum / count;
+    return ret / 64;
 }
 
-float filterApplyAveragef(float input, uint8_t count, float averageState[])
+int32_t firFilterInt32CalcPartialAverage(firFilterInt32_t *filter, uint8_t count)
 {
-    float sum = 0;
-    for (int ii = count - 1; ii > 0; --ii) {
-        averageState[ii] = averageState[ii-1];
-        sum += averageState[ii];
+    int32_t ret = 0;
+    for (int ii = 0; ii < count; ++ii) {
+        ret += filter->buf[ii];
     }
-    averageState[0] = input;
-    sum += input;
-    return sum / count;
+    return ret / count;
 }
 
+int32_t firFilterInt32CalcAverage(firFilterInt32_t *filter)
+{
+    return firFilterInt32CalcPartialAverage(filter, filter->coeffsLength);
+}
+
+int32_t firFilterInt32LastInput(firFilterInt32_t *filter)
+{
+    return filter->buf[0];
+}

src/main/common/filter.h

@@ -15,20 +15,54 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-typedef struct filterStatePt1_s {
-	float state;
-	float RC;
-	float constdT;
-} filterStatePt1_t;
+#pragma once
 
-/* this holds the data required to update samples thru a filter */
+typedef struct pt1Filter_s {
+    float state;
+    float RC;
+    float constdT;
+} pt1Filter_t;
+
+// this holds the data required to update samples thru a filter
 typedef struct biquad_s {
     float b0, b1, b2, a1, a2;
     float x1, x2, y1, y2;
 } biquad_t;
 
-float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
-float applyBiQuadFilter(float sample, biquad_t *state);
-void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate);
-int32_t filterApplyAverage(int32_t input, uint8_t count, int32_t averageState[]);
-float filterApplyAveragef(float input, uint8_t count, float averageState[]);
+typedef struct firFilter_s {
+    float *buf;
+    const float *coeffs;
+    uint8_t bufLength;
+    uint8_t coeffsLength;
+} firFilter_t;
+
+typedef struct firFilterInt32_s {
+    int32_t *buf;
+    const int8_t *coeffs;
+    uint8_t bufLength;
+    uint8_t coeffsLength;
+} firFilterInt32_t;
+
+
+void biQuadFilterInit(biquad_t *filter, float filterCutFreq, uint32_t refreshRate);
+float biQuadFilterApply(biquad_t *filter, float input);
+
+
+float pt1FilterApply(pt1Filter_t *filter, float input, uint8_t f_cut, float dT);
+
+
+void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
+void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
+void firFilterUpdate(firFilter_t *filter, float input);
+float firFilterApply(firFilter_t *filter);
+float firFilterCalcPartialAverage(firFilter_t *filter, uint8_t count);
+float firFilterCalcAverage(firFilter_t *filter);
+float firFilterLastInput(firFilter_t *filter);
+
+void firFilterInt32Init(firFilterInt32_t *filter, int32_t *buf, uint8_t bufLength, const int8_t *coeffs);
+void firFilterInt32Init2(firFilterInt32_t *filter, int32_t *buf, uint8_t bufLength, const int8_t *coeffs, uint8_t coeffsLength);
+void firFilterInt32Update(firFilterInt32_t *filter, float input);
+int32_t firFilterInt32Apply(firFilterInt32_t *filter);
+int32_t firFilterInt32CalcPartialAverage(firFilterInt32_t *filter, uint8_t count);
+int32_t firFilterInt32CalcAverage(firFilterInt32_t *filter);
+int32_t firFilterInt32LastInput(firFilterInt32_t *filter);

src/main/config/config_unittest.h

@@ -46,26 +46,14 @@ bool unittest_outsideRealtimeGuardInterval;
 #ifdef SRC_MAIN_FLIGHT_PID_LUXFLOAT_C_
 #ifdef UNIT_TEST
 
-float unittest_pidLuxFloatCore_lastRateForDelta[3];
-float unittest_pidLuxFloatCore_deltaState[3][DTERM_AVERAGE_COUNT];
 float unittest_pidLuxFloatCore_PTerm[3];
 float unittest_pidLuxFloatCore_ITerm[3];
 float unittest_pidLuxFloatCore_DTerm[3];
 
-#define SET_PID_LUX_FLOAT_CORE_LOCALS(axis) \
-    { \
-        lastRateForDelta[axis] = unittest_pidLuxFloatCore_lastRateForDelta[axis]; \
-        for (int ii = 0; ii < DTERM_AVERAGE_COUNT; ++ii) { \
-            deltaState[axis][ii] = unittest_pidLuxFloatCore_deltaState[axis][ii]; \
-        } \
-    }
+#define SET_PID_LUX_FLOAT_CORE_LOCALS(axis) {}
 
 #define GET_PID_LUX_FLOAT_CORE_LOCALS(axis) \
     { \
-        unittest_pidLuxFloatCore_lastRateForDelta[axis] = lastRateForDelta[axis]; \
-        for (int ii = 0; ii < DTERM_AVERAGE_COUNT; ++ii) { \
-            unittest_pidLuxFloatCore_deltaState[axis][ii] = deltaState[axis][ii]; \
-        } \
         unittest_pidLuxFloatCore_PTerm[axis] = PTerm; \
         unittest_pidLuxFloatCore_ITerm[axis] = ITerm; \
         unittest_pidLuxFloatCore_DTerm[axis] = DTerm; \
@@ -83,26 +71,14 @@ float unittest_pidLuxFloatCore_DTerm[3];
 #ifdef SRC_MAIN_FLIGHT_PID_MWREWRITE_C_
 #ifdef UNIT_TEST
 
-int32_t unittest_pidMultiWiiRewriteCore_lastRateForDelta[3];
-int32_t unittest_pidMultiWiiRewriteCore_deltaState[3][DTERM_AVERAGE_COUNT];
 int32_t unittest_pidMultiWiiRewriteCore_PTerm[3];
 int32_t unittest_pidMultiWiiRewriteCore_ITerm[3];
 int32_t unittest_pidMultiWiiRewriteCore_DTerm[3];
 
-#define SET_PID_MULTI_WII_REWRITE_CORE_LOCALS(axis) \
-    { \
-        lastRateForDelta[axis] = unittest_pidMultiWiiRewriteCore_lastRateForDelta[axis]; \
-        for (int ii = 0; ii < DTERM_AVERAGE_COUNT; ++ii) { \
-            deltaState[axis][ii] = unittest_pidMultiWiiRewriteCore_deltaState[axis][ii]; \
-        } \
-    }
+#define SET_PID_MULTI_WII_REWRITE_CORE_LOCALS(axis) {}
 
 #define GET_PID_MULTI_WII_REWRITE_CORE_LOCALS(axis) \
     { \
-        unittest_pidMultiWiiRewriteCore_lastRateForDelta[axis] = lastRateForDelta[axis]; \
-        for (int ii = 0; ii < DTERM_AVERAGE_COUNT; ++ii) { \
-            unittest_pidMultiWiiRewriteCore_deltaState[axis][ii] = deltaState[axis][ii]; \
-        } \
         unittest_pidMultiWiiRewriteCore_PTerm[axis] = PTerm; \
         unittest_pidMultiWiiRewriteCore_ITerm[axis] = ITerm; \
         unittest_pidMultiWiiRewriteCore_DTerm[axis] = DTerm; \

src/main/flight/imu.c

@@ -406,7 +406,7 @@ static bool isMagnetometerHealthy(void)
 
 static void imuCalculateEstimatedAttitude(void)
 {
-    static filterStatePt1_t accLPFState[3];
+    static pt1Filter_t accLPFState[3];
     static uint32_t previousIMUUpdateTime;
     float rawYawError = 0;
     int32_t axis;
@@ -421,7 +421,7 @@ static void imuCalculateEstimatedAttitude(void)
     // Smooth and use only valid accelerometer readings
     for (axis = 0; axis < 3; axis++) {
         if (imuRuntimeConfig->acc_cut_hz > 0) {
-            accSmooth[axis] = filterApplyPt1(accADC[axis], &accLPFState[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6f);
+            accSmooth[axis] = pt1FilterApply(&accLPFState[axis], accADC[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6f);
         } else {
             accSmooth[axis] = accADC[axis];
         }