src/lib/StubbornReceiver/stubborn_receiver.cpp

   1 #include <cstdint>
   2 #include <algorithm>
   3 #include <cstring>
   4 #include "stubborn_receiver.h"
   5
   6 StubbornReceiver::StubbornReceiver()
   7 {
   8     ResetState();
   9     data = nullptr;
  10     length = 0;
  11 }
  12
  13 void StubbornReceiver::setMaxPackageIndex(uint8_t maxPackageIndex)
  14 {
  15     if (this->maxPackageIndex != maxPackageIndex)
  16     {
  17         this->maxPackageIndex = maxPackageIndex;
  18         ResetState();
  19     }
  20 }
  21
  22 void StubbornReceiver::ResetState()
  23 {
  24     currentPackage = 1;
  25     currentOffset = 0;
  26     telemetryConfirm = false;
  27 }
  28
  29 bool StubbornReceiver::GetCurrentConfirm()
  30 {
  31     return telemetryConfirm;
  32 }
  33
  34 void StubbornReceiver::SetDataToReceive(uint8_t* dataToReceive, uint8_t maxLength)
  35 {
  36     length = maxLength;
  37     data = dataToReceive;
  38     currentPackage = 1;
  39     currentOffset = 0;
  40     finishedData = false;
  41 }
  42
  43 void StubbornReceiver::ReceiveData(uint8_t const packageIndex, uint8_t const * const receiveData, uint8_t dataLen)
  44 {
  45     // Resync
  46     if (packageIndex == maxPackageIndex)
  47     {
  48         telemetryConfirm = !telemetryConfirm;
  49         currentPackage = 1;
  50         currentOffset = 0;
  51         finishedData = false;
  52         return;
  53     }
  54
  55     if (finishedData)
  56     {
  57         return;
  58     }
  59
  60     bool acceptData = false;
  61     // If this is the last package, accept as being complete
  62     if (packageIndex == 0 && currentPackage > 1)
  63     {
  64         // PackageIndex 0 (the final packet) can also contain data
  65         acceptData = true;
  66         finishedData = true;
  67     }
  68     // If this package is the expected index, accept and advance index
  69     else if (packageIndex == currentPackage)
  70     {
  71         acceptData = true;
  72     }
  73     // If this is the first package from the sender, and we're mid-receive
  74     // assume the sender has restarted without resync or is freshly booted
  75     // skip the resync process entirely and just pretend this is a fresh boot too
  76     else if (packageIndex == 1 && currentPackage > 1)
  77     {
  78         currentPackage = 1;
  79         currentOffset = 0;
  80         acceptData = true;
  81     }
  82
  83     if (acceptData)
  84     {
  85         uint8_t len = std::min((uint8_t)(length - currentOffset), dataLen);
  86         memcpy(&data[currentOffset], receiveData, len);
  87         currentPackage++;
  88         currentOffset += len;
  89         telemetryConfirm = !telemetryConfirm;
  90     }
  91 }
  92
  93 bool StubbornReceiver::HasFinishedData()
  94 {
  95     return finishedData;
  96 }
  97
  98 void StubbornReceiver::Unlock()
  99 {
 100     if (finishedData)
 101     {
 102         currentPackage = 1;
 103         currentOffset = 0;
 104         finishedData = false;
 105     }
 106 }