Speed testing IMUs

Dependencies:   MadgwickAHRS mbed

Fork of IMU_serial by Patrick Ciccone

Revision:
1:8715b7dd52ef
Parent:
0:80a695ae3cc3
Child:
3:c1902ecb30a7
--- a/main.cpp	Wed Sep 28 23:59:57 2016 +0000
+++ b/main.cpp	Wed Oct 05 23:37:42 2016 +0000
@@ -104,31 +104,7 @@
         while(1);
     }// end of no IMU's
 
-
-    /* //offsets from calibration
-        mpu9250[0].magbias[0] = -136.016697;
-        mpu9250[0].magbias[1] = 180.7041165;
-        mpu9250[0].magbias[2] = 531.2385865;
-        mpu9250[1].magbias[0] = -100.170666;
-        mpu9250[1].magbias[1] = 38.8694595;
-        mpu9250[1].magbias[2] = 162.8754425;
-        mpu9250[2].magbias[0] = -3.1089525;
-        mpu9250[2].magbias[1] = -143.011841;
-        mpu9250[2].magbias[2] = 612.3983765;
-        mpu9250[3].magbias[0] = 100.5001715;
-        mpu9250[3].magbias[1] = 244.6917115;
-        mpu9250[3].magbias[2] = 302.4588625;
-        mpu9250[4].magbias[0] = -175.6558455;
-        mpu9250[4].magbias[1] = 200.3966675;
-        mpu9250[4].magbias[2] = 573.4207155;
-        mpu9250[5].magbias[0] = -38.8619135;
-        mpu9250[5].magbias[1] = -484.9966735;
-        mpu9250[5].magbias[2] = 690.353607;
-        mpu9250[6].magbias[0] = -83.4814225;
-        mpu9250[6].magbias[1] = 131.300442;
-        mpu9250[6].magbias[2] = 200.63311;
-    */
-
+    /*
     mpu9250[0].magbias[0] = -259.875;
     mpu9250[0].magbias[1] = 306.65;
     mpu9250[0].magbias[2] = 334.755;
@@ -150,7 +126,58 @@
     mpu9250[6].magbias[0] = -97.02;
     mpu9250[6].magbias[1] = 278.445;
     mpu9250[6].magbias[2] = 23.985;
+    */
+    /*
+    mpu9250[0].magbias[0] = 528.025;
+    mpu9250[0].magbias[1] = -77.24;
+    mpu9250[0].magbias[2] = -364.87;
+    mpu9250[1].magbias[0] = 210.35;
+    mpu9250[1].magbias[1] = -60.52;
+    mpu9250[1].magbias[2] = 232.01;
+    mpu9250[2].magbias[0] = 226.175;
+    mpu9250[2].magbias[1] = 5.445;
+    mpu9250[2].magbias[2] = -451.24;
+    mpu9250[3].magbias[0] = 97.445;
+    mpu9250[3].magbias[1] = -185.585;
+    mpu9250[3].magbias[2] = -376.69;
+    mpu9250[4].magbias[0] = 428.44;
+    mpu9250[4].magbias[1] = -364.695;
+    mpu9250[4].magbias[2] = -550.065;
+    mpu9250[5].magbias[0] = 308.285;
+    mpu9250[5].magbias[1] = 668.055;
+    mpu9250[5].magbias[2] = -491.725;
+    mpu9250[6].magbias[0] = 83.03;
+    mpu9250[6].magbias[1] = 20.205;
+    mpu9250[6].magbias[2] = -151.41;
+    mpu9250[7].magbias[0] = -103.81;
+    mpu9250[7].magbias[1] = 35.75;
+    mpu9250[7].magbias[2] = -94.875;
+    */
 
+    mpu9250[0].magbias[0] = 278.785;
+    mpu9250[0].magbias[1] = 205.86;
+    mpu9250[0].magbias[2] = 60.225;
+    mpu9250[1].magbias[0] = 102.595;
+    mpu9250[1].magbias[1] = 87.055;
+    mpu9250[1].magbias[2] = 163.405;
+    mpu9250[2].magbias[0] = 183.43;
+    mpu9250[2].magbias[1] = -4.665;
+    mpu9250[2].magbias[2] = -60.715;
+    mpu9250[3].magbias[0] = 190.275;
+    mpu9250[3].magbias[1] = 199.215;
+    mpu9250[3].magbias[2] = -275.505;
+    mpu9250[4].magbias[0] = 240.075;
+    mpu9250[4].magbias[1] = -26.0;
+    mpu9250[4].magbias[2] = -209.325;
+    mpu9250[5].magbias[0] = 270.815;
+    mpu9250[5].magbias[1] = 361.685;
+    mpu9250[5].magbias[2] = -20.705;
+    mpu9250[6].magbias[0] = -10.88;
+    mpu9250[6].magbias[1] = 288.51;
+    mpu9250[6].magbias[2] = -38.98;
+    mpu9250[7].magbias[0] = -92.965;
+    mpu9250[7].magbias[1] = 34.195;
+    mpu9250[7].magbias[2] = -18.675;
 
     menu(pc);
 } // end of main
@@ -386,8 +413,8 @@
                 // On interrupt, check if data ready interrupt
                 uint32_t checksum = 0;
                 mpu9250[i].readimu();
-                
-                
+
+
                 mpu9250[i].Now = t.read_us();
                 mpu9250[i].deltat = (float)((mpu9250[i].Now - mpu9250[i].lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
                 mpu9250[i].lastUpdate = mpu9250[i].Now;
@@ -409,9 +436,9 @@
                 mpu9250[i].pitch *= 180.0f / PI;
                 mpu9250[i].yaw   *= 180.0f / PI;
                 mpu9250[i].roll  *= 180.0f / PI;
-                
-                
-                
+
+
+
                 transmit(pc);
                 Data[1] = 1000*mpu9250[i].ax;
                 Data[2] = 1000*mpu9250[i].ay;
@@ -422,7 +449,7 @@
                 Data[7] = mpu9250[i].mx;
                 Data[8] = mpu9250[i].my;
                 Data[9] = mpu9250[i].mz;
-                
+
                 Data[10] = mpu9250[i].temperature;
                 Data[11] = mpu9250[i].q[0];
                 Data[12] = mpu9250[i].q[1];
@@ -432,7 +459,7 @@
                 Data[16] = mpu9250[i].pitch;
                 Data[17] = mpu9250[i].roll;
                 Data[18] = mpu9250[i].checksum;
-                
+
                 Data[19] = i;
 
                 count = t.read_ms();