Diff: main.cpp

Revision:

4:404c35f32ce3

Parent:

1:71c319f03fda

diff -r c05fbe0aef1f -r 404c35f32ce3 main.cpp
--- a/main.cpp	Thu Sep 04 20:16:36 2014 +0000
+++ b/main.cpp	Thu Sep 04 21:19:05 2014 +0000
@@ -30,7 +30,7 @@
 //F401_init84 myinit(0);
 #include "mbed.h"
 #include "MPU9250.h"
-#include "N5110.h"
+#include "AHRS.h"
 
 // Using NOKIA 5110 monochrome 84 x 48 pixel display
 // pin 9 - Serial clock out (SCLK)
@@ -41,17 +41,16 @@
 //Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
 
 float sum = 0;
+float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};           // vector to hold quaternion
+
 uint32_t sumCount = 0;
 char buffer[14];
-
-   MPU9250 mpu9250;
+I2C i2c(p7, p6);
+   MPU9250 mpu9250(&i2c);
    
    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);
    
 
         
@@ -66,9 +65,6 @@
   
   t.start();        
   
-  lcd.init();
-//  lcd.setBrightness(0.05);
-  
     
   // 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
@@ -78,11 +74,7 @@
   {  
     pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
     pc.printf("MPU9250 is online...\n\r");
-    lcd.clear();
-    lcd.printString("MPU9250 is", 0, 0);
     sprintf(buffer, "0x%x", whoami);
-    lcd.printString(buffer, 0, 1);
-    lcd.printString("shoud be 0x71", 0, 2);  
     wait(1);
     
     mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
@@ -117,13 +109,7 @@
    {
     pc.printf("Could not connect to MPU9250: \n\r");
     pc.printf("%#x \n",  whoami);
- 
-    lcd.clear();
-    lcd.printString("MPU9250", 0, 0);
-    lcd.printString("no connection", 0, 1);
-    sprintf(buffer, "WHO_AM_I 0x%x", whoami);
-    lcd.printString(buffer, 0, 2); 
- 
+  
     while(1) ; // Loop forever if communication doesn't happen
     }
 
@@ -176,7 +162,7 @@
     
    // 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);
+    MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz, deltat, q);
 
     // Serial print and/or display at 0.5 s rate independent of data rates
     delt_t = t.read_ms() - count;
@@ -237,7 +223,6 @@
 //    sprintf(buffer, "rate = %f", (float) sumCount/sum);
 //    lcd.printString(buffer, 0, 5);
     
-    myled= !myled;
     count = t.read_ms(); 
 
     if(count > 1<<21) {