File content as of revision 1:0229fc98a26f:

#include "OmniPosition.h"

OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
    RawSerial(serialTX, serialRX, DEFAULT_BAUD),
    readCounter(SERIAL_BUFFER_SIZE, 0),
    takeCounter(SERIAL_BUFFER_SIZE, 0)
{
    buffer = new char[SERIAL_BUFFER_SIZE];
    attach(callback(this, &OmniPosition::readData));
    ticker.attach(callback(this, &OmniPosition::assemble), RECEIVE_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
}