File content as of revision 0:243364135087:

#include "R1370.h"

R1370::R1370(PinName tx, PinName rx) : RawSerial(tx, rx, 115200)
{
    attach(callback(this, &R1370::receiveByte));
    thread.start(callback(this, &R1370::assembleLoop));
}

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

void R1370::assemble()
{
    char checksum = 0;
    for(int i = 2; i <= 13; i++) {
        checksum += buf[i];
    }
    if(checksum == buf[14]) {
        index = buf[2];
        angle = (buf[3] & 0xFF) | ((buf[4] << 8) & 0xFF00);
        rate = (buf[5] & 0xFF) | ((buf[6] << 8) & 0xFF00);
        acc[0] = (buf[7] & 0xFF) | ((buf[8] << 8) & 0xFF00);
        acc[1] = (buf[9] & 0xFF) | ((buf[10] << 8) & 0xFF00);
        acc[2] = (buf[11] & 0xFF) | ((buf[12] << 8) & 0xFF00);
        reserved = buf[13];
    }
}

void R1370::assembleLoop()
{
    while(true) {
        if(buf.size() > R1370_BUFFER_SIZE) {
            if(buf[0] == R1370_HEADER0 && buf[1] == R1370_HEADER1) {
                assemble();
                buf.erase(buf.begin(), buf.begin() + (R1370_BUFFER_SIZE - 1));
            } else {
                buf.erase(buf.begin());
            }
            assemble();
        }
    }
}

float R1370::getAngle()
{
    return (float)(angle / 100.0);
}

float R1370::getRate()
{
    return (float)(rate / 100.0);
}

int16_t R1370::getAcc(char l)
{
    if(l == 'x' || l == 'X') {
        return acc[0];
    } else if(l == 'y' || l == 'Y') {
        return acc[1];
    } else if(l == 'z' || l == 'Z') {
        return acc[2];
    } else {
        return 0;
    }
}

int16_t R1370::getAcc(int i)
{
    if(i >= 0 && i < 3) {
        return acc[i];
    } else {
        return 0;
    }
}

int16_t R1370::getAccX()
{
    return acc[0];
}

int16_t R1370::getAccY()
{
    return acc[1];
}

int16_t R1370::getAccZ()
{
    return acc[2];
}