Diff: main.cpp

Revision:

1:61bf659e4a1f

Parent:

0:1a6e8ffa801b

Child:

2:bb20d5091065

diff -r 1a6e8ffa801b -r 61bf659e4a1f main.cpp
--- 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]);