vesc_uart_test.ino

#include "datatypes.h"
#include "crc.h"

#define PACKET_LENGTH_IDENTIFICATION_BYTE_SHORT 2
#define PACKET_LENGTH_IDENTIFICATION_BYTE_LONG 3
#define PACKET_TERMINATION_BYTE 3

#define PACKET_PAYLOAD_MAX_LENGTH 512

// Start byte + packet length bytes + payload + 2 bytes (CRC) + termination byte
#define PACKET_MAX_LENGTH (1 + 2 + PACKET_PAYLOAD_MAX_LENGTH + 2 + 1)

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  Serial.begin(115200);
  delay(1000);
}

void loop() {
  uint8_t payload[1] = {COMM_GET_VALUES};
  send_packet(payload, sizeof(payload));
  delay(1000);  
  if (received_bytes_count() > 0) {
    blink_ok();
  } else {
    blink_error();
  }
  delay(1000);
}

void blink_error() {
  // 3 short blinks  
  for (int i=0; i<3; i++) {
    digitalWrite(LED_BUILTIN, HIGH);
    delay(100);
    digitalWrite(LED_BUILTIN, LOW);
    delay(100);
  }  
}

void blink_ok() {
  // 1 long blink
  digitalWrite(LED_BUILTIN, HIGH);
  delay(1000);
  digitalWrite(LED_BUILTIN, LOW);  
}

void send_packet(uint8_t packetPayload[], uint16_t packetPayloadLength)
{
    uint8_t packet[PACKET_MAX_LENGTH] = {0};
    uint16_t index = 0;

    if (packetPayloadLength <= 256)
    {
        packet[index++] = PACKET_LENGTH_IDENTIFICATION_BYTE_SHORT;
        packet[index++] = packetPayloadLength;
    }
    else
    {
        packet[index++] = PACKET_LENGTH_IDENTIFICATION_BYTE_LONG;

        // The packet payload length is splitted up to 2 bytes
        packet[index++] = (uint8_t) (packetPayloadLength >> 8);
        packet[index++] = (uint8_t) (packetPayloadLength >> 0 & 0xFF);
    }

    // Copy payload to packet starting at index
    memcpy(&packet[index], packetPayload, packetPayloadLength);

    // Increment packet index by number of copied payload bytes
    index += packetPayloadLength;

    // CRC16 checksum (2 bytes)
    uint16_t crc16Checksum = crc16(packetPayload, packetPayloadLength);
    packet[index++] = (uint8_t) (crc16Checksum >> 8);
    packet[index++] = (uint8_t) (crc16Checksum >> 0 & 0xFF);

    // Termination byte
    packet[index++] = PACKET_TERMINATION_BYTE;

    // Write packet until index
    Serial.write(packet, index);
}

int received_bytes_count() {
  int count = 0;
  while (Serial.available()) {
    Serial.read();
  }
  return count;
}