Filter: LowPassFilter: split into two classes for constant and varable dt

ArduCopter/mode.h

@@ -984,7 +984,7 @@ class ModeFlowHold : public Mode {
     // calculate attitude from flow data
     void flow_to_angle(Vector2f &bf_angle);
 
-    LowPassFilterVector2f flow_filter;
+    LowPassFilterConstDtVector2f flow_filter;
 
     bool flowhold_init(bool ignore_checks);
     void flowhold_run();

ArduCopter/mode_flowhold.cpp

@@ -240,7 +240,7 @@ void ModeFlowHold::run()
 
     // check for filter change
     if (!is_equal(flow_filter.get_cutoff_freq(), flow_filter_hz.get())) {
-        flow_filter.set_cutoff_frequency(flow_filter_hz.get());
+        flow_filter.set_cutoff_frequency(copter.scheduler.get_loop_rate_hz(), flow_filter_hz.get());
     }
 
     // get pilot desired climb rate

libraries/APM_Control/AP_AutoTune.h

@@ -128,9 +128,9 @@ class AP_AutoTune
     // 5 point mode filter for FF estimate
     ModeFilterFloat_Size5 ff_filter;
 
-    LowPassFilterFloat actuator_filter;
-    LowPassFilterFloat rate_filter;
-    LowPassFilterFloat target_filter;
+    LowPassFilterConstDtFloat actuator_filter;
+    LowPassFilterConstDtFloat rate_filter;
+    LowPassFilterConstDtFloat target_filter;
 
     // separate slew limiters for P and D
     float slew_limit_max, slew_limit_tau;

libraries/AP_GyroFFT/AP_GyroFFT.h

@@ -157,7 +157,7 @@ class AP_GyroFFT
         }
 
     private:
-        LowPassFilterFloat _lowpass_filter[XYZ_AXIS_COUNT];
+        LowPassFilterConstDtFloat _lowpass_filter[XYZ_AXIS_COUNT];
         FilterWithBuffer<float,3> _median_filter[XYZ_AXIS_COUNT];
     };
 
@@ -311,7 +311,7 @@ class AP_GyroFFT
     // smoothing filter on the bandwidth
     MedianLowPassFilter3dFloat _center_bandwidth_filter[FrequencyPeak::MAX_TRACKED_PEAKS];
     // smoothing filter on the frequency fit
-    LowPassFilterFloat _harmonic_fit_filter[XYZ_AXIS_COUNT];
+    LowPassFilterConstDtFloat _harmonic_fit_filter[XYZ_AXIS_COUNT];
 
     // configured sampling rate
     uint16_t _fft_sampling_rate_hz;

libraries/AP_ICEngine/AP_ICEngine.h

@@ -83,7 +83,7 @@ class AP_ICEngine {
 
 #if AP_RPM_ENABLED
     // filter for RPM value
-    LowPassFilterFloat _rpm_filter;
+    LowPassFilterConstDtFloat _rpm_filter;
     float filtered_rpm_value;
 #endif
 

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h

@@ -182,7 +182,7 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend
         Vector3f gyro;
         uint8_t accel_count;
         uint8_t gyro_count;
-        LowPassFilterVector3f accel_filter{4000, 188};
+        LowPassFilterConstDtVector3f accel_filter{4000, 188};
     } _accum;
 };
 

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h

@@ -152,7 +152,7 @@ class AP_InertialSensor_Invensensev2 : public AP_InertialSensor_Backend
         Vector3f gyro;
         uint8_t accel_count;
         uint8_t gyro_count;
-        LowPassFilterVector3f accel_filter{4500, 188};
+        LowPassFilterConstDtVector3f accel_filter{4500, 188};
     } _accum;
 };
 

libraries/Filter/LowPassFilter.cpp

@@ -1,8 +1,6 @@
 //
 /// @file LowPassFilter.cpp
-/// @brief  A class to implement a low pass filter without losing precision even for int types
-///         the downside being that it's a little slower as it internally uses a float
-///         and it consumes an extra 4 bytes of memory to hold the constant gain
+/// @brief  A class to implement a low pass filter
 
 #ifndef HAL_DEBUG_BUILD
 #define AP_INLINE_VECTOR_OPS
@@ -12,134 +10,134 @@
 #include <AP_InternalError/AP_InternalError.h>
 
 ////////////////////////////////////////////////////////////////////////////////////////////
-// DigitalLPF
+// DigitalLPF, base class
 ////////////////////////////////////////////////////////////////////////////////////////////
 
 template <class T>
 DigitalLPF<T>::DigitalLPF() {
   // built in initialization
-  _output = T();
+  output = T();
 }
 
 // add a new raw value to the filter, retrieve the filtered result
 template <class T>
-T DigitalLPF<T>::apply(const T &sample, float cutoff_freq, float dt) {
-    if (is_negative(cutoff_freq) || is_negative(dt)) {
-        INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
-        _output = sample;
-        return _output;
-    }
-    if (is_zero(cutoff_freq)) {
-        _output = sample;
-        return _output;
-    }
-    if (is_zero(dt)) {
-        return _output;
-    }
-    float rc = 1.0f/(M_2PI*cutoff_freq);
-    alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
-    _output += (sample - _output) * alpha;
-    if (!initialised) {
-        initialised = true;
-        _output = sample;
-    }
-    return _output;
-}
-
-template <class T>
-T DigitalLPF<T>::apply(const T &sample) {
-    _output += (sample - _output) * alpha;
+T DigitalLPF<T>::_apply(const T &sample, const float &alpha) {
+    output += (sample - output) * alpha;
     if (!initialised) {
         initialised = true;
-        _output = sample;
-    }
-    return _output;
-}
-
-template <class T>
-void DigitalLPF<T>::compute_alpha(float sample_freq, float cutoff_freq) {
-    if (sample_freq <= 0) {
-        alpha = 1;
-    } else {
-        alpha = calc_lowpass_alpha_dt(1.0/sample_freq, cutoff_freq);
+        output = sample;
     }
+    return output;
 }
 
 // get latest filtered value from filter (equal to the value returned by latest call to apply method)
 template <class T>
 const T &DigitalLPF<T>::get() const {
-    return _output;
+    return output;
 }
 
+// Reset filter to given value
 template <class T>
-void DigitalLPF<T>::reset(T value) { 
-    _output = value;
+void DigitalLPF<T>::reset(const T &value) { 
+    output = value;
     initialised = true;
 }
-
-////////////////////////////////////////////////////////////////////////////////////////////
-// LowPassFilter
-////////////////////////////////////////////////////////////////////////////////////////////
 
-// constructors
+// Set reset flag such that the filter will be reset to the next value applied
 template <class T>
-LowPassFilter<T>::LowPassFilter() :
-    _cutoff_freq(0.0f) {}
+void DigitalLPF<T>::reset() {
+    initialised = false;
+}
 
-template <class T>
-LowPassFilter<T>::LowPassFilter(float cutoff_freq) :
-    _cutoff_freq(cutoff_freq) {}
+template class DigitalLPF<float>;
+template class DigitalLPF<Vector2f>;
+template class DigitalLPF<Vector3f>;
+
+////////////////////////////////////////////////////////////////////////////////////////////
+// Low pass filter with constant time step
+////////////////////////////////////////////////////////////////////////////////////////////
 
+// constructor
 template <class T>
-LowPassFilter<T>::LowPassFilter(float sample_freq, float cutoff_freq)
+LowPassFilterConstDt<T>::LowPassFilterConstDt(const float &sample_freq, const float &new_cutoff_freq)
 {
-    set_cutoff_frequency(sample_freq, cutoff_freq);
+    set_cutoff_frequency(sample_freq, new_cutoff_freq);
 }
 
 // change parameters
 template <class T>
-void LowPassFilter<T>::set_cutoff_frequency(float cutoff_freq) {
-    _cutoff_freq = cutoff_freq;
+void LowPassFilterConstDt<T>::set_cutoff_frequency(const float &sample_freq, const float &new_cutoff_freq) {
+    cutoff_freq = new_cutoff_freq;
+
+    if (sample_freq <= 0) {
+        alpha = 1;
+    } else {
+        alpha = calc_lowpass_alpha_dt(1.0/sample_freq, cutoff_freq);
+    }
 }
 
+// return the cutoff frequency
 template <class T>
-void LowPassFilter<T>::set_cutoff_frequency(float sample_freq, float cutoff_freq) {
-    _cutoff_freq = cutoff_freq;
-    _filter.compute_alpha(sample_freq, cutoff_freq);
+float LowPassFilterConstDt<T>::get_cutoff_freq(void) const {
+    return cutoff_freq;
 }
 
-// return the cutoff frequency
+// add a new raw value to the filter, retrieve the filtered result
 template <class T>
-float LowPassFilter<T>::get_cutoff_freq(void) const {
-    return _cutoff_freq;
+T LowPassFilterConstDt<T>::apply(const T &sample) {
+    return this->_apply(sample, alpha);
 }
 
+template class LowPassFilterConstDt<float>;
+template class LowPassFilterConstDt<Vector2f>;
+template class LowPassFilterConstDt<Vector3f>;
+
+////////////////////////////////////////////////////////////////////////////////////////////
+// Low pass filter with variable time step
+////////////////////////////////////////////////////////////////////////////////////////////
+
 template <class T>
-T LowPassFilter<T>::apply(T sample, float dt) {
-    return _filter.apply(sample, _cutoff_freq, dt);
+LowPassFilter<T>::LowPassFilter(const float &new_cutoff_freq)
+{
+    set_cutoff_frequency(new_cutoff_freq);
 }
 
+// change parameters
 template <class T>
-T LowPassFilter<T>::apply(T sample) {
-    return _filter.apply(sample);
+void LowPassFilter<T>::set_cutoff_frequency(const float &new_cutoff_freq) {
+    cutoff_freq = new_cutoff_freq;
 }
 
+// return the cutoff frequency
 template <class T>
-const T &LowPassFilter<T>::get() const {
-    return _filter.get();
+float LowPassFilter<T>::get_cutoff_freq() const {
+    return cutoff_freq;
 }
 
+// add a new raw value to the filter, retrieve the filtered result
 template <class T>
-void LowPassFilter<T>::reset(T value) {
-    _filter.reset(value);
+T LowPassFilter<T>::apply(const T &sample, const float &dt) {
+    if (is_negative(cutoff_freq) || is_negative(dt)) {
+        INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
+        this->reset(sample);
+        return this->get();
+    }
+    if (is_zero(cutoff_freq)) {
+        this->reset(sample);
+        return this->get();
+    }
+    if (is_zero(dt)) {
+        return this->get();
+    }
+    const float rc = 1.0f/(M_2PI*cutoff_freq);
+    const float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
+    return this->_apply(sample, alpha);
 }
 
 /* 
  * Make an instances
  * Otherwise we have to move the constructor implementations to the header file :P
  */
-template class LowPassFilter<int>;
-template class LowPassFilter<long>;
 template class LowPassFilter<float>;
 template class LowPassFilter<Vector2f>;
 template class LowPassFilter<Vector3f>;