From f62ec043cfc4f91a237081d3ea4cb6805d4f792d Mon Sep 17 00:00:00 2001
From: rav-rav <github@raav.info>
Date: Wed, 4 May 2016 22:44:33 +0200
Subject: [PATCH] fix error in biquad coefficients calculation improve biquad
 precision and performance by using direct form 2 transposed instead of direct
 form 1 keep float results for luxfloat pid controller, instead of casting
 twice

---
 src/main/common/filter.c | 39 ++++++++++++++++-----------------------
 src/main/common/filter.h |  2 +-
 src/main/flight/pid.c    |  2 +-
 src/main/sensors/gyro.c  |  6 +++++-
 src/main/sensors/gyro.h  |  1 +
 5 files changed, 24 insertions(+), 26 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 7d26d2ca8..21fd2948f 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -28,6 +28,7 @@
 #define M_PI_FLOAT	3.14159265358979323846f
 
 #define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
+#define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
 
 // PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
 float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT) {
@@ -56,44 +57,36 @@ void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate)
     omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
     sn = sinf(omega);
     cs = cosf(omega);
-    alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
+    //this is wrong, should be hyperbolic sine
+    //alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
+    alpha = sn / (2 * BIQUAD_Q);
 
-    b0 = (1 - cs) /2;
+    b0 = (1 - cs) / 2;
     b1 = 1 - cs;
-    b2 = (1 - cs) /2;
+    b2 = (1 - cs) / 2;
     a0 = 1 + alpha;
     a1 = -2 * cs;
     a2 = 1 - alpha;
 
     /* precompute the coefficients */
-    newState->b0 = b0 /a0;
-    newState->b1 = b1 /a0;
-    newState->b2 = b2 /a0;
-    newState->a1 = a1 /a0;
-    newState->a2 = a2 /a0;
+    newState->b0 = b0 / a0;
+    newState->b1 = b1 / a0;
+    newState->b2 = b2 / a0;
+    newState->a1 = a1 / a0;
+    newState->a2 = a2 / a0;
 
     /* zero initial samples */
-    newState->x1 = newState->x2 = 0;
-    newState->y1 = newState->y2 = 0;
+    newState->d1 = newState->d2 = 1;
 }
 
 /* Computes a biquad_t filter on a sample */
-float applyBiQuadFilter(float sample, biquad_t *state)
+float applyBiQuadFilter(float sample, biquad_t *state) //direct form 2 transposed
 {
     float result;
 
-    /* compute result */
-    result = state->b0 * sample + state->b1 * state->x1 + state->b2 * state->x2 -
-        state->a1 * state->y1 - state->a2 * state->y2;
-
-    /* shift x1 to x2, sample to x1 */
-    state->x2 = state->x1;
-    state->x1 = sample;
-
-    /* shift y1 to y2, result to y1 */
-    state->y2 = state->y1;
-    state->y1 = result;
-
+    result = state->b0 * sample + state->d1;
+    state->d1 = state->b1 * sample - state->a1 * result + state->d2;
+    state->d2 = state->b2 * sample - state->a2 * result;
     return result;
 }
 
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 500995602..809063f4b 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -26,7 +26,7 @@ typedef struct filterStatePt1_s {
 /* 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;
+    float d1, d2;
 } biquad_t;
 
 float applyBiQuadFilter(float sample, biquad_t *state);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index cc3312820..091eda087 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -202,7 +202,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
             }
         }
 
-        gyroRate = gyroADC[axis] / 4.0f; // gyro output scaled to rewrite scale
+        gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled to rewrite scale
 
         // --------low-level gyro-based PID. ----------
         // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index de3412760..17981d588 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -38,6 +38,7 @@
 
 uint16_t calibratingG = 0;
 int32_t gyroADC[XYZ_AXIS_COUNT];
+float gyroADCf[XYZ_AXIS_COUNT];
 int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
 static gyroConfig_t *gyroConfig;
@@ -155,7 +156,10 @@ void gyroUpdate(void)
         if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
 
         if (gyroFilterStateIsSet) {
-            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]));
+            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++){
+                gyroADCf[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]);
+                gyroADC[axis] = lrintf(gyroADCf[axis]);
+            }
         }
     }
 
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 48e5e677e..05f78706f 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -33,6 +33,7 @@ extern gyro_t gyro;
 extern sensor_align_e gyroAlign;
 
 extern int32_t gyroADC[XYZ_AXIS_COUNT];
+extern float gyroADCf[XYZ_AXIS_COUNT];
 extern int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
 
 typedef struct gyroConfig_s {
-- 
2.11.4.GIT