Line data    Source code

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