Reset the biquad filter used for servos to 1500 on startup

Filter was initialized at zero, which delayed the servo centering
by ~2 seconds. The filter is now reset to 1500 during initialization,
so the servos receive 1500 from the start.

Coupled with #3540, this completely eliminates all servo twitching
during boot.

Thanks to @shellixyz for testing!

src/main/common/filter.c

@@ -205,6 +205,13 @@ float biquadFilterApply(biquadFilter_t *filter, float input)
     return result;
 }
 
+float biquadFilterReset(biquadFilter_t *filter, float value)
+{
+    filter->d1 = value - (value * filter->b0);
+    filter->d2 = (filter->b2 - filter->a2) * value;
+    return value;
+}
+
 /*
  * FIR filter
  */

src/main/common/filter.h

@@ -73,6 +73,7 @@ void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs,
 void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs);
 void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType);
 float biquadFilterApply(biquadFilter_t *filter, float sample);
+float biquadFilterReset(biquadFilter_t *filter, float value);
 float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
 
 void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);

src/main/flight/servos.c

@@ -177,6 +177,7 @@ static void filterServos(void)
         if (!servoFilterIsSet) {
             for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
                 biquadFilterInitLPF(&servoFilter[i], servoConfig()->servo_lowpass_freq, gyro.targetLooptime);
+                biquadFilterReset(&servoFilter[i], servo[i]);
             }
             servoFilterIsSet = true;
         }