File content as of revision 8:930369f241fb:

#include "OmniPosition.h"

OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
    RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD)
{
    attach(callback(this, &OmniPosition::receiveByte));
    thread.start(callback(this, &OmniPosition::assembleLoop));
}

void OmniPosition::receiveByte()
{
    buf.push_back(getc());
}

void OmniPosition::assemble()
{
    unsigned char checksum = 0;
    if(((unsigned char)(buf[2] + buf[3] + buf[4] + buf[5] + buf[6] + buf[7])) == buf[8]) {
        X = ((buf[2]<<8)|buf[3]) - 32768;
        Y = ((buf[4]<<8)|buf[5]) - 32768;
        theta = (buf[6] & 0xFF) | ((buf[7] << 8) & 0xFF00);
    }
}

void OmniPosition::assembleLoop()
{
    while(true) {
        if(buf.size() > OP_SERIAL_BUFFER_SIZE) {
            if(buf[0] == OP_HEADER_FIRST_BYTE && buf[1] == OP_HEADER_SECOND_BYTE) {
                assemble();
                buf.erase(buf.begin(), buf.begin() + (OP_SERIAL_BUFFER_SIZE - 1));
            } else {
                buf.erase(buf.begin());
            }
        }
    }
}

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

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

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

void OmniPosition::reset()
{
    resetSend = true;
    if(resetSend) {
        putc('R');
        resetSend = false;
    } else {
        //putc(0);
    }
}