File content as of revision 3:47676abdf529:

#include "OmniPosition.h"

OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
    RawSerial(serialTX, serialRX, DEFAULT_BAUD),
    readCounter(SERIAL_BUFFER_SIZE, 0),
    takeCounter(SERIAL_BUFFER_SIZE, 0),
    angle(0)
{
    buffer = new uint8_t[SERIAL_BUFFER_SIZE];
    data = new uint8_t[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;
    ++takeCounter;
    data[0] = buffer[(int)takeCounter];
    ++takeCounter;
    data[1] = buffer[(int)takeCounter];
    angleInt = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00);
    angle = angleInt/100.0;
    if(angle > 180){
        angle = angle - 655; 
        }
    
}

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

double OmniPosition::getAngle()
{
    return angle;
}


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