Diff: main.cpp

Revision:

4:7fa9cfebb00c

Parent:

3:c1902ecb30a7

--- a/main.cpp	Thu Oct 06 17:35:14 2016 +0000
+++ b/main.cpp	Thu Oct 06 23:42:28 2016 +0000
@@ -8,7 +8,7 @@
 (1) hardcoded output - allow for an output object to change what interface data comes from
 (2) hardcoded input -allow for an input object to change what interface data goes to
 (3) robustness, sometimes needs to be rebooted.  maybe the serial interface times out?
-(4) optimization - lots of ineffeciencies in code, 
+(4) optimization - lots of ineffeciencies in code,
 (5) need to rerun some speed tests - find out how long it takes to get data from IMU, current sampling rate, if rate can be increased
 (6) have ?4? i2c ports, can maybe use all 4 to log faster?
 (7) need to run some speed tests on mux, find out how long it takes to switch a channel
@@ -163,7 +163,7 @@
     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;
@@ -188,6 +188,14 @@
     mpu9250[7].magbias[0] = -92.965;
     mpu9250[7].magbias[1] = 34.195;
     mpu9250[7].magbias[2] = -18.675;
+    */
+    mpu9250[0].magbias[0] = 153.895;
+    mpu9250[0].magbias[1] = 347.325;
+    mpu9250[0].magbias[2] = -227.89;
+    mpu9250[1].magbias[0] = 158.285;
+    mpu9250[1].magbias[1] = 314.005;
+    mpu9250[1].magbias[2] = -302.905;
+
 
     menu(pc);
 } // end of main
@@ -428,11 +436,12 @@
                 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;
-                MadgwickAHRSupdate(mpu9250[i].gx*PI/180.0f, mpu9250[i].gy*PI/180.0f, mpu9250[i].gz*PI/180.0f, mpu9250[i].ax, mpu9250[i].ay, mpu9250[i].az, mpu9250[i].my, mpu9250[i].mx, -1.0f*mpu9250[i].mz);
-                mpu9250[i].q[0] = q0;
-                mpu9250[i].q[1] = q1;
-                mpu9250[i].q[2] = q2;
-                mpu9250[i].q[3] = q3;
+                //mpu9250[i].MadgwickAHRSupdate(mpu9250[i].gx*PI/180.0f, mpu9250[i].gy*PI/180.0f, mpu9250[i].gz*PI/180.0f, mpu9250[i].ax, mpu9250[i].ay, mpu9250[i].az, mpu9250[i].my, mpu9250[i].mx, -1.0f*mpu9250[i].mz, mpu9250[i].q[0], mpu9250[i].q[1], mpu9250[i].q[2], mpu9250[i].q[3]);
+                mpu9250[i].Madgwickupdate();
+                //mpu9250[i].q[0] = q0;
+                //mpu9250[i].q[1] = q1;
+                //mpu9250[i].q[2] = q2;
+                //mpu9250[i].q[3] = q3;
 
                 delt_t = t.read_ms() - count;