Diff: main.cpp

Revision:

0:c6daf7016b8c

Child:

2:7be1ac6eb8cb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Aug 07 13:50:51 2017 +0000
@@ -0,0 +1,91 @@
+/*#include "mbed.h"
+#include "inv_mpu.h"
+#include "mdcompat.h"
+Serial pc(USBTX,USBRX);
+DigitalOut led(LED1);
+int main()
+{
+    imu_init();
+    
+    while(1){
+        led=1;
+        wait_ms(100);
+        led=0;
+        wait_ms(100);
+    }  
+}*/
+
+#include "SparkFunMPU9250-DMP.h"
+
+Serial pc(USBTX,USBRX);
+DigitalOut led(LED1);
+MPU9250_DMP imu;
+long DEBUG[]={2,2,2,2};
+int main() 
+{
+  pc.baud(115200);
+  pc.printf("Hello World\n");
+  // Call imu.begin() to verify communication and initialize
+    pc.printf("imu.begin()is being run\n");
+  if (imu.begin() != INV_SUCCESS){
+    pc.printf("imu.begin() have failed");
+    while (1){
+      pc.printf("Unable to communicate with MPU-9250");
+      pc.printf("Check connections, and try again.\n");
+      wait_ms(5000);
+    }
+  }
+  pc.printf("imu.begin() suceeded\n");
+  imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat
+               DMP_FEATURE_GYRO_CAL, // Use gyro calibration
+              20); // Set DMP FIFO rate to 20 Hz
+  // DMP_FEATURE_LP_QUAT can also be used. It uses the 
+  // accelerometer in low-power mode to estimate quat's.
+  // DMP_FEATURE_LP_QUAT and 6X_LP_QUAT are mutually exclusive
+  pc.printf("imu.dmpBegin() suceeded\n");
+
+while(1){
+  // Check for new data in the FIFO
+  led=1;
+  wait_ms(500);
+  led=0;
+  wait_ms(500);
+  if ( imu.fifoAvailable() ){//fifo is not being available
+    led=0;
+    wait_ms(100);
+    led=1;
+    wait_ms(100);
+    // Use dmpUpdateFifo to update the ax, gx, mx, etc. values
+    if ( imu.dmpUpdateFifo(DEBUG) == INV_SUCCESS){
+      // computeEulerAngles can be used -- after updating the
+      // quaternion values -- to estimate roll, pitch, and yaw
+      imu.computeEulerAngles();
+      //printIMUData();
+     #if 1
+      pc.printf("%lf",DEBUG[0]);
+      pc.printf("\t");
+      pc.printf("%lf",DEBUG[1]);
+      pc.printf("\t");
+      pc.printf("%lf",DEBUG[2]);
+      pc.printf("\t");
+      pc.printf("%lf\n ",DEBUG[3]);
+     #endif
+    }
+  }
+}
+  return 0;
+}
+
+void printIMUData(void)
+{  
+  // After calling dmpUpdateFifo() the ax, gx, mx, etc. values
+  // are all updated.
+  // Quaternion values are, by default, stored in Q30 long
+  // format. calcQuat turns them into a float between -1 and 1
+  float q0 = imu.calcQuat(imu.qw);
+  float q1 = imu.calcQuat(imu.qx);
+  float q2 = imu.calcQuat(imu.qy);
+  float q3 = imu.calcQuat(imu.qz);
+  
+  pc.printf("R:%lf  P:%lf   Y:%lf \n", imu.roll,imu.pitch,imu.yaw);
+}