Andrew Miller / Mbed 2 deprecated scoutTest

Quaternion calculations on data from PNI's SpacePoint Scout

Dependencies:   mbed

main.cpp@0:5155f500559c, 2013-01-02 (annotated)

Committer:
awmiller
Date:
Wed Jan 02 05:44:11 2013 +0000
Revision:
0:5155f500559c
andrew's space point scout test script

Who changed what in which revision?

UserRevisionLine numberNew contents of line
awmiller 0:5155f500559c 1 #include "mbed.h"
awmiller 0:5155f500559c 2
awmiller 0:5155f500559c 3
awmiller 0:5155f500559c 4
awmiller 0:5155f500559c 5 Serial pc(USBTX, USBRX); // tx, rx
awmiller 0:5155f500559c 6 const int addr = 0x18; // define the I2C Address of the SpacepointRM
awmiller 0:5155f500559c 7
awmiller 0:5155f500559c 8 union Quat_ci {
awmiller 0:5155f500559c 9 short i[4];
awmiller 0:5155f500559c 10 char d[8];
awmiller 0:5155f500559c 11 } Quat;
awmiller 0:5155f500559c 12
awmiller 0:5155f500559c 13
awmiller 0:5155f500559c 14
awmiller 0:5155f500559c 15 I2C i2c(p28, p27); // sda, scl
awmiller 0:5155f500559c 16
awmiller 0:5155f500559c 17 int main() {
awmiller 0:5155f500559c 18 pc.printf("Working... \n");
awmiller 0:5155f500559c 19 char cmd[1];
awmiller 0:5155f500559c 20 cmd[0] = (char) 0x31;
awmiller 0:5155f500559c 21 i2c.write(addr,cmd,1);
awmiller 0:5155f500559c 22 i2c.read(addr,Quat.d,8);
awmiller 0:5155f500559c 23 char temp;
awmiller 0:5155f500559c 24
awmiller 0:5155f500559c 25 temp = Quat.d[0]; Quat.d[0] = Quat.d[1];Quat.d[1]=temp;
awmiller 0:5155f500559c 26 temp = Quat.d[2]; Quat.d[2] = Quat.d[3];Quat.d[3]=temp;
awmiller 0:5155f500559c 27 temp = Quat.d[4]; Quat.d[4] = Quat.d[5];Quat.d[5]=temp;
awmiller 0:5155f500559c 28 temp = Quat.d[6]; Quat.d[6] = Quat.d[7];Quat.d[7]=temp;
awmiller 0:5155f500559c 29
awmiller 0:5155f500559c 30 float qx = (float)(Quat.i[0] - 32768)/32768;
awmiller 0:5155f500559c 31 float qy = (float)(Quat.i[1] - 32768)/32768;
awmiller 0:5155f500559c 32 float qz = (float)(Quat.i[2] - 32768)/32768;
awmiller 0:5155f500559c 33 float qw = (float)(Quat.i[3] - 32768)/32768;
awmiller 0:5155f500559c 34
awmiller 0:5155f500559c 35 //pc.printf("%d,%d,%d,%d \n",Quat.i[0],Quat.i[1],Quat.i[2],Quat.i[3]);
awmiller 0:5155f500559c 36 pc.printf("%f,%f,%f,%f \n",qx,qy,qz,qw);
awmiller 0:5155f500559c 37
awmiller 0:5155f500559c 38 float sqx = pow(qx , 2);
awmiller 0:5155f500559c 39 float sqy = pow(qx , 2);
awmiller 0:5155f500559c 40 float sqz = pow(qx , 2);
awmiller 0:5155f500559c 41 float sqw = pow(qx , 2);
awmiller 0:5155f500559c 42
awmiller 0:5155f500559c 43
awmiller 0:5155f500559c 44 float HEADING = atan2((2*((qx*qw)+(qy*qz))),(1-(2*(sqz+sqw))));
awmiller 0:5155f500559c 45
awmiller 0:5155f500559c 46 float PITCH = asin((2*((qx*qz) - (qw*qy))) );
awmiller 0:5155f500559c 47
awmiller 0:5155f500559c 48 float ROLL = atan2( (2*( (qx*qy) + (qz*qw) )) , (1-2*(sqy+sqz)) );
awmiller 0:5155f500559c 49
awmiller 0:5155f500559c 50 pc.printf("Heading: %f \n Pitch: %f \n Roll: %f \n",HEADING,PITCH,ROLL);
awmiller 0:5155f500559c 51
awmiller 0:5155f500559c 52 }