Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AH/Containers/Array.hpp>
       4             : 
       5             : /// @addtogroup Filters
       6             : /// @{
       7             : 
       8             : /** 
       9             :  * @brief   Finite Impulse Response filter implementation.
      10             :  * 
      11             :  * Implements the following difference equation:
      12             :  * 
      13             :  * @f[
      14             :  * y[n] = \sum_{i=0}^{N-1} b_i \cdot x[n-i]
      15             :  * @f]
      16             :  */
      17             : template <uint8_t N, class T = float>
      18             : class FIRFilter {
      19             :   public:
      20             :     /**
      21             :      * @brief   Construct a new FIR Filter object.
      22             :      * 
      23             :      * The coefficients @f$ b @f$ can be derived from the transfer function:
      24             :      * 
      25             :      * @f[
      26             :      * H(z) = b_0 + b_1 z^{-1} + \ldots + b_{N_b} z ^{-N_b}
      27             :      * @f]
      28             :      * 
      29             :      * @param   coefficients 
      30             :      *          The coefficients of the transfer function numerator.
      31             :      */
      32           3 :     FIRFilter(const AH::Array<T, N> &coefficients) {
      33          32 :         for (uint8_t i = 0; i < 2 * N - 1; ++i)
      34          29 :             this->coefficients[i] = coefficients[(2 * N - 1 - i) % N];
      35           3 :     }
      36             : 
      37             :     /**
      38             :      * @brief   Update the internal state with the new input @f$ x[n] @f$ and
      39             :      *          return the new output @f$ y[n] @f$.
      40             :      * 
      41             :      * @param   input 
      42             :      *          The new input @f$ x[n] @f$.
      43             :      * @return  The new output @f$ y[n] @f$.
      44             :      */
      45          60 :     T operator()(T input) {
      46             :         // Save the new value to the ring buffer.
      47          60 :         x[index_b] = input;
      48             : 
      49             :         // Calculate the offset to the shifted coefficients.
      50          60 :         T *coeff_shift = coefficients.end() - N - index_b;
      51             : 
      52             :         // Multiply and accumulate the inputs and their respective coefficients.
      53          60 :         T acc = {};
      54         380 :         for (uint8_t i = 0; i < N; i++)
      55         320 :             acc += x[i] * coeff_shift[i];
      56             : 
      57             :         // Increment and wrap around the index of the ring buffer.
      58          60 :         index_b++;
      59          60 :         if (index_b == N)
      60          26 :             index_b = 0;
      61             : 
      62          60 :         return acc;
      63             :     }
      64             : 
      65             :   private:
      66             :     uint8_t index_b = 0;
      67             :     AH::Array<T, N> x = {};
      68             :     AH::Array<T, 2 * N - 1> coefficients;
      69             : };
      70             : 
      71             : /// @}