File content as of revision 11:7028941a60c1:

#include "OmniPosition.h"

OmniPosition::OmniPosition(PinName serialTX, PinName serialRX)
    : RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD), debugled(PA_11) {
  attach(callback(this, &OmniPosition::receiveByte));
  X = 0;
  Y = 0;
  bufferSize = 9;
  bufferPoint = 0;
  receivedBytes = 0;
}

void OmniPosition::receiveByte() {
  buffer[bufferPoint % bufferSize] = getc();
  if (bufferPoint != 0xff) {
    ++bufferPoint;
  } else {
    bufferPoint = (255 % bufferSize) + 1;
  }

  ++receivedBytes;

  if (receivedBytes >= bufferSize) {
    checkData();
  }
}

void OmniPosition::checkData() {
  for (int i = 0; i < bufferSize; i++) {
    if (buffer[i % bufferSize] == OP_HEADER_FIRST_BYTE &&
        buffer[(i + 1) % bufferSize] == OP_HEADER_SECOND_BYTE) {
      uint8_t checksum = 0;
      for (int j = 0; j < bufferSize - 3; j++) {
        checksum += buffer[(i + 2 + j) % bufferSize];
      }
      if ((uint8_t)checksum == buffer[(i + bufferSize - 1) % bufferSize]) {
        debugled = !debugled;
        X = ((buffer[(i + 2 + 0) % bufferSize] << 8) |
             buffer[(i + 2 + 1) % bufferSize]) -
            32768;
        Y = ((buffer[(i + 2 + 2) % bufferSize] << 8) |
             buffer[(i + 2 + 3) % bufferSize]) -
            32768;
        theta = (buffer[(i + 2 + 4) % bufferSize] & 0xFF) |
                ((buffer[(i + 2 + 5) % bufferSize] << 8) & 0xFF00);
        receivedBytes = 0;
        return;
      }
    }
  }
}

int16_t OmniPosition::getX() { return X; }

int16_t OmniPosition::getY() { return Y; }

float OmniPosition::getTheta() {
  return (float)(theta / 100.0) * (M_PI / 180.0);
}

void OmniPosition::reset() { putc('R'); }