Diff: main.cpp

Revision:

0:f463e270d211

diff -r 000000000000 -r f463e270d211 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 19 21:12:50 2018 +0000
@@ -0,0 +1,136 @@
+/* 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 "N5110.h"
+
+// Using NOKIA 5110 monochrome 84 x 48 pixel display
+// pin 9 - Serial clock out (SCLK)
+// pin 8 - Serial data out (DIN)
+// pin 7 - Data/Command select (D/C)
+// pin 5 - LCD chip select (CS)
+// pin 6 - LCD reset (RST)
+//Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
+
+float sum = 0;
+uint32_t sumCount = 0;
+int count = 0;
+float yaw = 0;
+float drift;
+MPU9250 imu(PTE25, PTE24);         // SDA, SCL 
+   
+   Timer t;
+
+   Serial pc(USBTX, USBRX); // tx, rx
+
+   //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
+  // N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
+   
+
+        
+int main()
+{
+  pc.baud(9600);  
+  pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);   
+    
+  // Read the WHO_AM_I register, this is a good test of communication
+  uint8_t whoami = imu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
+  pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
+  
+  if (whoami == 0x71) // WHO_AM_I should always be 0x68
+  {  
+    pc.printf("MPU9250 is online...\n\r");
+    
+    wait(1);
+    
+    imu.resetMPU9250(); // Reset registers to default in preparation for device calibration
+    imu.calibrateMPU9250(imu.gyroBias, imu.accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
+    imu.initMPU9250(); 
+    imu.initAK8963(imu.magCalibration);
+    wait(2);
+   }
+   else
+   {
+    pc.printf("Could not connect to MPU9250: \n\r");
+    pc.printf("%#x \n",  whoami);
+ 
+    while(1) ; // Loop forever if communication doesn't happen
+    }
+
+    imu.getAres(); // Get accelerometer sensitivity
+    imu.getGres(); // Get gyro sensitivity
+    imu.getMres(); // Get magnetometer sensitivity
+    pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/imu.aRes);
+    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/imu.gRes);
+    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/imu.mRes);
+    imu.magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
+    imu.magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
+    imu.magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
+    t.start();        
+
+ while(1) {
+  
+  // If intPin goes high, all data registers have new data
+  if(imu.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
+
+    imu.readAccelData(imu.accelCount);  // Read the x/y/z adc values   
+    // Now we'll calculate the accleration value into actual g's
+    imu.ax = (float)imu.accelCount[0]*imu.aRes - imu.accelBias[0];  // get actual g value, this depends on scale being set
+    imu.ay = (float)imu.accelCount[1]*imu.aRes - imu.accelBias[1];   
+    imu.az = (float)imu.accelCount[2]*imu.aRes - imu.accelBias[2];  
+   
+    imu.readGyroData(imu.gyroCount);  // Read the x/y/z adc values
+    // Calculate the gyro value into actual degrees per second
+    imu.gx = (float)imu.gyroCount[0]*imu.gRes - imu.gyroBias[0];  // get actual gyro value, this depends on scale being set
+    imu.gy = (float)imu.gyroCount[1]*imu.gRes - imu.gyroBias[1];  
+    imu.gz = (float)imu.gyroCount[2]*imu.gRes - imu.gyroBias[2];   
+  
+    imu.readMagData(imu.magCount);  // Read the x/y/z adc values   
+    // Calculate the magnetometer values in milliGauss
+    // Include factory calibration per data sheet and user environmental corrections
+    imu.mx = (float)imu.magCount[0]*imu.mRes*imu.magCalibration[0] - imu.magbias[0];  // get actual magnetometer value, this depends on scale being set
+    imu.my = (float)imu.magCount[1]*imu.mRes*imu.magCalibration[1] - imu.magbias[1];  
+    imu.mz = (float)imu.magCount[2]*imu.mRes*imu.magCalibration[2] - imu.magbias[2];   
+  }
+   
+  if(imu.gz>.3 || imu.gz < -.3){
+     yaw = (yaw - t.read()*imu.gz+drift);
+    t.reset();        
+    if(yaw > 360)
+        yaw -= 360;
+    if(yaw < 0)
+        yaw += 360;
+    pc.printf("Yaw: %f \n\r", yaw);
+    }
+
+}
+}
+ 
\ No newline at end of file