Diff: main.cpp

Revision:

0:5155f500559c

diff -r 000000000000 -r 5155f500559c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jan 02 05:44:11 2013 +0000
@@ -0,0 +1,52 @@
+#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);
+    
+}