Merge branch 'filter_extend' of

https://github.com/borisbstyle/betaflight into borisbstyle-filter_extend Conflicts: src/main/flight/pid.c src/main/io/serial_cli.c src/main/mw.c
saperlot · Nov 14, 2015 · e7a87c4 · e7a87c4
2 parents df7d4be + ce2ea79
commit e7a87c4
Show file tree

Hide file tree

Showing 11 changed files with 154 additions and 66 deletions.
diff --git a/Makefile b/Makefile
@@ -222,6 +222,7 @@ COMMON_SRC = build_config.c \
 		   common/printf.c \
 		   common/typeconversion.c \
 		   common/encoding.c \
+		   common/filter.c \
 		   main.c \
 		   mw.c \
 		   flight/altitudehold.c \
@@ -230,7 +231,6 @@ COMMON_SRC = build_config.c \
 		   flight/imu.c \
 		   flight/mixer.c \
 		   flight/lowpass.c \
-		   flight/filter.c \
 		   drivers/bus_i2c_soft.c \
 		   drivers/serial.c \
 		   drivers/sound_beeper.c \

diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c
@@ -1,3 +1,20 @@
+/*
+ * 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 <stdlib.h>
 #include <stdarg.h>
 #include <string.h>

diff --git a/src/main/common/colorconversion.h b/src/main/common/colorconversion.h
@@ -1,3 +1,19 @@
+/*
+ * 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
 
 rgbColor24bpp_t* hsvToRgb24(const hsvColor_t *c);
diff --git a/src/main/common/encoding.c b/src/main/common/encoding.c
@@ -1,3 +1,20 @@
+/*
+ * 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 "encoding.h"
 
 /**

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
@@ -0,0 +1,37 @@
+/*
+ * 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 "common/filter.h"
+
+
+// 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 );
+	}
+
+    filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
+
+    return filter->state;
+}
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
@@ -0,0 +1,24 @@
+/*
+ * 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/>.
+ */
+
+typedef struct filterStatePt1_s {
+	float state;
+	float RC;
+	float constdT;
+} filterStatePt1_t;
+
+float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
diff --git a/src/main/config/config.c b/src/main/config/config.c
@@ -177,9 +177,9 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->D8[PIDVEL] = 1;
 
     pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
-    pidProfile->dterm_cut_hz = 0;
-    pidProfile->pterm_cut_hz = 0;
     pidProfile->gyro_cut_hz = 0;
+    pidProfile->pterm_cut_hz = 0;
+    pidProfile->dterm_cut_hz = 0;
 
     pidProfile->P_f[ROLL] = 1.4f;     // new PID with preliminary defaults test carefully
     pidProfile->I_f[ROLL] = 0.4f;

diff --git a/src/main/flight/filter.c b/src/main/flight/filter.c
diff --git a/src/main/flight/filter.h b/src/main/flight/filter.h
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -25,6 +25,7 @@
 
 #include "common/axis.h"
 #include "common/maths.h"
+#include "common/filter.h"
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
@@ -42,12 +43,12 @@
 #include "flight/imu.h"
 #include "flight/navigation.h"
 #include "flight/gtune.h"
-#include "flight/filter.h"
 
 #include "config/runtime_config.h"
 
 extern uint16_t cycleTime;
 extern uint8_t motorCount;
+extern float dT;
 
 int16_t heading;
 int16_t axisPID[3];
@@ -105,12 +106,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     static float lastError[3];
     static float delta1[3], delta2[3];
     float delta, deltaSum;
-    float dT;
     int axis;
     float horizonLevelStrength = 1;
 
-    dT = (float)cycleTime * 0.000001f;
-
     if (FLIGHT_MODE(HORIZON_MODE)) {
 
         // Figure out the raw stick positions
@@ -177,7 +175,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
 
         // Pterm low pass
         if (pidProfile->pterm_cut_hz) {
-            PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
+            PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
         }
 
         // -----calculate I component.
@@ -201,7 +199,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
 
         // Dterm low pass
         if (pidProfile->dterm_cut_hz) {
-            deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
+            deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
         }
 
         DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
@@ -282,7 +280,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
 
         // Pterm low pass
         if (pidProfile->pterm_cut_hz) {
-            PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
+            PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
         }
 
         delta = (gyroADC[axis] - lastGyro[axis]) / 4;   // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
@@ -293,7 +291,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
 
         // Dterm low pass
         if (pidProfile->dterm_cut_hz) {
-            DTerm = filterApplyPt1(DTerm, &DTermState[axis], pidProfile->dterm_cut_hz);
+            DTerm = filterApplyPt1(DTerm, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
         }
 
         DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5;   // 32 bits is needed for calculation
@@ -424,7 +422,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
 
         // Pterm low pass
         if (pidProfile->pterm_cut_hz) {
-            PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
+            PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT);
         }
 
         // -----calculate I component
@@ -453,7 +451,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
 
         // Dterm delta low pass
         if (pidProfile->dterm_cut_hz) {
-            deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
+            deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
         }
 
         DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;

diff --git a/src/main/mw.c b/src/main/mw.c
@@ -26,6 +26,7 @@
 #include "common/axis.h"
 #include "common/color.h"
 #include "common/utils.h"
+#include "common/filter.h"
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
@@ -73,8 +74,6 @@
 #include "flight/failsafe.h"
 #include "flight/gtune.h"
 #include "flight/navigation.h"
-#include "flight/filter.h"
-
 
 #include "config/runtime_config.h"
 #include "config/config.h"
@@ -97,6 +96,7 @@ enum {
 uint32_t currentTime = 0;
 uint32_t previousTime = 0;
 uint16_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
+float dT;
 
 int16_t magHold;
 int16_t headFreeModeHold;
@@ -141,7 +141,7 @@ void updateGtuneState(void)
         }
     } else {
         if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
-    	    DISABLE_FLIGHT_MODE(GTUNE_MODE);
+            DISABLE_FLIGHT_MODE(GTUNE_MODE);
         }
     }
 }
@@ -584,7 +584,7 @@ void processRx(void)
 
     if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) {
         // bumpless transfer to Level mode
-    	canUseHorizonMode = false;
+        canUseHorizonMode = false;
 
         if (!FLIGHT_MODE(ANGLE_MODE)) {
             pidResetErrorAngle();
@@ -675,14 +675,14 @@ void filterRc(void){
     uint16_t rxRefreshRate, filteredCycleTime;
 
     // Set RC refresh rate for sampling and channels to filter
-   	initRxRefreshRate(&rxRefreshRate);
+    initRxRefreshRate(&rxRefreshRate);
 
-    filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1);
+    filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
     rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
 
     if (isRXDataNew) {
         for (int channel=0; channel < 4; channel++) {
-        	deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
+            deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
             lastCommand[channel] = rcCommand[channel];
         }
 
@@ -702,6 +702,27 @@ void filterRc(void){
     }
 }
 
+// Gyro Low Pass
+void filterGyro(void) {
+    int axis;
+    static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT];
+
+    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        if (masterConfig.looptime > 0) {
+            // Static dT calculation based on configured looptime
+            if (!gyroADCState[axis].constdT) {
+                gyroADCState[axis].constdT = (float)masterConfig.looptime * 0.000001f;
+            }
+
+            gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz, gyroADCState[axis].constdT);
+        }
+
+        else {
+            gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz, dT);
+        }
+    }
+}
+
 void loop(void)
 {
     static uint32_t loopTime;
@@ -758,14 +779,10 @@ void loop(void)
         cycleTime = (int32_t)(currentTime - previousTime);
         previousTime = currentTime;
 
-        // Gyro Low Pass
-        if (currentProfile->pidProfile.gyro_cut_hz) {
-            int axis;
-            static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT];
+        dT = (float)cycleTime * 0.000001f;
 
-            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        	    gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz);
-            }
+        if (currentProfile->pidProfile.gyro_cut_hz) {
+            filterGyro();
         }
 
         annexCode();
@@ -780,7 +797,7 @@ void loop(void)
 
 #ifdef MAG
         if (sensors(SENSOR_MAG)) {
-        	updateMagHold();
+            updateMagHold();
         }
 #endif