Wasp/Wasp.cpp at master · iqe/Wasp · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
#include "Arduino.h"
#include "Wasp.h"
#include "crc16_ccitt.h"

#define WASP_DEFAULT_TIMEOUT 100

#define WASP_SFLAG   0x5B // '['
#define WASP_EFLAG   0x5D // ']'
#define WASP_ESC     0x5C // '\'
#define WASP_ESC_XOR 0x20 // ' '

Wasp::Wasp(HardwareSerial *serial) {
  this->serial = serial;
  this->timeout = WASP_DEFAULT_TIMEOUT;
}

Wasp::Wasp(HardwareSerial *serial, int timeout) {
  this->serial = serial;
  this->timeout = timeout;
}

void Wasp::sendMessage(unsigned char *content, int length) {
  serial->write(WASP_SFLAG);
  writeContent(content, length);
  writeCrc(content, length);
  serial->write(WASP_EFLAG);
}

void Wasp::begin(long baudrate) {
  serial->begin(baudrate);
}

int Wasp::readMessage(unsigned char *buffer, int bufsize) {
  int c;
  int contentLength = 0;
  bool inMessage = false, afterEsc = false;
  unsigned long lastByteTimestamp = millis();

  while (contentLength <= bufsize) {
    if (millis() - lastByteTimestamp > timeout) {
      return -3;
    }

    if (serial->available() > 0) {
      c = serial->read();
      lastByteTimestamp = millis();

      switch (c) {
        case -1:
          return -1;
        case WASP_SFLAG:
          inMessage = true;
          contentLength = 0;
          break;
        case WASP_EFLAG:
          if (inMessage) {
            return checkCrc(buffer, contentLength);
          }
          inMessage = false;
          break;
        case WASP_ESC:
          afterEsc = true;
          break;
        default:
          if (inMessage) {
            if (afterEsc) {
              c ^= WASP_ESC_XOR;
              afterEsc = false;
            }
            buffer[contentLength++] = c;
          }
      }
    }
  }

  return -2;
}

void Wasp::writeContent(unsigned char *content, int length) {
  for (int i = 0; i < length; i++) {
    writeByte(content[i]);
  }
}

void Wasp::writeCrc(unsigned char *content, int length) {
  crc_t crc = crc16(content, length);
  unsigned char high = highByte(crc);
  unsigned char low = lowByte(crc);

  writeByte(low);
  writeByte(high);
}

void Wasp::writeByte(unsigned char b) {
  if (b == WASP_SFLAG || b == WASP_EFLAG || b == WASP_ESC) {
    serial->write(WASP_ESC);
    serial->write(b ^ WASP_ESC_XOR);
  } else {
    serial->write(b);
  }
}

int Wasp::checkCrc(unsigned char *content, int length) {
  if (length < 2) { // too small for crc
    return -1;
  }

  uint16_t low = content[length - 2];
  uint16_t high = content[length - 1];

  crc_t expectedCrc = (high << 8) | low;
  crc_t actualCrc = crc16(content, length - 2);

  return expectedCrc == actualCrc ? length - 2 : -1;
}

crc_t Wasp::crc16(unsigned char *content, int length) {
  crc_t crc = crc_init();
  crc = crc_update(crc, content, length);
  crc = crc_finalize(crc);

  return crc;
}