File content as of revision 2:edd33d3ad0fd:

#include "OmniPosition.h"

OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
    RawSerial(serialTX, serialRX, DEFAULT_BAUD),
    readCounter(SERIAL_BUFFER_SIZE, 0),
    takeCounter(SERIAL_BUFFER_SIZE, 0),
    X(0),
    Y(0)
{
    buffer = new char[SERIAL_BUFFER_SIZE];
    data = new char[2];
    attach(callback(this, &OmniPosition::readData));
    assembleTicker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ);
    sendTicker.attach(callback(this, &OmniPosition::send), SEND_FREQ);
}

void OmniPosition::readData()
{
    buffer[(int)readCounter] = getc();
    ++readCounter;
}

void OmniPosition::assemble()
{
    //Find header
    headerCheck = false;
    headerPoint = 0xff;

    for(int i = 0; i < SERIAL_BUFFER_SIZE; i++) {
        if(buffer[i] == HEADER_FIRST_BYTE) {
            takeCounter = i;
            ++takeCounter;
            if(buffer[(int)takeCounter] == HEADER_SECOND_BYTE) {
                headerCheck = true;
                headerPoint = i;
            }
        }
    }
    if(headerPoint == 0xff) {
        return;
    }

    //assemble
    takeCounter = headerPoint;  //firstheader
    ++takeCounter;  //secondheader

    ++takeCounter;
    data[0] = buffer[(int)takeCounter];
    ++takeCounter;
    data[1] = buffer[(int)takeCounter];
    X = ((data[0]<<8)|data[1]) - 32768;

    ++takeCounter;
    data[0] = buffer[(int)takeCounter];
    ++takeCounter;
    data[1] = buffer[(int)takeCounter];
    Y = ((data[0]<<8)|data[1]) - 32768;
}

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

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

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

void OmniPosition::reset()
{
    resetSend = true;
}