BE@R lab / Mbed 2 deprecated MPU9250AHRS

Basic program to obtain properly-scaled gyro, accelerometer, and magnetometer data from the MPU-9250 9-axis motion sensor and do 9 DoF sensor fusion using the open-source Madgwick and Mahony sensor fusion filters. Running on STM32F401RE Nucleo board at 84 MHz achieves sensor fusion filter update rates of ~5000 Hz.

Dependencies:   mbed

Fork of MPU9250AHRS by Kris Winer

Diff: main.cpp

Revision:
4:1e5db958fd1b
Parent:
3:3e04c1c03cab
Child:
5:d31487b34216
diff -r 3e04c1c03cab -r 1e5db958fd1b main.cpp
--- a/main.cpp	Fri Dec 18 12:59:56 2015 +0000
+++ b/main.cpp	Fri Dec 18 20:49:39 2015 +0000
@@ -186,10 +186,12 @@
 
             if(magCount[2]<zmin)
                 zmin = magCount[2];
+            if(magCount[2]>zmax)
+                zmax = magCount[2];                
+                /*
             if(mz>zmax)
                 zmax = mz;
-                
-
+                */
                 
             wait_ms(10);
     }
@@ -204,6 +206,14 @@
     magbias[1] = ((ymax-ymin)/2.0f - ymax);  // User environmental x-axis correction in milliGauss
     magbias[2] = ((zmax-zmin)/2.0f - zmax);  // User environmental x-axis correction in milliGauss
 
+    magbias[0] = -1.0;
+    magbias[1] = -1.0;
+    magbias[2] = -1.0;
+
+    magCalibration[0] = 2.0f / (xmax -xmin);
+    magCalibration[1] = 2.0f / (ymax -ymin);
+    magCalibration[2] = 2.0f / (zmax -zmin);
+
     //magbias[0] = (xmin-xmax)/2.0f;  // User environmental x-axis correction in milliGauss, should be automatically calculated
     //magbias[1] = (ymin-ymax)/2.0f;  // User environmental x-axis correction in milliGauss
     //magbias[2] = (zmin-zmax)/2.0f;  // User environmental x-axis correction in milliGauss
@@ -251,9 +261,13 @@
         */
            // pc.printf("FINISH scan\r\n\r\n");
 
-            mx = (float)magCount[0]*mRes*magCalibration[0] + magbias[0];  // get actual magnetometer value, this depends on scale being set
-            my = (float)magCount[1]*mRes*magCalibration[1] + magbias[1];
-            mz = (float)magCount[2]*mRes*magCalibration[2] + magbias[2];
+//            mx = (float)magCount[0]*mRes*magCalibration[0] + magbias[0];  // get actual magnetometer value, this depends on scale being set
+//            my = (float)magCount[1]*mRes*magCalibration[1] + magbias[1];
+//            mz = (float)magCount[2]*mRes*magCalibration[2] + magbias[2];
+
+            mx = ((float)magCount[0]-xmin)*magCalibration[0] + magbias[0];  // get actual magnetometer value, this depends on scale being set
+            my = ((float)magCount[1]-ymin)*magCalibration[1] + magbias[1];
+            mz = ((float)magCount[2]-zmin)*magCalibration[2] + magbias[2];
 
             // mx = (float)magCount[0]*1.499389499f - magbias[0];  // get actual magnetometer value, this depends on scale being set
             // my = (float)magCount[1]*1.499389499f - magbias[1];