src/lib/MSP/msp.cpp

   1 #include "msp.h"
   2
   3 #include "logging.h"
   4
   5 /* ==========================================
   6 MSP V2 Message Structure:
   7 Offset: Usage:         In CRC:  Comment:
   8 ======= ======         =======  ========
   9 0       $                       Framing magic start char
  10 1       X                       'X' in place of v1 'M'
  11 2       type                    '<' / '>' / '!' Message Type (TODO find out what ! type is)
  12 3       flag           +        uint8, flag, usage to be defined (set to zero)
  13 4       function       +        uint16 (little endian). 0 - 255 is the same function as V1 for backwards compatibility
  14 6       payload size   +        uint16 (little endian) payload size in bytes
  15 8       payload        +        n (up to 65535 bytes) payload
  16 n+8     checksum                uint8, (n= payload size), crc8_dvb_s2 checksum
  17 ========================================== */
  18
  19 // CRC helper function. External to MSP class
  20 // TODO: Move all our CRC functions to a CRC lib
  21 uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a)
  22 {
  23     crc ^= a;
  24     for (int ii = 0; ii < 8; ++ii) {
  25         if (crc & 0x80) {
  26             crc = (crc << 1) ^ 0xD5;
  27         } else {
  28             crc = crc << 1;
  29         }
  30     }
  31     return crc;
  32 }
  33
  34 bool
  35 MSP::processReceivedByte(uint8_t c)
  36 {
  37     switch (m_inputState) {
  38
  39         case MSP_IDLE:
  40             // Wait for framing char
  41             if (c == '$') {
  42                 m_inputState = MSP_HEADER_START;
  43             }
  44             break;
  45
  46         case MSP_HEADER_START:
  47             // Waiting for 'X' (MSPv2 native)
  48             switch (c) {
  49                 case 'X':
  50                     m_inputState = MSP_HEADER_X;
  51                     break;
  52                 default:
  53                     m_inputState = MSP_IDLE;
  54                     break;
  55             }
  56             break;
  57
  58         case MSP_HEADER_X:
  59             // Wait for the packet type (cmd or req)
  60             m_inputState = MSP_HEADER_V2_NATIVE;
  61
  62             // Start of a new packet
  63             // reset the packet, offset iterator, and CRC
  64             m_packet.reset();
  65             m_offset = 0;
  66             m_crc = 0;
  67
  68             switch (c) {
  69                 case '<':
  70                     m_packet.type = MSP_PACKET_COMMAND;
  71                     break;
  72                 case '>':
  73                     m_packet.type = MSP_PACKET_RESPONSE;
  74                     break;
  75                 default:
  76                     m_packet.type = MSP_PACKET_UNKNOWN;
  77                     m_inputState = MSP_IDLE;
  78                     break;
  79             }
  80             break;
  81
  82         case MSP_HEADER_V2_NATIVE:
  83             // Read bytes until we have a full header
  84             m_inputBuffer[m_offset++] = c;
  85             m_crc = crc8_dvb_s2(m_crc, c);
  86
  87             // If we've received the correct amount of bytes for a full header
  88             if (m_offset == sizeof(mspHeaderV2_t)) {
  89                 // Copy header values into packet
  90                 mspHeaderV2_t* header = (mspHeaderV2_t*)&m_inputBuffer[0];
  91                 m_packet.payloadSize = header->payloadSize;
  92                 m_packet.function = header->function;
  93                 m_packet.flags = header->flags;
  94                 // reset the offset iterator for re-use in payload below
  95                 m_offset = 0;
  96                                 if (m_packet.payloadSize == 0)
  97                         m_inputState = MSP_CHECKSUM_V2_NATIVE;
  98                                 else
  99                         m_inputState = MSP_PAYLOAD_V2_NATIVE;
 100             }
 101             break;
 102
 103         case MSP_PAYLOAD_V2_NATIVE:
 104             // Read bytes until we reach payloadSize
 105             m_packet.payload[m_offset++] = c;
 106             m_crc = crc8_dvb_s2(m_crc, c);
 107
 108             // If we've received the correct amount of bytes for payload
 109             if (m_offset == m_packet.payloadSize) {
 110                 // Then we're up to the CRC
 111                 m_inputState = MSP_CHECKSUM_V2_NATIVE;
 112             }
 113             break;
 114
 115         case MSP_CHECKSUM_V2_NATIVE:
 116             // Assert that the checksums match
 117             if (m_crc == c) {
 118                 m_inputState = MSP_COMMAND_RECEIVED;
 119             }
 120             else {
 121                 DBGLN("CRC failure on MSP packet - Got %d expected %d", c, m_crc);
 122                 m_inputState = MSP_IDLE;
 123             }
 124             break;
 125
 126         default:
 127             m_inputState = MSP_IDLE;
 128             break;
 129     }
 130
 131     // If we've successfully parsed a complete packet
 132     // return true so the calling function knows that
 133     // a new packet is ready.
 134     if (m_inputState == MSP_COMMAND_RECEIVED) {
 135         return true;
 136     }
 137     return false;
 138 }
 139
 140 mspPacket_t*
 141 MSP::getReceivedPacket()
 142 {
 143     return &m_packet;
 144 }
 145
 146 void
 147 MSP::markPacketReceived()
 148 {
 149     // Set input state to idle, ready to receive the next packet
 150     // The current packet data will be discarded internally
 151     m_inputState = MSP_IDLE;
 152 }
 153
 154 bool
 155 MSP::sendPacket(mspPacket_t* packet, Stream* port)
 156 {
 157     // Sanity check the packet before sending
 158     if (packet->type != MSP_PACKET_COMMAND && packet->type != MSP_PACKET_RESPONSE) {
 159         // Unsupported packet type (note: ignoring '!' until we know what it is)
 160         return false;
 161     }
 162
 163     if (packet->type == MSP_PACKET_RESPONSE && packet->payloadSize == 0) {
 164         // Response packet with no payload
 165         return false;
 166     }
 167
 168     // Write out the framing chars
 169     port->write('$');
 170     port->write('X');
 171
 172     // Write out the packet type
 173     if (packet->type == MSP_PACKET_COMMAND) {
 174         port->write('<');
 175     }
 176     else if (packet->type == MSP_PACKET_RESPONSE) {
 177         port->write('>');
 178     }
 179
 180     // Subsequent bytes are contained in the crc
 181     uint8_t crc = 0;
 182
 183     // Pack header struct into buffer
 184     uint8_t headerBuffer[5];
 185     mspHeaderV2_t* header = (mspHeaderV2_t*)&headerBuffer[0];
 186     header->flags = packet->flags;
 187     header->function = packet->function;
 188     header->payloadSize = packet->payloadSize;
 189
 190     // Write out the header buffer, adding each byte to the crc
 191     for (uint8_t i = 0; i < sizeof(mspHeaderV2_t); ++i) {
 192         port->write(headerBuffer[i]);
 193         crc = crc8_dvb_s2(crc, headerBuffer[i]);
 194     }
 195
 196     // Write out the payload, adding each byte to the crc
 197     for (uint16_t i = 0; i < packet->payloadSize; ++i) {
 198         port->write(packet->payload[i]);
 199         crc = crc8_dvb_s2(crc, packet->payload[i]);
 200     }
 201
 202     // Write out the crc
 203     port->write(crc);
 204
 205     return true;
 206 }