Diff: main.cpp

Revision:

8:b9206956f396

Parent:

7:a862072e0981

Child:

9:ef46e12b4560

--- a/main.cpp	Mon Aug 14 07:37:15 2017 +0000
+++ b/main.cpp	Mon Aug 14 09:03:52 2017 +0000
@@ -1,10 +1,9 @@
 #include "SparkFunMPU9250-DMP.h"
 #include "mdcompat.h"
-#include "stm32f103c8t6.h"
-Serial pc(SERIAL_TX,SERIAL_RX);
+
+Serial pc(USBTX,USBRX);
 DigitalOut led(LED1);
 MPU9250_DMP imu;
-//variables
 unsigned char whoami[1]={0};
 unsigned char fifo_count[2]={32,32};
 unsigned char temp[1]={32};
@@ -13,8 +12,6 @@
 unsigned char regadd;
 char registeradress[5];
 unsigned char registerdata[]={33};
-unsigned long  count=0;
-//Prototypes
 void printIMUData(void);
 int main() 
 {
@@ -35,13 +32,13 @@
   
     if(imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat
                DMP_FEATURE_GYRO_CAL, // Use gyro calibration
-              20)==INV_ERROR){ // Set DMP FIFO rate to 20 Hz
+              150)==INV_ERROR){ // Set DMP FIFO rate to 150 Hz
         pc.printf("imu.dmpBegin have failed\n");//dmpLoad function under it fails which is caused by memcmp(firmware+ii, cur, this_write) (it is located at row 2871 of inv_mpu.c)
               }
     else{
         pc.printf("imu.dmpBegin() suceeded\n");
     }
-      pc.printf("$GETEU,*\n");//indicationg that data will send to matlab by serial
+    pc.printf("$GETEU,*\n");
 #endif
 #if 0//this scope is only for debugging purposes of mbed_i2c_read and write functions
     mbed_i2c_read(0x68,0x75,1,whoami);
@@ -72,6 +69,10 @@
     }*/
   // Check for new data in the FIFO
   if (imu.fifoAvailable()){//fifo is not being available
+    led=0;
+    wait_ms(1);
+    led=1;
+    wait_ms(1);
     // Use dmpUpdateFifo to update the ax, gx, mx, etc. values
     if ( imu.dmpUpdateFifo() == INV_SUCCESS){
       // computeEulerAngles can be used -- after updating the
@@ -81,10 +82,7 @@
     }
   }
   else{
-    led=1;
-    wait_ms(500);
-    led=0;
-    wait_ms(500);      
+          led=0;
   }
 }
   return 0;
@@ -101,5 +99,5 @@
   float q2 = imu.calcQuat(imu.qy);
   float q3 = imu.calcQuat(imu.qz);
   
-  pc.printf("t:%d\t Y:%lf\t  P:%lf\t   R:%lf\n",get_ms(&count), imu.yaw,imu.pitch,imu.roll);
+  pc.printf("$,%.4lf,%.4lf,%.4lf,*\n", imu.yaw,imu.pitch,imu.roll);
 }