Line data    Source code

       1             : /* ✔ */
       2             : 
       3             : #pragma once
       4             : 
       5             : #include <AH/Settings/Warnings.hpp>
       6             : AH_DIAGNOSTIC_WERROR() // Enable errors on warnings
       7             : 
       8             : #include <stdint.h>
       9             : #include <AH/STL/limits>
      10             : #include <AH/STL/type_traits>
      11             : 
      12             : /**
      13             :  * @brief   Exponential moving average filter.
      14             :  * 
      15             :  * Fast integer EMA implementation where the weight factor is a power of two.
      16             :  * 
      17             :  * Difference equation: @f$ y[n] = \alpha·x[n]+(1-\alpha)·y[n-1] @f$
      18             :  * where @f$ \alpha = \left(\frac{1}{2}\right)^{K} @f$, @f$ x @f$ is the
      19             :  * input sequence, and @f$ y @f$ is the output sequence.
      20             :  *
      21             :  * [An in-depth explanation of the EMA filter](https://tttapa.github.io/Pages/Mathematics/Systems-and-Control-Theory/Digital-filters/Exponential%20Moving%20Average/)
      22             :  *
      23             :  * @tparam  K
      24             :  *          The amount of bits to shift by. This determines the location
      25             :  *          of the pole in the EMA transfer function, and therefore the
      26             :  *          cut-off frequency.  
      27             :  *          The higher this number, the more filtering takes place.  
      28             :  *          The pole location is @f$ 1 - 2^{-K} @f$.
      29             :  * @tparam  input_t
      30             :  *          The integer type to use for the input and output of the filter. 
      31             :  *          Can be signed or unsigned.
      32             :  * @tparam  state_t
      33             :  *          The unsigned integer type to use for the internal state of the
      34             :  *          filter. A fixed-point representation with @f$ K @f$ fractional
      35             :  *          bits is used, so this type should be at least @f$ M + K @f$ bits
      36             :  *          wide, where @f$ M @f$ is the maximum number of bits of the input.
      37             :  *
      38             :  * Some examples of different combinations of template parameters:
      39             :  * 
      40             :  * 1. Filtering the result of `analogRead`: analogRead returns an integer
      41             :  *    between 0 and 1023, which can be represented using 10 bits, so 
      42             :  *    @f$ M = 10 @f$. If `input_t` and `output_t` are both `uint16_t`,
      43             :  *    the maximum shift factor `K` is @f$ 16 - M = 6 @f$. If `state_t`
      44             :  *    is increased to `uint32_t`, the maximum shift factor `K` is 
      45             :  *    @f$ 32 - M = 22 @f$.
      46             :  * 2. Filtering a signed integer between -32768 and 32767: this can be 
      47             :  *    represented using a 16-bit signed integer, so `input_t` is `int16_t`,
      48             :  *    and @f$ M = 16 @f$. (2¹⁵ = 32768)
      49             :  *    Let's say the shift factor `K` is 1, then the minimum width of 
      50             :  *    `state_t` should be @f$ M + K = 17 @f$ bits, so `uint32_t` would be 
      51             :  *    a sensible choice.
      52             :  * 
      53             :  * @ingroup    AH_Filters
      54             :  */
      55             : template <uint8_t K,
      56             :           class input_t = uint_fast16_t,
      57             :           class state_t = typename std::make_unsigned<input_t>::type>
      58             : class EMA {
      59             :   public:
      60             :     /// Constructor: initialize filter to zero or optional given value.
      61          25 :     EMA(input_t initial = input_t(0))
      62          25 :       : state(zero + (state_t(initial) << K) - initial) {}
      63             : 
      64             :     /**
      65             :      * @brief   Reset the filter to the given value.
      66             :      * 
      67             :      * @param   value 
      68             :      *          The value to reset the filter state to.
      69             :      */
      70           9 :     void reset(input_t value = input_t(0)) {
      71           9 :         state = zero + (state_t(value) << K) - value;
      72           9 :     }
      73             : 
      74             :     /**
      75             :      * @brief   Filter the input: Given @f$ x[n] @f$, calculate @f$ y[n] @f$.
      76             :      *
      77             :      * @param   input
      78             :      *          The new raw input value.
      79             :      * @return  The new filtered output value.
      80             :      */
      81        2063 :     input_t filter(input_t input) {
      82        2063 :       state         += state_t(input);
      83        2063 :       state_t output = (state + half) >> K;
      84        2063 :       output        -= zero >> K;
      85        2063 :       state         -= output;
      86        2063 :       return input_t(output);
      87             :     }
      88             : 
      89             :     /// @copydoc    EMA::filter(input_t)
      90        2021 :     input_t operator()(input_t input) {
      91        2021 :         return filter(input);
      92             :     }
      93             : 
      94             :     constexpr static state_t 
      95             :       max_state  = std::numeric_limits<state_t>::max(),
      96             :       half_state = max_state / 2 + 1,
      97             :       zero       = std::is_unsigned<input_t>::value ? state_t(0) : half_state,
      98             :       half       = K > 0 ? state_t(1) << (K - 1) : state_t(0);
      99             :   
     100             :     static_assert(std::is_unsigned<state_t>::value, 
     101             :                   "state type should be unsigned");
     102             : 
     103             :     static_assert(max_state >= std::numeric_limits<input_t>::max(),
     104             :                   "state type cannot be narrower than input type");               
     105             : 
     106             :     /// Verify the input range to make sure it's compatible with the shift
     107             :     /// factor and the width of the state type.
     108             :     ///
     109             :     /// Examples:
     110             :     /// ~~~cpp
     111             :     /// EMA<5, int_fast16_t, uint_fast16_t> filter;
     112             :     /// static_assert(filter.supports_range(-1024, 1023),
     113             :     ///               "use a wider state or input type, or a smaller shift factor");
     114             :     /// ~~~
     115             :     /// ~~~cpp
     116             :     /// EMA<5, uint_fast16_t, uint_fast16_t> filter;
     117             :     /// static_assert(filter.supports_range(0u, 2047u),
     118             :     ///               "use a wider state or input type, or a smaller shift factor");
     119             :     /// ~~~
     120             :     template <class T>
     121             :     constexpr static bool supports_range(T min, T max) {
     122             :       using sstate_t = typename std::make_signed<state_t>::type;
     123             :       return min <= max &&
     124             :              min >= std::numeric_limits<input_t>::min() &&
     125             :              max <= std::numeric_limits<input_t>::max() &&
     126             :              (std::is_unsigned<input_t>::value
     127             :                ? state_t(max) <= (max_state >> K)
     128             :                : min >= -sstate_t(max_state >> (K + 1)) - 1 &&  
     129             :                  max <= sstate_t(max_state >> (K + 1)));
     130             :     }
     131             : 
     132             :   private:
     133             :     state_t state;
     134             : };
     135             : 
     136             : // -------------------------------------------------------------------------- //
     137             : 
     138             : /**
     139             :  * @brief   A class for single-pole infinite impulse response filters
     140             :  *          or exponential moving average filters.
     141             :  * 
     142             :  * This version uses floating point maths.
     143             :  * 
     144             :  * Difference equation: @f$ y[n] = \alpha·x[n]+(1-\alpha)·y[n-1] @f$
     145             :  * @f$ x @f$ is the input sequence, and @f$ y @f$ is the output sequence.
     146             :  *
     147             :  * [An in-depth explanation of the EMA filter]
     148             :  * (https://tttapa.github.io/Pages/Mathematics/Systems-and-Control-Theory/Digital-filters/Exponential%20Moving%20Average/)
     149             :  * 
     150             :  * @ingroup    AH_Filters
     151             :  */
     152             : class EMA_f {
     153             :   public:
     154             :     /**
     155             :      * @brief   Create an exponential moving average filter with a pole at the
     156             :      *          given location.
     157             :      * 
     158             :      * @param   pole
     159             :      *          The pole of the filter (@f$1-\alpha@f$).  
     160             :      *          Should be a value in the range 
     161             :      *          @f$ \left[0,1\right) @f$.  
     162             :      *          Zero means no filtering, and closer to one means more filtering.
     163             :      */
     164           1 :     EMA_f(float pole) : alpha(1 - pole) {}
     165             : 
     166             :     /**
     167             :      * @brief   Filter the input: Given @f$ x[n] @f$, calculate @f$ y[n] @f$.
     168             :      *
     169             :      * @param   value
     170             :      *          The new raw input value.
     171             :      * @return  The new filtered output value.
     172             :      */
     173          12 :     float filter(float value) {
     174          12 :         filtered += (value - filtered) * alpha;
     175          12 :         return filtered;
     176             :     }
     177             : 
     178             :     /// @copydoc    filter(float)
     179          12 :     float operator()(float value) { return filter(value); }
     180             : 
     181             :   private:
     182             :     float alpha;
     183             :     float filtered = 0;
     184             : };
     185             : 
     186             : AH_DIAGNOSTIC_POP()