hi robert

stuff.cpp

#define COMMAND_BUFFER_SIZE (200)

struct CMDPacket {
  char command[COMMAND_BUFFER_SIZE];
};

#include <cstring> // memcpy

// easy to change which struct is being encoded/decoded
#define CODE_T CMDPacket

// can change to char, short, int, long etc for different reliability/length tradeoffs
#define CSUM_T int

// struct + checksum
constexpr int get_csum_packet_size() {
  return sizeof(CODE_T) + sizeof(CSUM_T);
}

// encodes payload+checksum into buffer. buffer should be get_csum_packet_size() bytes long.
void csum_encode(const CODE_T &data, char *buffer) {
  memcpy(buffer, &data, sizeof(CODE_T));
  buffer += sizeof(CODE_T);

  CSUM_T csum = 0;
  const unsigned char *data_bytes = (const unsigned char *)&data;
  for (size_t i = 0; i < sizeof(CODE_T); i++) {
    csum += data_bytes[i];
  }

  memcpy(buffer, &csum, sizeof(CSUM_T));
}

// decodes payload+checksum from buffer. buffer should be get_csum_packet_size() bytes long.
bool csum_decode(const char *buffer, CODE_T &data) {
  memcpy(&data, buffer, sizeof(CODE_T));
  buffer += sizeof(CODE_T);

  CSUM_T received_csum;
  memcpy(&received_csum, buffer, sizeof(CSUM_T));

  CSUM_T csum = 0;
  const unsigned char *data_bytes = (const unsigned char *)&data;
  for (size_t i = 0; i < sizeof(CODE_T); i++) {
    csum += data_bytes[i];
  }

  return received_csum == csum;
}

constexpr int get_max_delimited_size() {
  return 2 * get_csum_packet_size() + 2; // worst case everything escaped + delimiters
}

char delimited[get_max_delimited_size()];

#define DS 'S'
#define DE 'E'
#define E '\\'
#define R (0b01011010)

// escapes start/end/esc inside the raw packet and wraps with start and end.
// writes to char array delimited. returns number of bytes to send.
size_t escape_delimit(const char *buffer) {
  const size_t in_len = (size_t)get_csum_packet_size();
  size_t out = 0;

  delimited[out++] = DS;

  for (size_t i = 0; i < in_len; i++) {
    char b = buffer[i];

    if (b == DS || b == DE || b == E) {
      delimited[out++] = E;
      delimited[out++] = b ^ R;
    } else {
      delimited[out++] = b;
    }
  }

  delimited[out++] = DE;

  return out;
}