From 64a78f82f87559050821fe83a6f879ff62280e40 Mon Sep 17 00:00:00 2001
From: Joel Fuster <j@fuster.org>
Date: Sat, 3 Jan 2015 12:25:46 -0500
Subject: [PATCH] Interim checkin

---
 src/main/flight/mixer.c | 24 +++++++++++++++++-------
 src/main/flight/mixer.h | 16 ++++++++++++----
 2 files changed, 29 insertions(+), 11 deletions(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 9ffd32f71a..95aeb7d3c6 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -70,6 +70,9 @@ static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
 static mixerMode_e currentMixerMode;
 static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS];
 
+static const float lowpassNormalizedButter5[] = 
+    { 3.2361f, 5.2361f, 5.2361f, 3.2361f, 1.0f };
+
 static const motorMixer_t mixerQuadX[] = {
     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
@@ -663,22 +666,29 @@ bool isMixerUsingServos(void)
     return useServo;
 }
 
-static float lowpass(lowpass_t *filter, float in, int16_t freqIdx )
+static void generate_lowpass_coeffs5(int16_t freq)
+{
+    const float lowpassNormalizedButter5[] = { 3.2361f, 5.2361f, 5.2361f, 3.2361f, 1.0f };
+
+
+
+}
+
+static float lowpass(lowpass_t *filter, float in, int16_t freq)
 {
     int16_t coefIdx;
     float out;
 
     // Check to see if cutoff frequency changed
-    if (freqIdx != filter->freqIdx) {
+    if (freq != filter->freq) {
         filter->init = false;
     }
 
     // Initialize if needed
     if (!filter->init) {
-        filter->freqIdx = freqIdx;
-        filter->freq = lowpass_table[filter->freqIdx][0];
-        filter->b = &lowpass_table[filter->freqIdx][1];
-        filter->a = &lowpass_table[filter->freqIdx][1 + LOWPASS_NUM_COEF];
+        generate_lowpass_coeffs5(freq, filter->b, filter->a);
+        //filter->b = &lowpass_table[filter->freqIdx][1];
+        //filter->a = &lowpass_table[filter->freqIdx][1 + LOWPASS_NUM_COEF];
         for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
             filter->x[coefIdx] = in;
             filter->y[coefIdx] = in;
@@ -711,7 +721,7 @@ void filterServos(void)
     if (mixerConfig->servo_lowpass_enable) {
         for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
             // Round to nearest
-            servo[servoIdx] = (int16_t)(lowpass(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx) + 0.5f);
+            servo[servoIdx] = (int16_t)(lowpass(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq) + 0.5f);
             // Sanity check
             servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
         }
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 8b4c64bc0d..16a1a0cc54 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -93,16 +93,24 @@ typedef struct servoParam_t {
 } servoParam_t;
 
 #define LOWPASS_NUM_COEF 6
+/*
+typedef struct lowpass_coeffs_t {
+    int16_t freq;                           // 1/1000ths of normalized frequency
+    float b[LOWPASS_NUM_COEF];
+    float a[LOWPASS_NUM_COEF];
+} lowpass_coeffs_t;
+*/
+
 typedef struct lowpass_t {
     bool init;
-    int16_t freqIdx;
-    float freq;
-    const float *b;
-    const float *a;
+    int16_t freq;                           // Normalized freq in 1/1000ths
+    float b[LOWPASS_NUM_COEF];
+    float a[LOWPASS_NUM_COEF];
     float x[LOWPASS_NUM_COEF];
     float y[LOWPASS_NUM_COEF];
 } lowpass_t;
 
+
 extern int16_t motor[MAX_SUPPORTED_MOTORS];
 extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 extern int16_t servo[MAX_SUPPORTED_SERVOS];