センサー動作確認版
Dependencies: MPU9150_DMP QuaternionMath iSDIO mbed-rtos mbed
taskSensor.cpp
- Committer:
- yishii
- Date:
- 2016-05-16
- Revision:
- 3:5f0d6133d34c
- Parent:
- 1:73543a1fbe62
File content as of revision 3:5f0d6133d34c:
#include "mbed.h" #include "rtos.h" #include "math.h" #include "common.h" #include "gpio_defs.h" #include "MPU9150.h" #include "Quaternion.h" static MPU9150 imu(PIN_I2C_SCL, PIN_I2C_SDA, PIN_MPU9250_INT); // SCL SDA INT // SDA = D4 // SCL = D5 // INT = D6 static bool imu_initialized = false; static void init_sensor(void) { if(imu.isReady()){ printf("MPU9150 is ready\r\n"); } else { printf("MPU9150 initialization failured\r\n"); while(1){ Thread::wait(1000); } } imu.initialiseDMP(); imu_initialized = true; } static char buffer[200]; void sensor_main(void) { Quaternion q1; printf("Entered to sensor_main\r\n"); float q[4]; uint8_t n = 0; uint16_t val; imu.setFifoReset(true); imu.setDMPEnabled(true); printf("sensor main loop start\r\n"); while(true){ //StatusLED = StatusLED == 1 ? 0 : 1; if(imu.getFifoCount() >= 48){ imu.getFifoBuffer(buffer, 48); //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 | * ================================================================================================ */ // 以下の#if 0~#endifを#if 1~#endifに変更すると、シリアルからセンサーの出力データが出る // 加速度センサー生値 #if 1 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])); #endif #if 0 // 各速度センサー生値 //pc.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])); #endif #if 0 // 地磁気センサー生値 //pc.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]); #endif #if 0 // センサーフュージョンによるQuartanion 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))); #endif #if 0 // Quartanionによる処理 : ProcessingでPCにてデモしたもの q1.decode(buffer); //printf("%f, %f, %f, %f\r\n", q1.w, q1.v.x, q1.v.y, q1.v.z); q[0] = q1.w; q[1] = q1.v.x; q[2] = q1.v.y; q[3] = q1.v.z; //TeaPot Demo Packet for MotionFit SDK putc('$'); //packet start putc((char)2); //assume packet type constant putc(buffer[0]); putc(buffer[1]); putc(buffer[4]); putc(buffer[5]); putc(buffer[8]); putc(buffer[9]); putc(buffer[12]); putc(buffer[13]); putc((char)n++); putc((char)0); putc('\r'); putc('\n'); #endif } else { //pc.printf("FIFO not enough\r\n"); } Thread::wait(10); } } void TaskSensor(void const *argument) { init_sensor(); while(true){ sensor_main(); } }