src/lib/LQCALC/LQCALC.h

   1 #pragma once
   2
   3 #include <stdint.h>
   4
   5 template <uint8_t N>
   6 class LQCALC
   7 {
   8 public:
   9     LQCALC(void)
  10     {
  11         reset();
  12         // count is reset here only once on construction to start LQ counting
  13         // at 100% on first connect, but 0 out of N after a failsafe
  14         count = 1;
  15     }
  16
  17     /* Set the bit for the current period to true and update the running LQ */
  18     void add()
  19     {
  20         if (currentIsSet())
  21             return;
  22         LQArray[index] |= LQmask;
  23         LQ += 1;
  24     }
  25
  26     /* Start a new period */
  27     void inc()
  28     {
  29         // Increment the counter by shifting one bit higher
  30         // If we've shifted out all the bits, move to next idx
  31         LQmask = LQmask << 1;
  32         if (LQmask == 0)
  33         {
  34             LQmask = (1 << 0);
  35             index += 1;
  36         }
  37
  38         // At idx N / 32 and bit N % 32, wrap back to idx=0, bit=0
  39         if ((index == (N / 32)) && (LQmask & (1 << (N % 32))))
  40         {
  41             index = 0;
  42             LQmask = (1 << 0);
  43         }
  44
  45         if ((LQArray[index] & LQmask) != 0)
  46         {
  47             LQArray[index] &= ~LQmask;
  48             LQ -= 1;
  49         }
  50
  51         if (count < N)
  52           ++count;
  53     }
  54
  55     /* Return the current running total of bits set, in percent */
  56     uint8_t getLQ() const
  57     {
  58         return (uint32_t)LQ * 100U / count;
  59     }
  60
  61     /* Return the current running total of bits set, up to N */
  62     uint8_t getLQRaw() const
  63     {
  64         return LQ;
  65     }
  66
  67     /* Return the number of periods recorded so far, up to N */
  68     uint8_t getCount() const
  69     {
  70         return count;
  71     }
  72
  73     /* Return N, the size of the LQ history */
  74     uint8_t getSize() const
  75     {
  76         return N;
  77     }
  78
  79     /* Initialize and zero the history */
  80     void reset()
  81     {
  82         // count is intentonally not zeroed here to start LQ counting up from 0
  83         // after a failsafe, instead of down from 100
  84         LQ = 0;
  85         index = 0;
  86         LQmask = (1 << 0);
  87         for (uint8_t i = 0; i < (sizeof(LQArray)/sizeof(LQArray[0])); i++)
  88             LQArray[i] = 0;
  89     }
  90
  91     /*  Return true if the current period was add()ed */
  92     bool currentIsSet() const
  93     {
  94         return LQArray[index] & LQmask;
  95     }
  96
  97 private:
  98     uint8_t LQ;
  99     uint8_t index; // current position in LQArray
 100     uint8_t count;
 101     uint32_t LQmask;
 102     uint32_t LQArray[(N + 31)/32];
 103 };