Diff: MPU9250.h

Revision:

1:61bf659e4a1f

Parent:

0:1a6e8ffa801b

Child:

2:bb20d5091065

--- a/MPU9250.h	Thu Apr 30 11:22:00 2015 +0000
+++ b/MPU9250.h	Sun Jun 07 16:48:13 2015 +0000
@@ -1,12 +1,18 @@
-/*Library modified by Josué Olmeda Castelló for imasD Tecnología
-
-*/
+/*****   
+        Library based on MPU-9250_Snowda library. It has been modified by Josué Olmeda Castelló for imasD Tecnología. It uses the 
+        mbed I2C class for comunications between the sensor and the master controller.
+        Methods related with data filtering have not been tested.
+        AD0 should be connected to GND.
+                                                            04/05/2015
+*****/
 
 #ifndef MPU9250_H
 #define MPU9250_H
  
 #include "mbed.h"
 #include "math.h"
+
+#define M_PI 3.14159265358979323846
  
 // See also MPU-9250 Register Map and Descriptions, Revision 4.0, RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in 
 // above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map
@@ -211,6 +217,7 @@
 int16_t tempCount;   // Stores the real internal chip temperature in degrees Celsius
 float temperature;
 float SelfTest[6];
+float orientation[1];
 
 int delt_t = 0; // used to control display output rate
 int count = 0;  // used to control display output rate
@@ -671,6 +678,43 @@
     }
 
 
+    
+    void getCompassOrientation(float * orient){ // Obtains the orientation of the device in degrees. 0 degrees North. 180 degrees South.
+        /*
+        Remember that it is the earth's rotational axis that defines the geographic north and south poles that we use for map references.
+        It turns out that there is a discrepancy of about 11.5 degrees between the geographic poles and the magnetic poles. The last is 
+        what the magnetometer will read. A value, called the declination angle, can be applied to the magnetic direction to correct for this.
+        On Valencia (Spain) this value is about 0 degrees.
+        */
+        float magn_x, magn_y;
+                
+        // First of all measure 3 axis magnetometer values (only X and Y axis is used):        
+        readMagData(magCount);  // Read the x/y/z adc values   
+                                // Calculate the magnetometer values in milliGauss
+                                // Include factory calibration per data sheet and user environmental corrections
+        if (I2Cstate == 0){ // no error on I2C            
+            I2Cstate = 1;
+            magn_x = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
+            magn_y = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
+        }
+        
+        // Now obtains the orientation value:
+        if (magn_y>0)
+            orient[0] = 90.0 - (float) ( atan(magn_x/magn_y)*180/M_PI );
+        else if (magn_y<0)
+            orient[0] = 270.0 - (float) ( atan(magn_x/magn_y)*180/M_PI );
+        else if (magn_y == 0){
+            if (magn_x<0)
+                orient[0] = 180.0;
+            else
+                orient[0] = 0.0;
+        }
+    }
+
+            
+        
+    
+    
 
 // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
 // (see http://www.x-io.co.uk/category/open-source/ for examples and more details)