An example program that decodes the data returned from the MPU9150 DMP

Dependencies:   CMSIS_DSP_401 MPU9150_DMP QuaternionMath mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "MPU9150.h"
00002 #include "Quaternion.h"
00003 
00004 Serial debug(USBTX, USBRX);
00005 MPU9150 imu(p10, p9, p15);
00006 
00007 DigitalOut led(LED1);
00008 
00009 char buffer[200];
00010 
00011 int main(){
00012     debug.baud(115200);
00013     
00014     if(imu.isReady()){
00015         debug.printf("MPU9150 is ready\r\n");
00016     } else {
00017         debug.printf("MPU9150 initialisation failure\r\n");
00018     }
00019     
00020     imu.initialiseDMP();
00021 
00022     Timer timer;
00023     timer.start();
00024 
00025     imu.setFifoReset(true);    
00026     imu.setDMPEnabled(true);    
00027     
00028     Quaternion q1;
00029     
00030     while(true){
00031 
00032         if(imu.getFifoCount() >= 48){
00033             imu.getFifoBuffer(buffer,  48);
00034             led = !led;
00035         }
00036         
00037         if(timer.read_ms() > 50){
00038             timer.reset();
00039            
00040            //This is the format of the data in the fifo, 
00041            /* ================================================================================================ *
00042              | Default MotionApps v4.1 48-byte FIFO packet structure:                                           |
00043              |                                                                                                  |
00044              | [QUAT W][      ][QUAT X][      ][QUAT Y][      ][QUAT Z][      ][GYRO X][      ][GYRO Y][      ] |
00045              |   0   1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  |
00046              |                                                                                                  |
00047              | [GYRO Z][      ][MAG X ][MAG Y ][MAG Z ][ACC X ][      ][ACC Y ][      ][ACC Z ][      ][      ] |
00048              |  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  |
00049              * ================================================================================================ */
00050              
00051             /*
00052             debug.printf("%d, %d, %d\r\n",  (int32_t)(((int32_t)buffer[34] << 24) + ((int32_t)buffer[35] << 16) + ((int32_t)buffer[36] << 8) + (int32_t)buffer[37]), 
00053                                             (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]), 
00054                                             (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45]));
00055                                                 
00056             debug.printf("%d, %d, %d\r\n",  (int32_t)(((int32_t)buffer[16] << 24) + ((int32_t)buffer[17] << 16) + ((int32_t)buffer[18] << 8) + (int32_t)buffer[19]),
00057                                             (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]),
00058                                             (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27]));
00059                                             
00060             debug.printf("%d, %d, %d\r\n",  (int16_t)(buffer[29] << 8) + buffer[28], 
00061                                             (int16_t)(buffer[31] << 8) + buffer[30], 
00062                                             (int16_t)(buffer[33] << 8) + buffer[32]);
00063                                             
00064             debug.printf("%f, %f, %f, %f\r\n", 
00065                 (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)),
00066                 (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)),
00067                 (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)),
00068                 (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30)));
00069             */
00070                 
00071             q1.decode(buffer);
00072             debug.printf("%f, %f, %f, %f\r\n", q1.w, q1.v.x, q1.v.y, q1.v.z);
00073 
00074 
00075             //TeaPot Demo Packet for MotionFit SDK
00076             /*
00077             debug.putc('$'); //packet start
00078             debug.putc((char)2); //assume packet type constant
00079             debug.putc((char)0); //count seems unused
00080             for(int i = 0; i < 16; i++){ //16 bytes for 4 32bit floats
00081                 debug.putc((char)(buffer[i]));
00082             }
00083             for(int i = 0; i < 5; i++){ //no idea, padded with 0
00084                 debug.putc((char)0);
00085             }
00086             */
00087         }   
00088     }
00089 }