Merge branch 'lowpass' of https://github.com/fusterjj/cleanflight into fusterjj-lowpass

Conflicts:
	src/main/flight/mixer.c

Makefile

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

docs/Mixer.md

@@ -45,14 +45,12 @@ You can also use the Command Line Interface (CLI) to set the mixer type:
 
 A low-pass filter can be enabled for the servos.  It may be useful for avoiding structural modes in the airframe, for example.  Currently it can only be configured via the CLI:
 
-1. Use `set servo_lowpass_freq_idx = nn` to select the cutoff frequency.  Valid values range from 0 to 99.
+1. Use `set servo_lowpass_freq = nnn` to select the cutoff frequency.  Valid values range from 10 to 400.  This is a fraction of the loop frequency in 1/1000ths. For example, `40` means `0.040`.
 2. Use `set servo_lowpass_enable = 1` to enable filtering.
 
-The actual cutoff frequency is determined by the value of the `looptime` variable and the selected index.  
+The cutoff frequency can be determined by the following formula:
-The formula is:
+`Frequency = 1000 * servo_lowpass_freq / looptime`
-`Frequency = 1000000 * (servo_lowpass_freq_idx + 1)*0.0025 / looptime )`
+
-
+For example, if `servo_lowpass_freq` is set to 40, and looptime is set to the default of 3500 us, the cutoff frequency will be 11.43 Hz.
-
-For example, if `servo_lowpass_freq_idx` is set to 40, and looptime is set to the default of 3500 us (0.0035 s), the cutoff frequency will be 29.3 Hz.
 
 

src/main/config/config.c

@@ -445,6 +445,8 @@ static void resetConf(void)
 
     currentProfile->mixerConfig.yaw_direction = 1;
     currentProfile->mixerConfig.tri_unarmed_servo = 1;
+    currentProfile->mixerConfig.servo_lowpass_freq = 400;
+    currentProfile->mixerConfig.servo_lowpass_enable = 0;
 
     // gimbal
     currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL;

src/main/flight/lowpass.c

@@ -0,0 +1,117 @@
+/*
+ * 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"
+
+//#define DEBUG_LOWPASS
+
+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);
+
+
+#if defined(UNIT_TEST) && defined(DEBUG_LOWPASS)
+    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);
+#if defined(UNIT_TEST) && defined(DEBUG_LOWPASS)
+        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

@@ -45,6 +45,7 @@
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/imu.h"
+#include "flight/lowpass.h"
 
 #include "config/runtime_config.h"
 #include "config/config.h"
@@ -54,10 +55,14 @@
 
 #define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4
 
+//#define MIXER_DEBUG
+
+extern int16_t debug[4];
+
 uint8_t motorCount = 0;
 int16_t motor[MAX_SUPPORTED_MOTORS];
 int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
-int16_t servo[MAX_SUPPORTED_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
+int16_t servo[MAX_SUPPORTED_SERVOS];
 
 static int useServo;
 
@@ -73,6 +78,7 @@ static gimbalConfig_t *gimbalConfig;
 
 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
 static mixerMode_e currentMixerMode;
+static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS];
 
 static const motorMixer_t mixerQuadX[] = {
     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
@@ -514,6 +520,11 @@ void mixTable(void)
     int16_t maxMotor;
     uint32_t i;
 
+    // paranoia: give all servos a default command
+    for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
+        servo[i] = DEFAULT_SERVO_MIDDLE;
+    }
+
     if (motorCount > 3) {
         // prevent "yaw jump" during yaw correction
         axisPID[YAW] = constrain(axisPID[YAW], -100 - ABS(rcCommand[YAW]), +100 + ABS(rcCommand[YAW]));
@@ -660,3 +671,25 @@ bool isMixerUsingServos(void)
 {
     return useServo;
 }
+
+void filterServos(void)
+{
+    int16_t servoIdx;
+
+#if defined(MIXER_DEBUG)
+    uint32_t startTime = micros();
+#endif
+
+    if (mixerConfig->servo_lowpass_enable) {
+        for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
+            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);
+        }
+    }
+#if defined(MIXER_DEBUG)
+    debug[0] = (int16_t)(micros() - startTime);
+#endif
+}
+

src/main/flight/mixer.h

@@ -66,6 +66,8 @@ typedef struct mixer_t {
 typedef struct mixerConfig_s {
     int8_t yaw_direction;
     uint8_t tri_unarmed_servo;              // send tail servo correction pulses even when unarmed
+    int16_t servo_lowpass_freq;             // lowpass servo filter frequency selection; 1/1000ths of loop freq
+    int8_t servo_lowpass_enable;            // enable/disable lowpass filter
 } mixerConfig_t;
 
 typedef struct flight3DConfig_s {
@@ -101,3 +103,4 @@ void mixerResetMotors(void);
 void mixTable(void);
 void writeServos(void);
 void writeMotors(void);
+void filterServos(void);

src/main/io/serial_cli.c

@@ -354,6 +354,8 @@ const clivalue_t valueTable[] = {
     { "yaw_control_direction",      VAR_INT8   | MASTER_VALUE,  &masterConfig.yaw_control_direction, -1, 1 },
     { "yaw_direction",              VAR_INT8   | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 },
     { "tri_unarmed_servo",          VAR_INT8   | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 },
+    { "servo_lowpass_freq",         VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_freq, 10, 400},
+    { "servo_lowpass_enable",       VAR_INT8   | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_enable, 0, 1 },
 
     { "default_rate_profile",       VAR_UINT8  | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 },
     { "rc_rate",                    VAR_UINT8  | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, 0, 250 },

src/main/mw.c

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

src/test/Makefile

@@ -52,7 +52,8 @@ TESTS = \
 	telemetry_hott_unittest \
 	rc_controls_unittest \
 	ledstrip_unittest \
-	ws2811_unittest
+	ws2811_unittest \
+	lowpass_unittest
 
 # All Google Test headers.  Usually you shouldn't change this
 # definition.
@@ -319,6 +320,31 @@ ws2811_unittest : \
 
 	$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
 
+
+$(OBJECT_DIR)/flight/lowpass.o : \
+	$(USER_DIR)/flight/lowpass.c \
+	$(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)/$@
+
+
 test: $(TESTS)
 	set -e && for test in $(TESTS) ; do \
 		$(OBJECT_DIR)/$$test; \

src/test/unit/lowpass_unittest.cc

@@ -0,0 +1,128 @@
+/*
+ * 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 <stdint.h>
+#include <limits.h>
+
+extern "C" {
+    #include "flight/lowpass.h" 
+}
+
+static lowpass_t lowpassFilterReference;
+static lowpass_t lowpassFilter;
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+static float lowpassRef(lowpass_t *filter, float in, int16_t freq)
+{
+    int16_t coefIdx;
+    float out;
+
+    // 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->xf[coefIdx] = in;
+            filter->yf[coefIdx] = in;
+        }
+        filter->init = true;
+    }
+
+    // Delays
+    for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) {
+        filter->xf[coefIdx] = filter->xf[coefIdx-1];
+        filter->yf[coefIdx] = filter->yf[coefIdx-1];
+    }
+    filter->xf[0] = in;
+
+    // Accumulate result
+    out = filter->xf[0] * filter->bf[0];
+    for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
+        out += filter->xf[coefIdx] * filter->bf[coefIdx];
+        out -= filter->yf[coefIdx] * filter->af[coefIdx];
+    }
+    filter->yf[0] = out;
+
+    return out;
+}
+
+TEST(LowpassTest, Lowpass)
+{
+    int16_t servoCmds[3000];
+    int16_t expectedOut[3000];
+    int16_t referenceOut;
+    int16_t filterOut;
+    uint16_t sampleIdx;
+    int16_t freq;
+
+    uint16_t sampleCount = sizeof(servoCmds) / sizeof(int16_t);
+
+    // generate inputs and expecteds
+    for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) {
+        if (sampleIdx < 1000) {
+            servoCmds[sampleIdx] = 500;
+        } else if (sampleIdx >= 1000 && sampleIdx < 2000) {
+            servoCmds[sampleIdx] = 2500;
+        } else {
+            servoCmds[sampleIdx] = 1500;
+        }
+
+        if ((sampleIdx >= 900 && sampleIdx < 1000) ||
+            (sampleIdx >= 1900 && sampleIdx < 2000)||
+            (sampleIdx >= 2900 && sampleIdx < 3000)) {
+            expectedOut[sampleIdx] = servoCmds[sampleIdx];
+        } else {
+            expectedOut[sampleIdx] = -1;
+        }
+    }
+
+    // Test all frequencies
+    for (freq = 10; freq <= 400; freq++) {
+        printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f));
+
+        // Run tests
+        for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) 
+        {
+            // Filter under test
+            filterOut = (int16_t)lowpassFixed(&lowpassFilter, servoCmds[sampleIdx], freq);
+
+            // Floating-point reference
+            referenceOut = (int16_t)(lowpassRef(&lowpassFilterReference, (float)servoCmds[sampleIdx], freq) + 0.5f);
+
+            if (expectedOut[sampleIdx] >= 0) {
+                EXPECT_EQ(filterOut, expectedOut[sampleIdx]);
+            }
+            // 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
+}
+
+// STUBS
+
+extern "C" {
+
+}
+
+