Revision 1:61bf659e4a1f, committed 2015-06-07

Comitter:

xosuuu

Date:

Sun Jun 07 16:48:13 2015 +0000

Parent:

0:1a6e8ffa801b

Child:

2:bb20d5091065

Commit message:

Sospecho que el I2C da tantos errores debido a que en el bucle infinito el programa hace continuas llamadas al I2C sin esperar. Por ello se ha querido probar incorporando dentro de bucle infinito waits de 5ms

Changed in this revision

--- 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)

--- a/main.cpp	Thu Apr 30 11:22:00 2015 +0000
+++ b/main.cpp	Sun Jun 07 16:48:13 2015 +0000
@@ -1,12 +1,23 @@
+/*****
+        Algorithm based on MPU-9250_Snowda program. It has been modified by Josué Olmeda Castelló for imasD Tecnología.
+                
+        This algorithm calibrates and reads data from accelerometer, gyroscope, magnetometer and the
+        internal temperature sensor. The lecture is made each time has a new mesured value (both gyro and accel data). 
+        A comunication with a computer is made using serial interface. The user can see the data measured with 1 second update rate.
+
+        This algorithm uses the STM32L152 development board and the MPU-9250 9-axis InvenSense movement sensor. The communication
+        between both devices is made through I2C serial interface.
+        
+        AD0 should be connected to GND.
+        
+                                                                04/05/2015
+*****/
+
 #include "mbed.h"
 #include "MPU9250.h"
 
-//-----------------------------------------------
-// Hyperterminal configuration:
-// 9600 bauds, 8-bit data, 1 stop bit, no parity
-//-----------------------------------------------
 
-Serial pc(USBTX, USBRX); // Default: 9600 bauds, 8-bit data, 1 stop bit, no parity
+Serial pc(USBTX, USBRX); // Huyperterminal default config: 9600 bauds, 8-bit data, 1 stop bit, no parity
 MPU9250 mpu9250;
 Timer t;
 //DigitalOut myled(LED1);
@@ -96,7 +107,8 @@
       
         // If intPin goes high, all data registers have new data
         if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
-                    
+            
+            wait(5);        
             mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
             // Now we'll calculate the accleration value into actual g's
             if (I2Cstate != 0) //error on I2C
@@ -104,10 +116,11 @@
             else{ // I2C read or write ok
                 I2Cstate = 1;
                 ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
-                ay = (float)accelCount[1]*aRes - accelBias[1];   
+                ay = (float)accelCount[1]*aRes - accelBias[1];
                 az = (float)accelCount[2]*aRes - accelBias[2];
             }   
             
+            wait(5);
             mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
             // Calculate the gyro value into actual degrees per second
             if (I2Cstate != 0) //error on I2C
@@ -118,7 +131,8 @@
                 gy = (float)gyroCount[1]*gRes - gyroBias[1]; 
                 gz = (float)gyroCount[2]*gRes - gyroBias[2];
             }
-  
+            
+            wait(5);
             mpu9250.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
@@ -129,7 +143,10 @@
                 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];
-            }   
+            }
+            
+            wait(5);            
+            mpu9250.getCompassOrientation(orientation);
         }
    
         Now = t.read_us();
@@ -169,6 +186,8 @@
             pc.printf("q1 = %f\n\r", q[1]);
             pc.printf("q2 = %f\n\r", q[2]);
             pc.printf("q3 = %f\n\r", q[3]);
+            
+            pc.printf("Compass orientation: %f\n", orientation[0]);