From 545e30e3c43c8662b6305094785d54dc871a8e3c Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Tue, 27 Jun 2017 23:12:38 +0100
Subject: [PATCH] Merge pull request #2872 from cleanflight/fix-servo-filtering

CF/BF - Fix servo filter initialisation.
---
 src/main/fc/fc_init.c    |  4 ++++
 src/main/flight/servos.c | 22 ++++++++++++----------
 src/main/flight/servos.h |  1 +
 3 files changed, 17 insertions(+), 10 deletions(-)

diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 546322f28..0e4d2f30a 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -502,6 +502,10 @@ void init(void)
     // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidInit()
     pidInit(currentPidProfile);
 
+#ifdef USE_SERVOS
+    servosFilterInit();
+#endif
+
     imuInit();
 
     mspFcInit();
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index 72b08821e..907a722e1 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -499,23 +499,25 @@ bool isMixerUsingServos(void)
     return useServo;
 }
 
+static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
+
+void servosFilterInit(void)
+{
+    if (servoConfig()->servo_lowpass_freq) {
+        for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
+            biquadFilterInitLPF(&servoFilter[servoIdx], servoConfig()->servo_lowpass_freq, targetPidLooptime);
+        }
+    }
+
+}
 static void filterServos(void)
 {
-    static int16_t servoIdx;
-    static bool servoFilterIsSet;
-    static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
-
 #if defined(MIXER_DEBUG)
     uint32_t startTime = micros();
 #endif
 
     if (servoConfig()->servo_lowpass_freq) {
-        for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
-            if (!servoFilterIsSet) {
-                biquadFilterInitLPF(&servoFilter[servoIdx], servoConfig()->servo_lowpass_freq, targetPidLooptime);
-                servoFilterIsSet = true;
-            }
-
+        for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
             servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
             // Sanity check
             servo[servoIdx] = constrain(servo[servoIdx], servoParams(servoIdx)->min, servoParams(servoIdx)->max);
diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h
index fe7a4db7d..c2cc8ed37 100644
--- a/src/main/flight/servos.h
+++ b/src/main/flight/servos.h
@@ -132,3 +132,4 @@ void loadCustomServoMixer(void);
 int servoDirection(int servoIndex, int fromChannel);
 void servoConfigureOutput(void);
 void servosInit(void);
+void servosFilterInit(void);