File content as of revision 0:d0d40a6fe11c:

#include "Arduimu.h"
#include <cstring>

#define preamble  "DIYd"

typedef union {
    int16_t integerVal;
    char bytes[2];
} value_t;


Arduimu::Arduimu(PinName tx, PinName rx):imu(tx,rx)
{
    imu.baud(38400);
    imu.attach(this,&Arduimu::receive);
}

float Arduimu::getRoll()
{
    return roll;
}
float Arduimu::getPitch()
{
    return pitch;
}
float Arduimu::getYaw()
{
    return yaw;
}
void Arduimu::getOrientation(float& roll, float& pitch, float& yaw)
{
    roll = this->roll;
    pitch = this->pitch;
    yaw = this->yaw;
}

void Arduimu::receive()
{
    char chk_a = 0;
    char chk_b = 0;
    value_t temp;
    temp.integerVal = 0;
    //The message is 14 bytes with a preamble of 'DIYd' + 0x06 + 0x02
    char msgBuf[14];
    //Wait for next message
    while(imu.getc() != 'D');
    __disable_irq();    // Disable Interrupts so we dont loose data
    msgBuf[0] = 'D';
    msgBuf[1] = imu.getc(); //I
    msgBuf[2] = imu.getc(); //Y
    msgBuf[3] = imu.getc(); //d
    msgBuf[4] = imu.getc(); //0x06
    msgBuf[5] = imu.getc(); //0x02

    if(strcmp(msgBuf,preamble) && msgBuf[4] == 0x06 && msgBuf[5] == 0x02) {
        //we have a valid message
        //Fill in the rest of the values
        for(int i = 6; i < 14; i++) {
            msgBuf[i] = imu.getc();
        }
        __enable_irq();     // Enable Interrupts
        for(int i = 6; i < 12; i++) {
            chk_a+=msgBuf[i];
            chk_b+=chk_a;
        }

        temp.bytes[0] = msgBuf[6];
        temp.bytes[1] = msgBuf[7];
        roll = (float) temp.integerVal / 100.0;

        temp.bytes[0] = msgBuf[8];
        temp.bytes[1] = msgBuf[9];
        pitch = (float) temp.integerVal / 100.0;

        temp.bytes[0] = msgBuf[10];
        temp.bytes[1] = msgBuf[11];
        yaw = (float) temp.integerVal / 100.0;
    } else
        __enable_irq();     // Enable Interrupts
}

void Arduimu::putc(char c)
{
    imu.putc(c);
}