File content as of revision 4:ee51008e03e2:

#include "R1370.h"

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

void R1370::receiveByte()
{
    buffer[bufferPoint % R1370_BUFFER_SIZE] = getc();
 
    if(bufferPoint != 0xff) {
        ++bufferPoint;
    } else {
        bufferPoint = (255%R1370_BUFFER_SIZE)+1;
    }
 
    ++receivedBytes;
 
    if(receivedBytes >= R1370_BUFFER_SIZE) checkData();
}

void R1370::checkData()
{
    for(int i = 0; i < R1370_BUFFER_SIZE; i++) {
        if(buffer[i % R1370_BUFFER_SIZE] == R1370_HEADER0 && buffer[(i + 1) % R1370_BUFFER_SIZE] == R1370_HEADER1) {

            uint8_t checksum = 0x00;
            for(int j = 0; j < R1370_BUFFER_SIZE - 3; j++) {
                checksum += buffer[(i + 2 + j)% R1370_BUFFER_SIZE];

            }
            if(checksum == buffer[(i + R1370_BUFFER_SIZE - 1)% R1370_BUFFER_SIZE]) {
                for(int j = 0; j < R1370_BUFFER_SIZE - 3; j++) {
                    data[j] = buffer[(i + 2 + j) % R1370_BUFFER_SIZE];
                }
                receivedBytes = 0;

                assemble();
                return;
            }
        }
    }
}

void R1370::assemble()
{
        index = data[0];
        angle = (data[1] & 0xFF) | ((data[2] << 8) & 0xFF00);
        rate = (data[3] & 0xFF) | ((data[4] << 8) & 0xFF00);
        acc[0] = (data[5] & 0xFF) | ((data[6] << 8) & 0xFF00);
        acc[1] = (data[7] & 0xFF) | ((data[8] << 8) & 0xFF00);
        acc[2] = (data[9] & 0xFF) | ((data[10] << 8) & 0xFF00);
        reserved = data[11];
        upbit_ = data[1];
        downbit_ = data[2];
}

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

unsigned char R1370::upbit()
{
    return upbit_;
}

unsigned char R1370::downbit()
{
    return downbit_;
}