Filter: fixed reset of filters to first value

the delay elements were set incorrectly

libraries/Filter/LowPassFilter2p.cpp

@@ -18,7 +18,7 @@ T DigitalBiquadFilter<T>::apply(const T &sample, const struct biquad_params &par
     }
 
     if (!initialised) {
-        reset(sample);
+        reset(sample, params);
         initialised = true;
     }
 
@@ -38,8 +38,8 @@ void DigitalBiquadFilter<T>::reset() {
 }
 
 template <class T>
-void DigitalBiquadFilter<T>::reset(const T &value) {
-    _delay_element_1 = _delay_element_2 = value;
+void DigitalBiquadFilter<T>::reset(const T &value, const struct biquad_params &params) {
+    _delay_element_1 = _delay_element_2 = value * (1.0 / (1 + params.a1 + params.a2));
     initialised = true;
 }
 
@@ -113,7 +113,7 @@ void LowPassFilter2p<T>::reset(void) {
 
 template <class T>
 void LowPassFilter2p<T>::reset(const T &value) {
-    return _filter.reset(value);
+    return _filter.reset(value, _params);
 }
 
 /* 

libraries/Filter/LowPassFilter2p.h

@@ -39,7 +39,7 @@ public:
 
     T apply(const T &sample, const struct biquad_params &params);
     void reset();
-    void reset(const T &value);
+    void reset(const T &value, const struct biquad_params &params);
     static void compute_params(float sample_freq, float cutoff_freq, biquad_params &ret);
     
 private:

libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp

@@ -26,16 +26,12 @@ void loop()
     for(int16_t i = 0; i < 300; i++ ) {
 
         // new data value
-        const float new_value = sinf((float)i * 2 * M_PI * 5 / 50.0f);  // 5hz
-
-        // output to user
-        hal.console->printf("applying: %6.4f", (double)new_value);
-
+        const float new_value = 17 + sinf((float)i * 2 * M_PI * 5 / 50.0f);  // 5hz
         // apply new value and retrieved filtered result
         const float filtered_value = low_pass_filter.apply(new_value);
 
-        // display results
-        hal.console->printf("\toutput: %6.4f\n", (double)filtered_value);
+        // output to user
+        hal.console->printf("applying: %6.4f -> %6.4f\n", (double)new_value, filtered_value);
 
         hal.scheduler->delay(10);
     }