File content as of revision 0:5155f500559c:

#include "mbed.h"       

        
 
Serial pc(USBTX, USBRX); // tx, rx
const int addr = 0x18; // define the I2C Address of the SpacepointRM

union Quat_ci {
    short i[4];
    char d[8];
    } Quat;

    
    
I2C i2c(p28, p27);        // sda, scl
 
int main() {
pc.printf("Working... \n");
    char cmd[1];
    cmd[0] = (char) 0x31;
    i2c.write(addr,cmd,1);
    i2c.read(addr,Quat.d,8);
    char temp;
    
    temp = Quat.d[0]; Quat.d[0] = Quat.d[1];Quat.d[1]=temp;
    temp = Quat.d[2]; Quat.d[2] = Quat.d[3];Quat.d[3]=temp;
    temp = Quat.d[4]; Quat.d[4] = Quat.d[5];Quat.d[5]=temp;
    temp = Quat.d[6]; Quat.d[6] = Quat.d[7];Quat.d[7]=temp;
    
    float qx = (float)(Quat.i[0] - 32768)/32768;
    float qy = (float)(Quat.i[1] - 32768)/32768;
    float qz = (float)(Quat.i[2] - 32768)/32768;
    float qw = (float)(Quat.i[3] - 32768)/32768;
    
    //pc.printf("%d,%d,%d,%d \n",Quat.i[0],Quat.i[1],Quat.i[2],Quat.i[3]);
    pc.printf("%f,%f,%f,%f \n",qx,qy,qz,qw);
    
    float sqx = pow(qx , 2);
    float sqy = pow(qx , 2);
    float sqz = pow(qx , 2);
    float sqw = pow(qx , 2);
    
    
    float HEADING = atan2((2*((qx*qw)+(qy*qz))),(1-(2*(sqz+sqw))));

    float PITCH = asin((2*((qx*qz) - (qw*qy))) );

    float ROLL = atan2( (2*( (qx*qy) + (qz*qw) )) , (1-2*(sqy+sqz)) );
    
    pc.printf("Heading: %f \n Pitch: %f \n Roll: %f \n",HEADING,PITCH,ROLL);
    
}