Revision 3:2e25f16c9fc3, committed 2017-08-10

Comitter:

ma123r

Date:

Thu Aug 10 11:13:37 2017 +0000

Parent:

2:3946867c4748

Commit message:

Just Testing Something

Changed in this revision

diff -r 3946867c4748 -r 2e25f16c9fc3 main.cpp
--- a/main.cpp	Thu Apr 20 15:30:45 2017 +0000
+++ b/main.cpp	Thu Aug 10 11:13:37 2017 +0000
@@ -1,33 +1,3 @@
-/* MPU9250 Basic Example Code
- by: Kris Winer
- date: April 1, 2014
- license: Beerware - Use this code however you'd like. If you 
- find it useful you can buy me a beer some time.
- 
- Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, 
- getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to 
- allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and 
- Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
- 
- SDA and SCL should have external pull-up resistors (to 3.3V).
- 10k resistors are on the EMSENSR-9250 breakout board.
- 
- Hardware setup:
- MPU9250 Breakout --------- Arduino
- VDD ---------------------- 3.3V
- VDDI --------------------- 3.3V
- SDA ----------------------- A4
- SCL ----------------------- A5
- GND ---------------------- GND
- 
- Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. 
- Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
- We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
- We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L /twi.h utility file.
- */
- 
-//#include "ST_F401_84MHZ.h" 
-//F401_init84 myinit(0);
 #include "mbed.h"
 #include "MPU9250.h"
 #include "MS5611I2C.h"
@@ -35,39 +5,41 @@
 float sum = 0;
 uint32_t sumCount = 0;
 char buffer[14];
-const uint16_t dly_ms = 1;    // delay for quat computing and printing
+const uint16_t dly_ms = 33;    // delay for o/p computing and printing
 
 MPU9250 mpu9250;
 
 //MS5611I2C ms5611(I2C_SDA, I2C_SCL, false);
-   
-Timer t;
 
+Timer t;
 Serial pc(USBTX, USBRX); // tx, rx
- 
+
+//HC-05
+Serial bt(D1,D0);
 
 /* MAIN */        
 int main()
-{
-  wait(3);
-  
+{  
+  //HC-05
+  bt.baud(9600);
+  //prints data on mobile
+  //bt.printf("Connection Established\r\n");
+  //print data on pc terminal
+  pc.printf("Connection Established\r\n");
   
   //pc.baud(115200);  
-  pc.baud(256000);
+  //pc.baud(256000);
 
   //Set up I2C
   i2c.frequency(400000);  // use fast (400 kHz) I2C  
-  
-  
+   
   //pc.printf("CPU SystemCoreClock is %d Hz\n", SystemCoreClock);   
-  
-  
+   
   //Print the Coefficients from the 
   //ms5611.printCoefficients();
   
   t.start();        
-  
-    
+   
   // Read the WHO_AM_I register, this is a good test of communication
   uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
   //pc.printf("I AM 0x%x\n", whoami); //pc.printf("I SHOULD BE 0x71\n");
@@ -95,9 +67,8 @@
    }
    else
    {
-        //pc.printf("Could not connect to MPU9250: \n");
-        //pc.printf("%#x \n",  whoami);
-  
+        pc.printf("Could not connect to MPU9250: \n");
+        pc.printf("%#x \n",  whoami);
         while(1) ; // Loop forever if communication doesn't happen
     }
 
@@ -109,12 +80,10 @@
     magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
 
 
-
 // main loop
  while(1) {  
   // 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
-
     mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
     // Now we'll calculate the accleration value into actual g's
     ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
@@ -134,7 +103,7 @@
     my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
     mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];   
   }
-   
+  
     Now = t.read_us();
     deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
     lastUpdate = Now;
@@ -142,7 +111,6 @@
     sum += deltat;
     sumCount++;
     
-    
     // Pass gyro rate as rad/s
     //mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
     mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
@@ -159,15 +127,25 @@
         yaw   -= 3.8f; // Declination in Cork, Ireland in Oct 2016
         roll  *= 180.0f / PI;
 
-        pc.printf("%.2f %.2f %.2f\n%.2f %.2f %.2f\n\n", 
+        pc.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n", 
         yaw,
         pitch, 
-        roll, 
+        roll,
         ax,
         ay,
-        az  
+        az
         );
-               
+        
+        // Send it to HC-06
+        bt.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n", 
+            yaw,
+            pitch, 
+            roll,
+            ax,
+            ay,
+            az
+            );            
+        
         myled= !myled;
         count = t.read_ms();