Ardupilot2/libraries/Filter/LowPassFilter.cpp

//
/// @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

#ifndef HAL_DEBUG_BUILD
#define AP_INLINE_VECTOR_OPS
#pragma GCC optimize("O2")
#endif
#include "LowPassFilter.h"
#include <AP_InternalError/AP_InternalError.h>

////////////////////////////////////////////////////////////////////////////////////////////
// DigitalLPF
////////////////////////////////////////////////////////////////////////////////////////////

template <class T>
DigitalLPF<T>::DigitalLPF() {
  // built in initialization
  _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;
    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);
    }
}

// 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;
}

template <class T>
void DigitalLPF<T>::reset(T value) { 
    _output = value;
    initialised = true;
}
    
////////////////////////////////////////////////////////////////////////////////////////////
// LowPassFilter
////////////////////////////////////////////////////////////////////////////////////////////

// constructors
template <class T>
LowPassFilter<T>::LowPassFilter() :
    _cutoff_freq(0.0f) {}

template <class T>
LowPassFilter<T>::LowPassFilter(float cutoff_freq) :
    _cutoff_freq(cutoff_freq) {}

template <class T>
LowPassFilter<T>::LowPassFilter(float sample_freq, float cutoff_freq)
{
    set_cutoff_frequency(sample_freq, cutoff_freq);
}

// change parameters
template <class T>
void LowPassFilter<T>::set_cutoff_frequency(float cutoff_freq) {
    _cutoff_freq = cutoff_freq;
}

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

// return the cutoff frequency
template <class T>
float LowPassFilter<T>::get_cutoff_freq(void) const {
    return _cutoff_freq;
}

template <class T>
T LowPassFilter<T>::apply(T sample, float dt) {
    return _filter.apply(sample, _cutoff_freq, dt);
}

template <class T>
T LowPassFilter<T>::apply(T sample) {
    return _filter.apply(sample);
}

template <class T>
const T &LowPassFilter<T>::get() const {
    return _filter.get();
}

template <class T>
void LowPassFilter<T>::reset(T value) {
    _filter.reset(value);
}

/* 
 * 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>;