Chris Pepper / Mbed 2 deprecated MPU9150_Example

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

Dependencies:   CMSIS_DSP_401 MPU9150_DMP QuaternionMath mbed

main.cpp@0:9f2982964a48, 2014-09-01 (annotated)

Committer:
p3p
Date:
Mon Sep 01 14:26:51 2014 +0000
Revision:
0:9f2982964a48
An Example program that decodes the data returned from the MPU9150 DMP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
p3p 0:9f2982964a48 1 #include "MPU9150.h"
p3p 0:9f2982964a48 2 #include "Quaternion.h"
p3p 0:9f2982964a48 3
p3p 0:9f2982964a48 4 Serial debug(USBTX, USBRX);
p3p 0:9f2982964a48 5 MPU9150 imu(p10, p9, p15);
p3p 0:9f2982964a48 6
p3p 0:9f2982964a48 7 DigitalOut led(LED1);
p3p 0:9f2982964a48 8
p3p 0:9f2982964a48 9 char buffer[200];
p3p 0:9f2982964a48 10
p3p 0:9f2982964a48 11 int main(){
p3p 0:9f2982964a48 12 debug.baud(115200);
p3p 0:9f2982964a48 13
p3p 0:9f2982964a48 14 if(imu.isReady()){
p3p 0:9f2982964a48 15 debug.printf("MPU9150 is ready\r\n");
p3p 0:9f2982964a48 16 } else {
p3p 0:9f2982964a48 17 debug.printf("MPU9150 initialisation failure\r\n");
p3p 0:9f2982964a48 18 }
p3p 0:9f2982964a48 19
p3p 0:9f2982964a48 20 imu.initialiseDMP();
p3p 0:9f2982964a48 21
p3p 0:9f2982964a48 22 Timer timer;
p3p 0:9f2982964a48 23 timer.start();
p3p 0:9f2982964a48 24
p3p 0:9f2982964a48 25 imu.setFifoReset(true);
p3p 0:9f2982964a48 26 imu.setDMPEnabled(true);
p3p 0:9f2982964a48 27
p3p 0:9f2982964a48 28 Quaternion q1;
p3p 0:9f2982964a48 29
p3p 0:9f2982964a48 30 while(true){
p3p 0:9f2982964a48 31
p3p 0:9f2982964a48 32 if(imu.getFifoCount() >= 48){
p3p 0:9f2982964a48 33 imu.getFifoBuffer(buffer, 48);
p3p 0:9f2982964a48 34 led = !led;
p3p 0:9f2982964a48 35 }
p3p 0:9f2982964a48 36
p3p 0:9f2982964a48 37 if(timer.read_ms() > 50){
p3p 0:9f2982964a48 38 timer.reset();
p3p 0:9f2982964a48 39
p3p 0:9f2982964a48 40 //This is the format of the data in the fifo,
p3p 0:9f2982964a48 41 /* ================================================================================================ *
p3p 0:9f2982964a48 42 | Default MotionApps v4.1 48-byte FIFO packet structure: |
p3p 0:9f2982964a48 43 | |
p3p 0:9f2982964a48 44 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
p3p 0:9f2982964a48 45 | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
p3p 0:9f2982964a48 46 | |
p3p 0:9f2982964a48 47 | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
p3p 0:9f2982964a48 48 | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 |
p3p 0:9f2982964a48 49 * ================================================================================================ */
p3p 0:9f2982964a48 50
p3p 0:9f2982964a48 51 /*
p3p 0:9f2982964a48 52 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]),
p3p 0:9f2982964a48 53 (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]),
p3p 0:9f2982964a48 54 (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45]));
p3p 0:9f2982964a48 55
p3p 0:9f2982964a48 56 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]),
p3p 0:9f2982964a48 57 (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]),
p3p 0:9f2982964a48 58 (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27]));
p3p 0:9f2982964a48 59
p3p 0:9f2982964a48 60 debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28],
p3p 0:9f2982964a48 61 (int16_t)(buffer[31] << 8) + buffer[30],
p3p 0:9f2982964a48 62 (int16_t)(buffer[33] << 8) + buffer[32]);
p3p 0:9f2982964a48 63
p3p 0:9f2982964a48 64 debug.printf("%f, %f, %f, %f\r\n",
p3p 0:9f2982964a48 65 (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)),
p3p 0:9f2982964a48 66 (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)),
p3p 0:9f2982964a48 67 (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)),
p3p 0:9f2982964a48 68 (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30)));
p3p 0:9f2982964a48 69 */
p3p 0:9f2982964a48 70
p3p 0:9f2982964a48 71 q1.decode(buffer);
p3p 0:9f2982964a48 72 debug.printf("%f, %f, %f, %f\r\n", q1.w, q1.v.x, q1.v.y, q1.v.z);
p3p 0:9f2982964a48 73
p3p 0:9f2982964a48 74
p3p 0:9f2982964a48 75 //TeaPot Demo Packet for MotionFit SDK
p3p 0:9f2982964a48 76 /*
p3p 0:9f2982964a48 77 debug.putc('$'); //packet start
p3p 0:9f2982964a48 78 debug.putc((char)2); //assume packet type constant
p3p 0:9f2982964a48 79 debug.putc((char)0); //count seems unused
p3p 0:9f2982964a48 80 for(int i = 0; i < 16; i++){ //16 bytes for 4 32bit floats
p3p 0:9f2982964a48 81 debug.putc((char)(buffer[i]));
p3p 0:9f2982964a48 82 }
p3p 0:9f2982964a48 83 for(int i = 0; i < 5; i++){ //no idea, padded with 0
p3p 0:9f2982964a48 84 debug.putc((char)0);
p3p 0:9f2982964a48 85 }
p3p 0:9f2982964a48 86 */
p3p 0:9f2982964a48 87 }
p3p 0:9f2982964a48 88 }
p3p 0:9f2982964a48 89 }