main.cpp

00001 #include "SparkFunMPU9250-DMP.h"
00002 #include "mdcompat.h"
00003 //In order to compile, you should define MPU9250 in build macros
00004 //definitions
00005 #define DEBUG 0
00006 //classes
00007 Serial pc(USBTX,USBRX);
00008 DigitalOut led(LED1);
00009 MPU9250_DMP imu;
00010 
00011 //variables
00012 #if DEBUG
00013     unsigned char whoami[1]={0};
00014     unsigned char outbuff[4];
00015     unsigned char inbuff[4];
00016 #endif
00017 
00018 unsigned char regadd;
00019 char registeradress[5];
00020 unsigned char registerdata[]={33};
00021 
00022 //Prorotypes
00023 void printIMUData(void);
00024 
00025 int main() 
00026 {
00027   pc.baud(115200);
00028   pc.printf("Hello World\n");
00029   imu_init();
00030   stamper_init();
00031 #if ~DEBUG//Regular program
00032   // Call imu.begin() to verify communication and initialize
00033   if (imu.begin() != INV_SUCCESS){
00034     while (1){
00035       pc.printf("Unable to communicate with MPU-9250");
00036       pc.printf("Check connections, and try again.\n");
00037       wait_ms(5000);
00038     }
00039   }
00040     pc.printf("imu.begin() suceeded\n");
00041   
00042     if(imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat
00043                DMP_FEATURE_GYRO_CAL, // Use gyro calibration
00044               150)==INV_ERROR){ // Set DMP FIFO rate to 150 Hz
00045         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)
00046               }
00047     else{
00048         pc.printf("imu.dmpBegin() suceeded\n");
00049     }
00050     pc.printf("$GETEU,*\n");
00051 #endif
00052 #if DEBUG //this scope is only for debugging purposes of mbed_i2c_read and write functions
00053     mbed_i2c_read(0x68,0x75,1,whoami);
00054     pc.printf("whoami=%d\n",whoami[0]);
00055     mbed_i2c_read(0x68,0x23,2,inbuff);
00056     pc.printf("Buffer=%d-%d\n",inbuff[0],inbuff[1]);
00057     outbuff[0]=inbuff[0]&0x01;
00058     outbuff[1]=inbuff[1]&0x02;
00059     pc.printf("updated buffer=%d-%d\n",outbuff[0],outbuff[1]);
00060     mbed_i2c_write(0x68,0x23,2,outbuff);
00061     mbed_i2c_read(0x68,0x23,2,inbuff);
00062     pc.printf("new out buffer=%d-%d(if zero, write function still needs to be fixed)\n",inbuff[0],inbuff[1]);
00063 #endif
00064 while(1){
00065     if(pc.readable()){
00066         pc.scanf("%s",&registeradress);
00067         regadd=(registeradress[0]-48)*100+(registeradress[1]-48)*10+(registeradress[2]-48);
00068         mbed_i2c_read(0x68,(unsigned)regadd,1,registerdata);
00069         pc.printf("%d is gotten from serial port, data at that register is %d\n",regadd,registerdata[0]);
00070     }
00071 
00072   // Check for new data in the FIFO
00073   if (imu.fifoAvailable()){//fifo is not being available
00074     led=0;
00075     wait_ms(1);
00076     led=1;
00077     wait_ms(1);
00078     // Use dmpUpdateFifo to update the ax, gx, mx, etc. values
00079     if ( imu.dmpUpdateFifo() == INV_SUCCESS){
00080       // computeEulerAngles can be used -- after updating the
00081       // quaternion values -- to estimate roll, pitch, and yaw
00082       imu.computeEulerAngles();
00083       printIMUData();
00084     }
00085   }
00086   else{
00087     led=0;
00088   }
00089 }
00090   return 0;
00091 }
00092 
00093 void printIMUData(void)
00094 {  
00095   // After calling dmpUpdateFifo() the ax, gx, mx, etc. values
00096   // are all updated.
00097   // Quaternion values are, by default, stored in Q30 long
00098   // format. calcQuat turns them into a float between -1 and 1
00099   float q0 = imu.calcQuat(imu.qw);
00100   float q1 = imu.calcQuat(imu.qx);
00101   float q2 = imu.calcQuat(imu.qy);
00102   float q3 = imu.calcQuat(imu.qz);
00103   
00104   pc.printf("$,%.4lf,%.4lf,%.4lf,*\n", imu.yaw,imu.pitch,imu.roll);
00105 }