Diff: imu.cpp

Revision:

3:f04d3d10d518

Parent:

2:d8b182fbe018

Child:

4:8f63393d49fb

--- a/imu.cpp	Fri Nov 04 22:24:02 2011 +0000
+++ b/imu.cpp	Sat Nov 05 01:02:59 2011 +0000
@@ -1,6 +1,11 @@
 #include "mbed.h"
 #include "imu.h"
 
+/* 
+ * IMU data averages, flat surface
+ * 20.138444, -15.096182, 232.355290, -29.691927, -14.679685, 45.097056, -75.690469, 0.737977, -159.505392
+ */
+
 IMU::IMU(int baud, PinName tx, PinName rx, Serial* pc){
     p_pc = pc;
     p_pc->printf("Initializing IMU...\n");
@@ -11,6 +16,7 @@
     i_IMU = 0;
     accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0;
     p_device->putc('6'); //Tell the IMU to start sending binary data, if it isn't already
+    newData = false;
 }
 
 IMU::~IMU() {
@@ -50,12 +56,15 @@
         }
         // Something went wrong.
         else if (i_IMU > 21) {
-            //p_pc->printf("\t\t\tIMU error.\n\r");
+            for (int i = 0; i < 20; i++) {
+                p_pc->printf("\n\r");
+            }
+            //p_pc->printf("\t\t\t\t\t\t\t\t\t**\n\r");
             i_IMU = 21;
         }
 
         // End of the set of data. Parse the buffer
-        else if (i_IMU == 21 /*&& bufIMU[0] == '$' && bufIMU[20] == '\n' data == '\n' && i_IMU == 19*/) {
+        else if (i_IMU == 21 && bufIMU[0] == '$' /*&& bufIMU[20] == '\n' data == '\n' && i_IMU == 19*/) {
             parse();
         }
 
@@ -66,13 +75,13 @@
     }
 }
 
-//So negative numbers that are transferred are in twos complement form
+// So negative numbers that are transferred are in twos complement form
 // and the compiler seems to like to use things created by bitwise operators
 // in unsigned form so all of the bits are switched and we switch it back and center
 // it around 512 which we are using as out zero value
 inline void IMU::makeCorrect(short* i) {
-    if ((*i)>>15) *i = 512-(~(*i));
-    else *i = 512+*i;
+    if ((*i)>>15) *i = 0-(~(*i));
+    else *i = 0+*i;
 }
 
 /*
@@ -81,7 +90,7 @@
 }*/
 
 void IMU::parse() {
-    p_pc->printf("Parsing\n\r");
+    //p_pc->printf("Parsing\n\r");
 
     //This should be easier to understand than bitshifts. bufIMU is a pointer (arrays are pointers).
     //We offset it to the start of the desired value. We then typecast bufIMU into a short (16 bit).
@@ -130,8 +139,9 @@
     makeCorrect(&magX);
     makeCorrect(&magY);
     makeCorrect(&magZ);
-
-
+    
+    newData = true;
+    
     //PC.printf("Data: %d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ);
 
     //accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0;
@@ -164,11 +174,44 @@
     imu.attach(&readIMU);
     //imu.p_device->attach(&readIMU);
     imu.putc('6');
+    
+    PC.printf("Waiting for data.");
+    
+    // Calibration variables
+    long long sumAccX = 0, sumAccY = 0, sumAccZ = 0;
+    long long sumGyrX = 0, sumGyrY = 0, sumGyrZ = 0;
+    long long sumMagX = 0, sumMagY = 0, sumMagZ = 0;
+    int num = 0;
+    
     while (true){
         if (imu.readable) {
             //PC.printf("fun");
             imu.getData();
-            PC.printf("Data: %d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", imu.accX, imu.accY, imu.accZ, imu.gyrX, imu.gyrY, imu.gyrZ, imu.magX, imu.magY, imu.magZ);
+            if (imu.newData) {
+                PC.printf("Data: %d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", imu.accX, imu.accY, imu.accZ, imu.gyrX, imu.gyrY, imu.gyrZ, imu.magX, imu.magY, imu.magZ);
+                
+                sumAccX += imu.accX;
+                sumAccY += imu.accY;
+                sumAccZ += imu.accZ;
+                
+                sumGyrX += imu.gyrX;
+                sumGyrY += imu.gyrY;
+                sumGyrZ += imu.gyrZ;
+                
+                sumMagX += imu.magX;
+                sumMagY += imu.magY;
+                sumMagZ += imu.magZ;
+                
+                num++;
+                
+                PC.printf("Averages: %f, %f, %f, %f, %f, %f, %f, %f, %f\n", 
+                    sumAccX/(double)num, sumAccY/(double)num, sumAccZ/(double)num,
+                    sumGyrX/(double)num, sumGyrY/(double)num, sumGyrZ/(double)num, 
+                    sumMagX/(double)num, sumMagY/(double)num, sumMagZ/(double)num
+                );
+                
+                imu.newData = false;
+            }
         }
     }
 }
\ No newline at end of file