Split filter into separate file for future reuse

Makefile

@@ -206,6 +206,7 @@ COMMON_SRC	 = build_config.c \
 		   flight/flight.c \
 		   flight/imu.c \
 		   flight/mixer.c \
+		   flight/lowpass.c \
 		   drivers/bus_i2c_soft.c \
 		   drivers/serial.c \
 		   drivers/sound_beeper.c \

src/main/flight/lowpass.c

@@ -0,0 +1,115 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include "flight/lowpass.h"
+
+void generateLowpassCoeffs2(int16_t freq, lowpass_t *filter)
+{
+    float fixedScaler;
+    int i;
+
+    // generates coefficients for a 2nd-order butterworth low-pass filter
+    float freqf = (float)freq*0.001f;
+    float omega = tanf((float)M_PI*freqf/2.0f);
+    float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f);
+
+
+#ifdef UNIT_TEST
+    printf("lowpass cutoff: %f, omega: %f\n", freqf, omega);
+#endif
+   
+    filter->bf[0] = scaling * omega*omega;
+    filter->bf[1] = 2.0f * filter->bf[0];
+    filter->bf[2] = filter->bf[0];
+
+    filter->af[0] = 1.0f;
+    filter->af[1] = scaling * (2.0f * omega*omega - 2.0f);
+    filter->af[2] = scaling * (omega*omega - 1.4142136f * omega + 1.0f);
+
+    // Scale for fixed-point
+    filter->input_bias = 1500; // Typical servo range is 1500 +/- 500
+    filter->input_shift = 16;
+    filter->coeff_shift = 24; 
+    fixedScaler = (float)(1ULL << filter->coeff_shift);
+    for (i = 0; i < LOWPASS_NUM_COEF; i++) {
+        filter->a[i] = LPF_ROUND(filter->af[i] * fixedScaler);
+        filter->b[i] = LPF_ROUND(filter->bf[i] * fixedScaler);
+#ifdef UNIT_TEST
+        printf("(%d) bf: %f af: %f b: %ld a: %ld\n", i, 
+                filter->bf[i], filter->af[i], filter->b[i], filter->a[i]);
+#endif
+    }
+
+    filter->freq = freq;
+}
+
+int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq)
+{
+    int16_t coefIdx;
+    int64_t out;
+    int32_t in_s;
+  
+    // Check to see if cutoff frequency changed
+    if (freq != filter->freq) {
+        filter->init = false;
+    }
+
+    // Initialize if needed
+    if (!filter->init) {
+        generateLowpassCoeffs2(freq, filter);
+        for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
+            filter->x[coefIdx] = (in - filter->input_bias) << filter->input_shift;
+            filter->y[coefIdx] = (in - filter->input_bias) << filter->input_shift;
+        }
+        filter->init = true;
+    }
+
+    // Unbias input and scale
+    in_s = (in - filter->input_bias) << filter->input_shift;
+
+    // Delays
+    for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) {
+        filter->x[coefIdx] = filter->x[coefIdx-1];
+        filter->y[coefIdx] = filter->y[coefIdx-1];
+    }
+    filter->x[0] = in_s;
+
+    // Accumulate result
+    out = filter->x[0] * filter->b[0];
+    for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
+        out -= filter->y[coefIdx] * filter->a[coefIdx];
+        out += filter->x[coefIdx] * filter->b[coefIdx];
+    }
+
+    // Scale output by coefficient shift
+    out >>= filter->coeff_shift;
+    filter->y[0] = (int32_t)out;
+
+    // Scale output by input shift and round
+    out = (out + (1 << (filter->input_shift-1))) >> filter->input_shift;
+    
+    // Reapply bias
+    out += filter->input_bias;
+
+    return (int32_t)out;
+}
+

src/main/flight/lowpass.h

@@ -0,0 +1,41 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+
+#define LOWPASS_NUM_COEF 3
+#define LPF_ROUND(x) (x < 0 ? (x - 0.5f) : (x + 0.5f))
+
+typedef struct lowpass_t {
+    bool init;
+    int16_t freq;                           // Normalized freq in 1/1000ths
+    float bf[LOWPASS_NUM_COEF];
+    float af[LOWPASS_NUM_COEF];
+    int64_t b[LOWPASS_NUM_COEF];
+    int64_t a[LOWPASS_NUM_COEF];
+    int16_t coeff_shift;
+    int16_t input_shift;
+    int32_t input_bias;
+    float xf[LOWPASS_NUM_COEF];
+    float yf[LOWPASS_NUM_COEF];
+    int32_t x[LOWPASS_NUM_COEF];
+    int32_t y[LOWPASS_NUM_COEF];
+} lowpass_t;
+
+void generateLowpassCoeffs2(int16_t freq, lowpass_t *filter);
+int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq);

src/main/flight/mixer.c

@@ -18,11 +18,6 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdlib.h>
-#include <math.h>
-
-#ifdef UNIT_TEST
-#include <stdio.h>
-#endif
 
 #include "platform.h"
 
@@ -30,12 +25,11 @@
 
 #include "common/axis.h"
 #include "common/maths.h"
-#ifndef UNIT_TEST
+
 #include "drivers/gpio.h"
 #include "drivers/timer.h"
 #include "drivers/pwm_output.h"
 #include "drivers/pwm_mapping.h"
-#endif
 
 #include "rx/rx.h"
 #include "io/gimbal.h"
@@ -44,6 +38,7 @@
 
 #include "flight/mixer.h"
 #include "flight/flight.h"
+#include "flight/lowpass.h"
 
 #include "config/runtime_config.h"
 #include "config/config.h"
@@ -55,19 +50,16 @@
 
 //#define MIXER_DEBUG
 
-#include "drivers/system.h"
 extern int16_t debug[4];
 
-#ifndef UNIT_TEST
 uint8_t motorCount = 0;
-static int useServo;
-static uint8_t servoCount;
-#endif
-
 int16_t motor[MAX_SUPPORTED_MOTORS];
 int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 int16_t servo[MAX_SUPPORTED_SERVOS];
 
+static int useServo;
+
+static uint8_t servoCount;
 
 static servoParam_t *servoConf;
 static mixerConfig_t *mixerConfig;
@@ -77,10 +69,8 @@ static airplaneConfig_t *airplaneConfig;
 static rxConfig_t *rxConfig;
 static gimbalConfig_t *gimbalConfig;
 
-#ifndef UNIT_TEST
 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
 static mixerMode_e currentMixerMode;
-#endif
 static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS];
 
 static const motorMixer_t mixerQuadX[] = {
@@ -244,7 +234,6 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon
     gimbalConfig = gimbalConfigToUse;
 }
 
-#ifndef UNIT_TEST
 int16_t determineServoMiddleOrForwardFromChannel(int nr)
 {
     uint8_t channelToForwardFrom = servoConf[nr].forwardFromChannel;
@@ -524,7 +513,7 @@ void mixTable(void)
     int16_t maxMotor;
     uint32_t i;
 
-    // paranoia: give all servos a default command; prevents drift on unused servos with lowpass enabled
+    // paranoia: give all servos a default command
     for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
         servo[i] = DEFAULT_SERVO_MIDDLE;
     }
@@ -676,99 +665,6 @@ bool isMixerUsingServos(void)
     return useServo;
 }
 
-#endif
-
-void generate_lowpass_coeffs2(int16_t freq, lowpass_t *filter)
-{
-    float fixedScaler;
-    int i;
-
-    // generates coefficients for a 2nd-order butterworth low-pass filter
-    float freqf = (float)freq*0.001f;
-    float omega = tanf(M_PI*freqf/2.0f);
-    float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f);
-
-
-#ifdef UNIT_TEST
-    printf("lowpass cutoff: %f, omega: %f\n", freqf, omega);
-#endif
-   
-    filter->bf[0] = scaling * omega*omega;
-    filter->bf[1] = 2.0f * filter->bf[0];
-    filter->bf[2] = filter->bf[0];
-
-    filter->af[0] = 1.0f;
-    filter->af[1] = scaling * (2.0f * omega*omega - 2.0f);
-    filter->af[2] = scaling * (omega*omega - 1.4142136f * omega + 1.0f);
-
-    // Scale for fixed-point
-    filter->input_bias = 1500; // Typical servo range is 1500 +/- 500
-    filter->input_shift = 16;
-    filter->coeff_shift = 24; 
-    fixedScaler = (float)(1ULL << filter->coeff_shift);
-    for (i = 0; i < LOWPASS_NUM_COEF; i++) {
-        filter->a[i] = LPF_ROUND(filter->af[i] * fixedScaler);
-        filter->b[i] = LPF_ROUND(filter->bf[i] * fixedScaler);
-#ifdef UNIT_TEST
-        printf("(%d) bf: %f af: %f b: %ld a: %ld\n", i, 
-                filter->bf[i], filter->af[i], filter->b[i], filter->a[i]);
-#endif
-    }
-
-    filter->freq = freq;
-}
-
-static int32_t lowpass_fixed(lowpass_t *filter, int32_t in, int16_t freq)
-{
-    int16_t coefIdx;
-    int64_t out;
-    int32_t in_s;
-  
-    // Check to see if cutoff frequency changed
-    if (freq != filter->freq) {
-        filter->init = false;
-    }
-
-    // Initialize if needed
-    if (!filter->init) {
-        generate_lowpass_coeffs2(freq, filter);
-        for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
-            filter->x[coefIdx] = (in - filter->input_bias) << filter->input_shift;
-            filter->y[coefIdx] = (in - filter->input_bias) << filter->input_shift;
-        }
-        filter->init = true;
-    }
-
-    // Unbias input and scale
-    in_s = (in - filter->input_bias) << filter->input_shift;
-
-    // Delays
-    for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) {
-        filter->x[coefIdx] = filter->x[coefIdx-1];
-        filter->y[coefIdx] = filter->y[coefIdx-1];
-    }
-    filter->x[0] = in_s;
-
-    // Accumulate result
-    out = filter->x[0] * filter->b[0];
-    for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
-        out -= filter->y[coefIdx] * filter->a[coefIdx];
-        out += filter->x[coefIdx] * filter->b[coefIdx];
-    }
-
-    // Scale output by coefficient shift
-    out >>= filter->coeff_shift;
-    filter->y[0] = (int32_t)out;
-
-    // Scale output by input shift and round
-    out = (out + (1 << (filter->input_shift-1))) >> filter->input_shift;
-    
-    // Reapply bias
-    out += filter->input_bias;
-
-    return (int32_t)out;
-}
-
 void filterServos(void)
 {
     int16_t servoIdx;
@@ -779,7 +675,7 @@ void filterServos(void)
 
     if (mixerConfig->servo_lowpass_enable) {
         for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
-            servo[servoIdx] = (int16_t)lowpass_fixed(&lowpassFilters[servoIdx], servo[servoIdx], mixerConfig->servo_lowpass_freq);
+            servo[servoIdx] = (int16_t)lowpassFixed(&lowpassFilters[servoIdx], servo[servoIdx], mixerConfig->servo_lowpass_freq);
 
             // Sanity check
             servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);

src/main/flight/mixer.h

@@ -92,26 +92,6 @@ typedef struct servoParam_t {
     int8_t forwardFromChannel;              // RX channel index, 0 based.  See CHANNEL_FORWARDING_DISABLED
 } servoParam_t;
 
-#define LOWPASS_NUM_COEF 3
-#define LPF_ROUND(x) (x < 0 ? (x - 0.5f) : (x + 0.5f))
-
-typedef struct lowpass_t {
-    bool init;
-    int16_t freq;                           // Normalized freq in 1/1000ths
-    float bf[LOWPASS_NUM_COEF];
-    float af[LOWPASS_NUM_COEF];
-    int64_t b[LOWPASS_NUM_COEF];
-    int64_t a[LOWPASS_NUM_COEF];
-    int16_t coeff_shift;
-    int16_t input_shift;
-    int32_t input_bias;
-    float xf[LOWPASS_NUM_COEF];
-    float yf[LOWPASS_NUM_COEF];
-    int32_t x[LOWPASS_NUM_COEF];
-    int32_t 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];

src/main/mw.c

@@ -715,6 +715,7 @@ void loop(void)
         );
 
         mixTable();
+        filterServos();
         writeServos();
         writeMotors();
 

src/test/Makefile

@@ -53,7 +53,7 @@ TESTS = \
 	rc_controls_unittest \
 	ledstrip_unittest \
 	ws2811_unittest \
-	mixer_unittest
+	lowpass_unittest
 
 # All Google Test headers.  Usually you shouldn't change this
 # definition.
@@ -320,16 +320,30 @@ ws2811_unittest : \
 
 	$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
 
-$(OBJECT_DIR)/flight/mixer.o : $(USER_DIR)/flight/mixer.c $(USER_DIR)/flight/mixer.h $(GTEST_HEADERS)
-	@mkdir -p $(dir $@)
-	$(CC) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/mixer.c -o $@
 
-$(OBJECT_DIR)/mixer_unittest.o : $(TEST_DIR)/mixer_unittest.cc $(USER_DIR)/flight/mixer.h $(GTEST_HEADERS)
+$(OBJECT_DIR)/flight/lowpass.o : \
-	@mkdir -p $(dir $@)
+	$(USER_DIR)/flight/lowpass.c \
-	$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/mixer_unittest.cc -o $@
+	$(USER_DIR)/flight/lowpass.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/lowpass.c -o $@
+
+$(OBJECT_DIR)/lowpass_unittest.o : \
+	$(TEST_DIR)/lowpass_unittest.cc \
+	$(USER_DIR)/flight/lowpass.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/lowpass_unittest.cc -o $@
+
+lowpass_unittest : \
+	$(OBJECT_DIR)/flight/lowpass.o \
+	$(OBJECT_DIR)/lowpass_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
 
-mixer_unittest : $(OBJECT_DIR)/flight/mixer.o $(OBJECT_DIR)/flight/mixer.o $(OBJECT_DIR)/mixer_unittest.o $(OBJECT_DIR)/gtest_main.a
-	$(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
 
 test: $(TESTS)
 	set -e && for test in $(TESTS) ; do \

src/test/unit/lowpass_unittest.cc

@@ -15,29 +15,19 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 #include <stdint.h>
-
 #include <limits.h>
 
 extern "C" {
-    #include "flight/mixer.h" 
+    #include "flight/lowpass.h" 
-    #include "rx/rx.h"
-    #include "io/gimbal.h"
-    #include "io/escservo.h"
-    extern void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfigToUse, gimbalConfig_t *gimbalConfigToUse);
-    extern void generate_lowpass_coeffs2(int16_t freq, lowpass_t *filter);
 }
 
-uint32_t debug[4];
+static lowpass_t lowpassFilterReference;
-static int16_t servoRef[MAX_SUPPORTED_SERVOS];
+static lowpass_t lowpassFilter;
-static int16_t referenceOut[MAX_SUPPORTED_SERVOS];
-static uint16_t freq;
-static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS];
-static servoParam_t servoConfig[MAX_SUPPORTED_SERVOS];
 
 #include "unittest_macros.h"
 #include "gtest/gtest.h"
 
-static float lowpass_ref(lowpass_t *filter, float in, int16_t freq)
+static float lowpassRef(lowpass_t *filter, float in, int16_t freq)
 {
     int16_t coefIdx;
     float out;
@@ -49,7 +39,7 @@ static float lowpass_ref(lowpass_t *filter, float in, int16_t freq)
 
     // Initialize if needed
     if (!filter->init) {
-        generate_lowpass_coeffs2(freq, filter);
+        generateLowpassCoeffs2(freq, filter);
         for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
             filter->xf[coefIdx] = in;
             filter->yf[coefIdx] = in;
@@ -75,24 +65,14 @@ static float lowpass_ref(lowpass_t *filter, float in, int16_t freq)
     return out;
 }
 
-static void filterServosReference(void)
+TEST(LowpassTest, Lowpass)
-{
-    int16_t servoIdx;
-
-    for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
-        // Round to nearest
-        referenceOut[servoIdx] = (int16_t)(lowpass_ref(&lowpassFilters[servoIdx], (float)servoRef[servoIdx], freq) + 0.5f);
-    }
-}
-
-
-TEST(MixerTest, ServoLowpassFilter)
 {
     int16_t servoCmds[3000];
     int16_t expectedOut[3000];
-    uint8_t servoIdx;
+    int16_t referenceOut;
+    int16_t filterOut;
     uint16_t sampleIdx;
-    static mixerConfig_t mixerConfig;
+    int16_t freq;
 
     uint16_t sampleCount = sizeof(servoCmds) / sizeof(int16_t);
 
@@ -115,38 +95,26 @@ TEST(MixerTest, ServoLowpassFilter)
         }
     }
 
-    // Set mixer configuration
-    for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
-        servoConfig[servoIdx].min = 0;
-        servoConfig[servoIdx].max = 3000;
-    }
-
     // Test all frequencies
-    for (freq = 10; freq <= 400; freq++)
+    for (freq = 10; freq <= 400; freq++) {
-    {
         printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f));
 
-        mixerConfig.servo_lowpass_enable = 1;
-        mixerConfig.servo_lowpass_freq = freq;
-        mixerUseConfigs(servoConfig, NULL, NULL, &mixerConfig, NULL, NULL, NULL);
-
         // Run tests
-        for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) {
+        for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) 
-            for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
+        {
-                servo[servoIdx] = servoCmds[sampleIdx];
+            // Filter under test
-                servoRef[servoIdx] = servoCmds[sampleIdx];
+            filterOut = (int16_t)lowpassFixed(&lowpassFilter, servoCmds[sampleIdx], freq);
-            }
 
-            filterServos();
+            // Floating-point reference
-            filterServosReference();
+            referenceOut = (int16_t)(lowpassRef(&lowpassFilterReference, (float)servoCmds[sampleIdx], freq) + 0.5f);
 
-            for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
+            if (expectedOut[sampleIdx] >= 0) {
-                if (expectedOut[sampleIdx] >= 0) {
+                EXPECT_EQ(filterOut, expectedOut[sampleIdx]);
-                    EXPECT_EQ(servo[servoIdx], expectedOut[sampleIdx]);
-                }
-                EXPECT_LE(servo[servoIdx], referenceOut[servoIdx] + 1);
-                EXPECT_GE(servo[servoIdx], referenceOut[servoIdx] - 1);
             }
+            // Some tolerance
+            // TODO adjust precision to remove the need for this?
+            EXPECT_LE(filterOut, referenceOut + 1);
+            EXPECT_GE(filterOut, referenceOut - 1);
         } // for each sample
     } // for each freq
 }
@@ -155,21 +123,6 @@ TEST(MixerTest, ServoLowpassFilter)
 
 extern "C" {
 
-void delay(uint32_t ms)
-{
-    UNUSED(ms);
-    return;
-}
-
-int constrain(int amt, int low, int high)
-{
-    return (amt > high ? high : (amt < low ? low : amt));
-}
-
-uint32_t micros()
-{
-    return 0;
-}
 }