File content as of revision 9:222f7fcbd05a:

#include "OmniPosition.h"

OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
    serial(serialTX, serialRX)
{
    thread.start(callback(this, &OmniPosition::assembleLoop));
    serial.baud(115200);
    serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER);
    serial.startReceive(BUFFER_SIZE);
}

void OmniPosition::assembleLoop()
{
    while(true) {
        serial.getData(rxdata);
        for(int i = 0; i < TWO_BYTE_DATA - 1; i++)data[i] = ((rxdata[2*i]<<8)|rxdata[2*i+1]) - 32768;
        data[2] = (rxdata[2*2] & 0xFF) | ((rxdata[2*2+1] << 8) & 0xFF00);
        X = data[0];
        Y =  data[1];
        theta = data[2];
    }
}

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);
    }
}