src/lib/CRSF2MSP/crsf2msp.cpp

   1 #include "crsf2msp.h"
   2
   3 extern GENERIC_CRC8 crsf_crc; // defined in crsf.cpp reused here
   4
   5 CROSSFIRE2MSP::CROSSFIRE2MSP()
   6 {
   7     reset();
   8 }
   9
  10 void CROSSFIRE2MSP::reset()
  11 {
  12     pktLen = 0;
  13     idx = 0;
  14     frameComplete = false;
  15     MSPvers = MSP_FRAME_UNKNOWN;
  16 }
  17
  18 void CROSSFIRE2MSP::parse(const uint8_t *data)
  19 {
  20     uint8_t CRSFpayloadLen = data[CRSF_FRAME_PAYLOAD_LEN_IDX] - CRSF_EXT_FRAME_PAYLOAD_LEN_SIZE_OFFSET;
  21     bool error = isError(data);
  22     bool newFrame = isNewFrame(data);
  23
  24     bool seqError;
  25     uint8_t seqNumber = getSeqNumber(data);
  26     uint8_t SeqNumberNext;
  27
  28     SeqNumberNext = (seqNumberPrev + 1) & 0b1111;
  29     SeqNumberNext == seqNumber ? seqError = false : seqError = true;
  30     seqNumberPrev = seqNumber;
  31
  32     if ((!newFrame && seqError) || error)
  33     {
  34         reset();
  35         return;
  36     }
  37
  38     if (newFrame) // If it's a new frame then out a header on first
  39     {
  40         idx = 3; // skip the header start wiring at offset 3.
  41         MSPvers = getVersion(data);
  42         src = data[CRSF_MSP_SRC_OFFSET];
  43         dest = data[CRSF_MSP_DEST_OFFSET];
  44         outBuffer[0] = '$';
  45         outBuffer[1] = (MSPvers == MSP_FRAME_V1 || MSPvers == MSP_FRAME_V1_JUMBO) ? 'M' : 'X';
  46         outBuffer[2] = error ? '!' : getHeaderDir(data);
  47         pktLen = getFrameLen(data, MSPvers);
  48     }
  49
  50     // process the chunk of MSP frame
  51     // if the last CRSF frame is zero padded we can't use the CRSF payload length
  52     // but if this isn't the last chunk we can't use the MSP payload length
  53     // the solution is to use the minimum of the two lengths
  54     uint32_t frameLen = pktLen - (idx - 3);
  55     uint32_t minLen = frameLen < CRSFpayloadLen ? frameLen : CRSFpayloadLen;
  56     memcpy(&outBuffer[idx], &data[CRSF_MSP_FRAME_OFFSET], minLen); // chunk of MSP data
  57     idx += minLen;
  58
  59     if (idx - 3 == pktLen) // we have a complete MSP frame, -3 because the header isn't counted
  60     {
  61         // we need to append the MSP checksum
  62         outBuffer[idx] = getChecksum(outBuffer + 3, pktLen, MSPvers); // +3 because the header isn't in checksum
  63         frameComplete = true;
  64
  65         FIFOout.lock();
  66         FIFOout.pushSize(idx + 1);
  67         FIFOout.pushBytes(outBuffer, idx + 1);
  68         FIFOout.unlock();
  69     }
  70 }
  71
  72 bool CROSSFIRE2MSP::isNewFrame(const uint8_t *data)
  73 {
  74     const uint8_t statusByte = data[CRSF_MSP_STATUS_BYTE_OFFSET];
  75     return (bool)((statusByte & 0b10000) >> 4); // bit active if there is a new frame
  76 }
  77
  78 bool CROSSFIRE2MSP::isError(const uint8_t *data)
  79 {
  80     const uint8_t statusByte = data[CRSF_MSP_STATUS_BYTE_OFFSET];
  81     return (bool)((statusByte & 0b10000000) >> 7);
  82 }
  83
  84 uint8_t CROSSFIRE2MSP::getSeqNumber(const uint8_t *data)
  85 {
  86     const uint8_t statusByte = data[CRSF_MSP_STATUS_BYTE_OFFSET];
  87     return statusByte & 0b1111; // first four bits is seq number
  88 }
  89
  90 MSPframeType_e CROSSFIRE2MSP::getVersion(const uint8_t *data)
  91 {
  92     const uint8_t statusByte = data[CRSF_MSP_STATUS_BYTE_OFFSET];
  93     const uint8_t payloadLen = data[CRSF_MSP_FRAME_OFFSET]; // first element is the payload length
  94     uint8_t headerVersion = ((statusByte & 0b01100000) >> 5);
  95     MSPframeType_e MSPvers;
  96
  97     if (headerVersion == 1)
  98     {
  99         if (payloadLen == 0xFF)
 100         {
 101             MSPvers = MSP_FRAME_V1_JUMBO;
 102         }
 103         else
 104         {
 105             MSPvers = MSP_FRAME_V1;
 106         }
 107     }
 108     else if (headerVersion == 2)
 109     {
 110         MSPvers = MSP_FRAME_V2;
 111     }
 112     else
 113     {
 114         MSPvers = MSP_FRAME_UNKNOWN;
 115     }
 116     return MSPvers;
 117 }
 118
 119 uint8_t CROSSFIRE2MSP::getChecksum(const uint8_t *data, const uint32_t len, MSPframeType_e mspVersion)
 120 {
 121     uint8_t checkSum = 0;
 122
 123     if (mspVersion == MSP_FRAME_V1 || mspVersion == MSP_FRAME_V1_JUMBO)
 124     {
 125         for (uint32_t i = 0; i < len; i++)
 126         {
 127             checkSum ^= data[i];
 128         }
 129         return checkSum;
 130     }
 131     else if (mspVersion == MSP_FRAME_V2)
 132     {
 133         checkSum = crsf_crc.calc(data, len);
 134     }
 135     else
 136     {
 137         checkSum = 0;
 138     }
 139     return checkSum;
 140 }
 141
 142 uint32_t CROSSFIRE2MSP::getFrameLen(const uint8_t *data, MSPframeType_e mspVersion)
 143 {
 144     uint8_t lowByte;
 145     uint8_t highByte;
 146
 147     if (mspVersion == MSP_FRAME_V1)
 148     {
 149         return (MSP_V1_FRAME_LEN_FROM_PAYLOAD_LEN(data[CRSF_MSP_FRAME_OFFSET]));
 150     }
 151     else if (mspVersion == MSP_FRAME_V1_JUMBO)
 152     {
 153         lowByte = data[CRSF_MSP_FRAME_OFFSET + 2];
 154         highByte = data[CRSF_MSP_FRAME_OFFSET + 3];
 155         return MSP_V1_JUMBO_FRAME_LEN_FROM_PAYLOAD_LEN((highByte << 8) | lowByte);
 156     }
 157     else if (mspVersion == MSP_FRAME_V2)
 158     {
 159         lowByte = data[CRSF_MSP_FRAME_OFFSET + 3];
 160         highByte = data[CRSF_MSP_FRAME_OFFSET + 4];
 161         return MSP_V2_FRAME_LEN_FROM_PAYLOAD_LEN((highByte << 8) | lowByte);
 162     }
 163     else
 164     {
 165         return 0;
 166     }
 167 }
 168
 169 uint8_t CROSSFIRE2MSP::getHeaderDir(const uint8_t *data)
 170 {
 171     const uint8_t statusByte = data[CRSF_MSP_TYPE_IDX];
 172     if (statusByte == 0x7A)
 173     {
 174         return '<';
 175     }
 176     else if (statusByte == 0x7B)
 177     {
 178         return '>';
 179     }
 180     else
 181     {
 182         return '!';
 183     }
 184 }
 185
 186 bool CROSSFIRE2MSP::isFrameReady()
 187 {
 188     return frameComplete;
 189 }
 190
 191 const uint8_t *CROSSFIRE2MSP::getFrame()
 192 {
 193     return outBuffer;
 194 }
 195
 196 uint32_t CROSSFIRE2MSP::getFrameLen()
 197 {
 198     return idx + 1; // include the last byte (crc)
 199 }
 200
 201 uint8_t CROSSFIRE2MSP::getSrc()
 202 {
 203     return src;
 204 }
 205
 206 uint8_t CROSSFIRE2MSP::getDest()
 207 {
 208     return dest;
 209 }