Merge pull request #2872 from cleanflight/fix-servo-filtering

CF/BF - Fix servo filter initialisation.

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();

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);

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);