src/lib/MSP/msp.h

   1 #pragma once
   2
   3 #include "targets.h"
   4
   5 // TODO: MSP_PORT_INBUF_SIZE should be changed to
   6 // dynamically allocate array length based on the payload size
   7 // Hardcoding payload size to 8 bytes for now, since MSP is
   8 // limited to a 4 byte payload on the BF side
   9 #define MSP_PORT_INBUF_SIZE 8
  10
  11 #define CHECK_PACKET_PARSING() \
  12   if (packet->readError) {\
  13     return;\
  14   }
  15
  16 typedef enum {
  17     MSP_IDLE,
  18     MSP_HEADER_START,
  19     MSP_HEADER_X,
  20
  21     MSP_HEADER_V2_NATIVE,
  22     MSP_PAYLOAD_V2_NATIVE,
  23     MSP_CHECKSUM_V2_NATIVE,
  24
  25     MSP_COMMAND_RECEIVED
  26 } mspState_e;
  27
  28 typedef enum {
  29     MSP_PACKET_UNKNOWN,
  30     MSP_PACKET_COMMAND,
  31     MSP_PACKET_RESPONSE
  32 } mspPacketType_e;
  33
  34 typedef struct __attribute__((packed)) {
  35     uint8_t  flags;
  36     uint16_t function;
  37     uint16_t payloadSize;
  38 } mspHeaderV2_t;
  39
  40 typedef struct {
  41     mspPacketType_e type;
  42     uint8_t         flags;
  43     uint16_t        function;
  44     uint16_t        payloadSize;
  45     uint8_t         payload[MSP_PORT_INBUF_SIZE];
  46     uint16_t        payloadReadIterator;
  47     bool            readError;
  48
  49     void reset()
  50     {
  51         type = MSP_PACKET_UNKNOWN;
  52         flags = 0;
  53         function = 0;
  54         payloadSize = 0;
  55         payloadReadIterator = 0;
  56         readError = false;
  57     }
  58
  59     void addByte(uint8_t b)
  60     {
  61         payload[payloadSize++] = b;
  62     }
  63
  64     void makeResponse()
  65     {
  66         type = MSP_PACKET_RESPONSE;
  67     }
  68
  69     void makeCommand()
  70     {
  71         type = MSP_PACKET_COMMAND;
  72     }
  73
  74     uint8_t readByte()
  75     {
  76         if (payloadReadIterator >= payloadSize) {
  77             // We are trying to read beyond the length of the payload
  78             readError = true;
  79             return 0;
  80         }
  81
  82         return payload[payloadReadIterator++];
  83     }
  84 } mspPacket_t;
  85
  86 /////////////////////////////////////////////////
  87
  88 class MSP
  89 {
  90 public:
  91     bool            processReceivedByte(uint8_t c);
  92     mspPacket_t*    getReceivedPacket();
  93     void            markPacketReceived();
  94     bool            sendPacket(mspPacket_t* packet, Stream* port);
  95
  96 private:
  97     mspState_e  m_inputState;
  98     uint16_t    m_offset;
  99     uint8_t     m_inputBuffer[MSP_PORT_INBUF_SIZE];
 100     mspPacket_t m_packet;
 101     uint8_t     m_crc;
 102 };