Diff: MPU9150.h

Revision:

9:b7062c55d36c

Parent:

0:39935bb3c1a1

diff -r 66640587e61d -r b7062c55d36c MPU9150.h
--- a/MPU9150.h	Tue Dec 01 02:03:48 2015 +0000
+++ b/MPU9150.h	Tue Dec 01 04:42:30 2015 +0000
@@ -2,6 +2,16 @@
 #define MPU9150_H
  
 #include "mbed.h"
+
+//#define DEBUG
+
+#ifdef DEBUG
+#include "USBSerial.h"                       // To use USB virtual serial, a driver is needed, check http://mbed.org/handbook/USBSerial
+#define LOG(args...)    pc.printf(args)
+USBSerial pc;
+#else
+#define LOG(args...)
+#endif
  
 // Define registers per MPU6050, Register Map and Descriptions, Rev 4.2, 08/19/2013 6 DOF Motion sensor fusion device
 // Invensense Inc., www.invensense.com
@@ -183,7 +193,7 @@
 int16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output
 int16_t magCount[3];    // Stores the 16-bit signed magnetometer sensor output
 float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0};  // Factory mag calibration and mag bias
-float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer
+float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}; ; // Bias corrections for gyro, accelerometer and mag
 float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 
 int16_t tempCount;   // Stores the real internal chip temperature in degrees Celsius
 float temperature;
@@ -337,6 +347,45 @@
   destination[2] =  (float)(rawData[2] - 128)/256.0f + 1.0f; 
 }
 
+void magcalMPU9150(float * dest1) 
+{
+  uint16_t ii = 0, sample_count = 0;
+  int32_t mag_bias[3] = {0, 0, 0};
+  int16_t mag_max[3] = {0, 0, 0}, mag_min[3] = {0, 0, 0}, mag_temp[3] = {0, 0, 0};
+ 
+  LOG("Mag Calibration: Wave device in a figure eight until done!");
+  wait(4);
+  
+   sample_count = 64;
+   for(ii = 0; ii < sample_count; ii++) {
+    readMagData(mag_temp);  // Read the mag data   
+    for (int jj = 0; jj < 3; jj++) {
+      if (ii == 0) {
+        mag_max[jj] = mag_temp[jj]; // Offsets may be large enough that mag_temp[i] may not be bipolar! 
+        mag_min[jj] = mag_temp[jj]; // This prevents max or min being pinned to 0 if the values are unipolar...
+      } else {
+        if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
+        if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
+      }
+    }
+    wait_ms(135);  // at 8 Hz ODR, new mag data is available every 125 ms
+   }
+
+//    Serial.println("mag x min/max:"); Serial.println(mag_max[0]); Serial.println(mag_min[0]);
+//    Serial.println("mag y min/max:"); Serial.println(mag_max[1]); Serial.println(mag_min[1]);
+//    Serial.println("mag z min/max:"); Serial.println(mag_max[2]); Serial.println(mag_min[2]);
+
+    mag_bias[0]  = (mag_max[0] + mag_min[0])/2;  // get average x mag bias in counts
+    mag_bias[1]  = (mag_max[1] + mag_min[1])/2;  // get average y mag bias in counts
+    mag_bias[2]  = (mag_max[2] + mag_min[2])/2;  // get average z mag bias in counts
+    
+    dest1[0] = (float) mag_bias[0]*mRes*magCalibration[0];  // save mag biases in G for main program
+    dest1[1] = (float) mag_bias[1]*mRes*magCalibration[1];   
+    dest1[2] = (float) mag_bias[2]*mRes*magCalibration[2];          
+
+   LOG("Mag Calibration done!");
+}
+
 int16_t readTempData()
 {
   uint8_t rawData[2];  // x/y/z gyro register data stored here