src/lib/StubbornSender/stubborn_sender.cpp

   1 #include <cstdint>
   2 #include <algorithm>
   3 #include <cstring>
   4 #include "stubborn_sender.h"
   5
   6 StubbornSender::StubbornSender()
   7     : data(nullptr), length(0)
   8 {
   9     ResetState();
  10 }
  11
  12 void StubbornSender::setMaxPackageIndex(uint8_t maxPackageIndex)
  13 {
  14     if (this->maxPackageIndex != maxPackageIndex)
  15     {
  16         this->maxPackageIndex = maxPackageIndex;
  17         ResetState();
  18     }
  19 }
  20
  21 void StubbornSender::ResetState()
  22 {
  23     bytesLastPayload = 0;
  24     currentOffset = 0;
  25     currentPackage = 1;
  26     telemetryConfirmExpectedValue = true;
  27     waitCount = 0;
  28     // 80 corresponds to UpdateTelemetryRate(ANY, 2, 1), which is what the TX uses in boost mode
  29     maxWaitCount = 80;
  30     senderState = SENDER_IDLE;
  31 }
  32
  33 /***
  34  * Queues a message to send, will abort the current message if one is currently being transmitted
  35  ***/
  36 void StubbornSender::SetDataToTransmit(uint8_t* dataToTransmit, uint8_t lengthToTransmit)
  37 {
  38     // if (lengthToTransmit / bytesPerCall >= maxPackageIndex)
  39     // {
  40     //     return;
  41     // }
  42
  43     length = lengthToTransmit;
  44     data = dataToTransmit;
  45     currentOffset = 0;
  46     currentPackage = 1;
  47     waitCount = 0;
  48     senderState = (senderState == SENDER_IDLE) ? SEND_PENDING : RESYNC_THEN_SEND;
  49 }
  50
  51 /**
  52  * @brief: Copy up to maxLen bytes from the current package to outData
  53  * @returns: packageIndex
  54  ***/
  55 uint8_t StubbornSender::GetCurrentPayload(uint8_t *outData, uint8_t maxLen)
  56 {
  57     uint8_t packageIndex;
  58
  59     bytesLastPayload = 0;
  60     switch (senderState)
  61     {
  62     case RESYNC:
  63     case RESYNC_THEN_SEND:
  64         packageIndex = maxPackageIndex;
  65         break;
  66     case SEND_PENDING:
  67         // This package can now be acked
  68         senderState = SENDING;
  69         // fallthrough
  70     case SENDING:
  71         {
  72             bytesLastPayload = std::min((uint8_t)(length - currentOffset), maxLen);
  73             // If this is the last data chunk, and there has been at least one other packet
  74             // skip the blank packet needed for WAIT_UNTIL_NEXT_CONFIRM
  75             if (currentPackage > 1 && (currentOffset + bytesLastPayload) >= length)
  76                 packageIndex = 0;
  77             else
  78                 packageIndex = currentPackage;
  79
  80             memcpy(outData, &data[currentOffset], bytesLastPayload);
  81         }
  82         break;
  83     default:
  84         packageIndex = 0;
  85     }
  86
  87     return packageIndex;
  88 }
  89
  90 void StubbornSender::ConfirmCurrentPayload(bool telemetryConfirmValue)
  91 {
  92     stubborn_sender_state_e nextSenderState = senderState;
  93
  94     switch (senderState)
  95     {
  96     case SENDING:
  97         if (telemetryConfirmValue != telemetryConfirmExpectedValue)
  98         {
  99             waitCount++;
 100             if (waitCount > maxWaitCount)
 101             {
 102                 telemetryConfirmExpectedValue = !telemetryConfirmValue;
 103                 nextSenderState = RESYNC;
 104             }
 105             break;
 106         }
 107
 108         currentOffset += bytesLastPayload;
 109         if (currentOffset >= length)
 110         {
 111             // A 0th packet is always requred so the reciver can
 112             // differentiate a new send from a resend, if this is
 113             // the first packet acked, send another, else IDLE
 114             if (currentPackage == 1)
 115                 nextSenderState = WAIT_UNTIL_NEXT_CONFIRM;
 116             else
 117                 nextSenderState = SENDER_IDLE;
 118         }
 119
 120         currentPackage++;
 121         telemetryConfirmExpectedValue = !telemetryConfirmExpectedValue;
 122         waitCount = 0;
 123         break;
 124
 125     case RESYNC:
 126     case RESYNC_THEN_SEND:
 127     case WAIT_UNTIL_NEXT_CONFIRM:
 128         if (telemetryConfirmValue == telemetryConfirmExpectedValue)
 129         {
 130             nextSenderState = (senderState == RESYNC_THEN_SEND) ? SENDING : SENDER_IDLE;
 131             telemetryConfirmExpectedValue = !telemetryConfirmValue;
 132         }
 133         // switch to resync if tx does not confirm value fast enough
 134         else if (senderState == WAIT_UNTIL_NEXT_CONFIRM)
 135         {
 136             waitCount++;
 137             if (waitCount > maxWaitCount)
 138             {
 139                 telemetryConfirmExpectedValue = !telemetryConfirmValue;
 140                 nextSenderState = RESYNC;
 141             }
 142         }
 143         break;
 144
 145     case SEND_PENDING:
 146         // TelemetryConfirm acks are not accepted before sending
 147         // fallthrough
 148     case SENDER_IDLE:
 149         break;
 150     }
 151
 152     senderState = nextSenderState;
 153 }
 154
 155 /*
 156  * Called when the telemetry ratio or air rate changes, calculate
 157  * the new threshold for how many times the telemetryConfirmValue
 158  * can be wrong in a row before giving up and going to RESYNC
 159  */
 160 void StubbornSender::UpdateTelemetryRate(uint16_t airRate, uint8_t tlmRatio, uint8_t tlmBurst)
 161 {
 162     // consipicuously unused airRate parameter, the wait count is strictly based on number
 163     // of packets, not time between the telemetry packets, or a wall clock timeout
 164     (void)airRate;
 165     // The expected number of packet periods between telemetry packets
 166     uint32_t packsBetween = tlmRatio * (1 + tlmBurst) / tlmBurst;
 167     maxWaitCount = packsBetween * SSENDER_MAX_MISSED_PACKETS;
 168 }