main.cpp

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