first publish
Dependencies: MPU9150_DMP MotorControl QuaternionMath SimpleIOMacros mbed
Fork of cool_step by
main.cpp
- Committer:
- heuristics
- Date:
- 2015-05-05
- Revision:
- 9:ca52462f9ebc
- Parent:
- 8:63eff653d0ba
- Child:
- 10:09dbd00164b9
File content as of revision 9:ca52462f9ebc:
#include "mbed.h" #include "MPU9150.h" #include "Quaternion.h" //led pins DigitalOut led1(P2_6); DigitalOut led2(P2_7); DigitalOut led3(P2_8); MPU9150 imu(P0_28, P0_27, P2_13); Serial RN42(P0_10, P0_11); Serial debug(P0_2, P0_3); Ticker infoTicker; Timer timer; Timer timer2; char buffer[200]; void infoTask(void) { led1=!led1; } int main() { RN42.baud(9600); debug.baud(115200); // initialize_connection(); infoTicker.attach(infoTask,0.2f); if(imu.isReady()) { debug.printf("MPU9150 is ready\r\n"); } else { debug.printf("MPU9150 initialisation failure\r\n"); } imu.initialiseDMP(); timer.start(); timer2.start(); imu.setFifoReset(true); imu.setDMPEnabled(true); Quaternion q1; while(1) { if(imu.getFifoCount() >= 48) { imu.getFifoBuffer(buffer, 48); led2 = !led2; debug.printf("%d\r\n",timer2.read_ms()); timer2.reset(); } if(timer.read_ms() > 50) { timer.reset(); //This is the format of the data in the fifo, /* ================================================================================================ * | Default MotionApps v4.1 48-byte FIFO packet structure: | | | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | | | | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | * ================================================================================================ */ /* 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]), (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]), (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45])); 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]), (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]), (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27])); debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28], (int16_t)(buffer[31] << 8) + buffer[30], (int16_t)(buffer[33] << 8) + buffer[32]); debug.printf("%f, %f, %f, %f\r\n", (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)), (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)), (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)), (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30))); */ q1.decode(buffer); debug.printf("%f, %f, %f, %f ", q1.w, q1.v.x, q1.v.y, q1.v.z); Vector3 vector_x(1,0,0),vector_y(0,1,0),vector_z(0,0,1); Vector3 pry=q1.getEulerAngles(); /*Vector3 vector_x_local=q1.rotate(vector_x); Vector3 vector_y_local=q1.rotate(vector_y); Vector3 vector_z_local=q1.rotate(vector_z); Vector3 direction_plane_normal=vector_z_local.cross_product(vector_z); Vector3 bar_projection=vector_z-vector_x_local*(vector_z.dot_product(vector_x_local)/vector_x_local.length_squared()); Vector3 bar_projection_g=bar_projection-vector_z*(bar_projection.dot_product(vector_z)/vector_z.length_squared()); float angle=acos(vector_y_local.dot_product(vector_z)/vector_y_local.length()/vector_z.length());*/ //debug.printf("angle: %f ", angle/PI*180); debug.printf("p: %f r: %f y: %f \r\n ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180); //TeaPot Demo Packet for MotionFit SDK /* debug.putc('$'); //packet start debug.putc((char)2); //assume packet type constant debug.putc((char)0); //count seems unused for(int i = 0; i < 16; i++){ //16 bytes for 4 32bit floats debug.putc((char)(buffer[i])); } for(int i = 0; i < 5; i++){ //no idea, padded with 0 debug.putc((char)0); } */ } } }