An example program that decodes the data returned from the MPU9150 DMP
Dependencies: CMSIS_DSP_401 MPU9150_DMP QuaternionMath mbed
Fork of MPU9150_Example by
main.cpp
00001 #include "MPU9150.h" 00002 #include "Quaternion.h" 00003 00004 Serial debug(USBTX, USBRX); 00005 MPU9150 imu(p27, p28, 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 wait(1); 00033 00034 00035 if(imu.getFifoCount() >= 48){ 00036 imu.getFifoBuffer(buffer, 48); 00037 led = !led; 00038 } 00039 q1.decode(buffer); 00040 // wait(1.1); 00041 if(timer.read_ms() >50){ 00042 timer.reset(); 00043 00044 //This is the format of the data in the fifo, 00045 /* ================================================================================================ * 00046 | Default MotionApps v4.1 48-byte FIFO packet structure: | 00047 | | 00048 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | 00049 | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | 00050 | | 00051 | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | 00052 | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | 00053 * ================================================================================================ */ 00054 00055 /* 00056 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]), 00057 (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]), 00058 (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45])); 00059 00060 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]), 00061 (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]), 00062 (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27])); 00063 00064 debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28], 00065 (int16_t)(buffer[31] << 8) + buffer[30], 00066 (int16_t)(buffer[33] << 8) + buffer[32]); 00067 00068 debug.printf("%f, %f, %f, %f\r\n", 00069 (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)), 00070 (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)), 00071 (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)), 00072 (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30))); 00073 */ 00074 //wait(0.01); 00075 00076 debug.printf("w:%f, v.x:%f, v.y:%f, v.z:%f\r\n", q1.w, q1.v.x, q1.v.y, q1.v.z); 00077 imu.setFifoReset(true); 00078 imu.setDMPEnabled(true); 00079 00080 //TeaPot Demo Packet for MotionFit SDK 00081 /* 00082 debug.putc('$'); //packet start 00083 debug.putc((char)2); //assume packet type constant 00084 debug.putc((char)0); //count seems unused 00085 for(int i = 0; i < 16; i++){ //16 bytes for 4 32bit floats 00086 debug.putc((char)(buffer[i])); 00087 } 00088 for(int i = 0; i < 5; i++){ //no idea, padded with 0 00089 debug.putc((char)0); 00090 } 00091 */ 00092 00093 } 00094 00095 } 00096 }
Generated on Mon Jul 18 2022 15:30:04 by 1.7.2