センサー動作確認版
Dependencies: MPU9150_DMP QuaternionMath iSDIO mbed-rtos mbed
taskSensor.cpp@0:65a0ae5578da, 2016-05-14 (annotated)
- Committer:
- yishii
- Date:
- Sat May 14 07:11:48 2016 +0000
- Revision:
- 0:65a0ae5578da
- Child:
- 1:73543a1fbe62
Supported to use with Nucleo F303K8
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yishii | 0:65a0ae5578da | 1 | #include "mbed.h" |
yishii | 0:65a0ae5578da | 2 | #include "rtos.h" |
yishii | 0:65a0ae5578da | 3 | #include "math.h" |
yishii | 0:65a0ae5578da | 4 | #include "common.h" |
yishii | 0:65a0ae5578da | 5 | #include "gpio_defs.h" |
yishii | 0:65a0ae5578da | 6 | #include "MPU9150.h" |
yishii | 0:65a0ae5578da | 7 | #include "Quaternion.h" |
yishii | 0:65a0ae5578da | 8 | |
yishii | 0:65a0ae5578da | 9 | static MPU9150 imu(PIN_I2C_SCL, PIN_I2C_SDA, PIN_MPU9250_INT); // SCL SDA INT |
yishii | 0:65a0ae5578da | 10 | // SDA = D4 |
yishii | 0:65a0ae5578da | 11 | // SCL = D5 |
yishii | 0:65a0ae5578da | 12 | // INT = D6 |
yishii | 0:65a0ae5578da | 13 | |
yishii | 0:65a0ae5578da | 14 | static bool imu_initialized = false; |
yishii | 0:65a0ae5578da | 15 | |
yishii | 0:65a0ae5578da | 16 | static void init_sensor(void) |
yishii | 0:65a0ae5578da | 17 | { |
yishii | 0:65a0ae5578da | 18 | if(imu.isReady()){ |
yishii | 0:65a0ae5578da | 19 | printf("MPU9150 is ready\r\n"); |
yishii | 0:65a0ae5578da | 20 | } else { |
yishii | 0:65a0ae5578da | 21 | printf("MPU9150 initialisation failure\r\n"); |
yishii | 0:65a0ae5578da | 22 | while(1){ |
yishii | 0:65a0ae5578da | 23 | Thread::wait(1000); |
yishii | 0:65a0ae5578da | 24 | } |
yishii | 0:65a0ae5578da | 25 | } |
yishii | 0:65a0ae5578da | 26 | imu.initialiseDMP(); |
yishii | 0:65a0ae5578da | 27 | imu_initialized = true; |
yishii | 0:65a0ae5578da | 28 | } |
yishii | 0:65a0ae5578da | 29 | |
yishii | 0:65a0ae5578da | 30 | static char buffer[200]; |
yishii | 0:65a0ae5578da | 31 | |
yishii | 0:65a0ae5578da | 32 | void sensor_main(void) |
yishii | 0:65a0ae5578da | 33 | { |
yishii | 0:65a0ae5578da | 34 | Quaternion q1; |
yishii | 0:65a0ae5578da | 35 | pc.printf("Entered to sensor_main\r\n"); |
yishii | 0:65a0ae5578da | 36 | float q[4]; |
yishii | 0:65a0ae5578da | 37 | uint8_t n = 0; |
yishii | 0:65a0ae5578da | 38 | uint16_t val; |
yishii | 0:65a0ae5578da | 39 | |
yishii | 0:65a0ae5578da | 40 | imu.setFifoReset(true); |
yishii | 0:65a0ae5578da | 41 | imu.setDMPEnabled(true); |
yishii | 0:65a0ae5578da | 42 | |
yishii | 0:65a0ae5578da | 43 | pc.printf("sensor main loop start\r\n"); |
yishii | 0:65a0ae5578da | 44 | |
yishii | 0:65a0ae5578da | 45 | while(true){ |
yishii | 0:65a0ae5578da | 46 | |
yishii | 0:65a0ae5578da | 47 | //StatusLED = StatusLED == 1 ? 0 : 1; |
yishii | 0:65a0ae5578da | 48 | |
yishii | 0:65a0ae5578da | 49 | if(imu.getFifoCount() >= 48){ |
yishii | 0:65a0ae5578da | 50 | imu.getFifoBuffer(buffer, 48); |
yishii | 0:65a0ae5578da | 51 | |
yishii | 0:65a0ae5578da | 52 | //This is the format of the data in the fifo, |
yishii | 0:65a0ae5578da | 53 | /* ================================================================================================ * |
yishii | 0:65a0ae5578da | 54 | | Default MotionApps v4.1 48-byte FIFO packet structure: | |
yishii | 0:65a0ae5578da | 55 | | | |
yishii | 0:65a0ae5578da | 56 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | |
yishii | 0:65a0ae5578da | 57 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | |
yishii | 0:65a0ae5578da | 58 | | | |
yishii | 0:65a0ae5578da | 59 | | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | |
yishii | 0:65a0ae5578da | 60 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | |
yishii | 0:65a0ae5578da | 61 | * ================================================================================================ */ |
yishii | 0:65a0ae5578da | 62 | |
yishii | 0:65a0ae5578da | 63 | // 以下の#if 0~#endifを#if 1~#endifに変更すると、シリアルからセンサーの出力データが出る |
yishii | 0:65a0ae5578da | 64 | |
yishii | 0:65a0ae5578da | 65 | // 加速度センサー生値 |
yishii | 0:65a0ae5578da | 66 | #if 0 |
yishii | 0:65a0ae5578da | 67 | 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]), |
yishii | 0:65a0ae5578da | 68 | (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]), |
yishii | 0:65a0ae5578da | 69 | (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45])); |
yishii | 0:65a0ae5578da | 70 | #endif |
yishii | 0:65a0ae5578da | 71 | #if 0 |
yishii | 0:65a0ae5578da | 72 | // 各速度センサー生値 |
yishii | 0:65a0ae5578da | 73 | //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]), |
yishii | 0:65a0ae5578da | 74 | (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]), |
yishii | 0:65a0ae5578da | 75 | (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27])); |
yishii | 0:65a0ae5578da | 76 | #endif |
yishii | 0:65a0ae5578da | 77 | #if 0 |
yishii | 0:65a0ae5578da | 78 | // 地磁気センサー生値 |
yishii | 0:65a0ae5578da | 79 | //pc.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28], |
yishii | 0:65a0ae5578da | 80 | (int16_t)(buffer[31] << 8) + buffer[30], |
yishii | 0:65a0ae5578da | 81 | (int16_t)(buffer[33] << 8) + buffer[32]); |
yishii | 0:65a0ae5578da | 82 | #endif |
yishii | 0:65a0ae5578da | 83 | #if 0 |
yishii | 0:65a0ae5578da | 84 | // センサーフュージョンによるQuartanion |
yishii | 0:65a0ae5578da | 85 | printf("%f, %f, %f, %f\r\n", |
yishii | 0:65a0ae5578da | 86 | (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)), |
yishii | 0:65a0ae5578da | 87 | (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)), |
yishii | 0:65a0ae5578da | 88 | (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)), |
yishii | 0:65a0ae5578da | 89 | (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30))); |
yishii | 0:65a0ae5578da | 90 | #endif |
yishii | 0:65a0ae5578da | 91 | #if 0 // Quartanionによる処理 : ProcessingでPCにてデモしたもの |
yishii | 0:65a0ae5578da | 92 | q1.decode(buffer); |
yishii | 0:65a0ae5578da | 93 | //printf("%f, %f, %f, %f\r\n", q1.w, q1.v.x, q1.v.y, q1.v.z); |
yishii | 0:65a0ae5578da | 94 | q[0] = q1.w; |
yishii | 0:65a0ae5578da | 95 | q[1] = q1.v.x; |
yishii | 0:65a0ae5578da | 96 | q[2] = q1.v.y; |
yishii | 0:65a0ae5578da | 97 | q[3] = q1.v.z; |
yishii | 0:65a0ae5578da | 98 | |
yishii | 0:65a0ae5578da | 99 | //TeaPot Demo Packet for MotionFit SDK |
yishii | 0:65a0ae5578da | 100 | |
yishii | 0:65a0ae5578da | 101 | pc.putc('$'); //packet start |
yishii | 0:65a0ae5578da | 102 | pc.putc((char)2); //assume packet type constant |
yishii | 0:65a0ae5578da | 103 | |
yishii | 0:65a0ae5578da | 104 | pc.putc(buffer[0]); |
yishii | 0:65a0ae5578da | 105 | pc.putc(buffer[1]); |
yishii | 0:65a0ae5578da | 106 | pc.putc(buffer[4]); |
yishii | 0:65a0ae5578da | 107 | pc.putc(buffer[5]); |
yishii | 0:65a0ae5578da | 108 | pc.putc(buffer[8]); |
yishii | 0:65a0ae5578da | 109 | pc.putc(buffer[9]); |
yishii | 0:65a0ae5578da | 110 | pc.putc(buffer[12]); |
yishii | 0:65a0ae5578da | 111 | pc.putc(buffer[13]); |
yishii | 0:65a0ae5578da | 112 | |
yishii | 0:65a0ae5578da | 113 | pc.putc((char)n++); |
yishii | 0:65a0ae5578da | 114 | pc.putc((char)0); |
yishii | 0:65a0ae5578da | 115 | pc.putc('\r'); |
yishii | 0:65a0ae5578da | 116 | pc.putc('\n'); |
yishii | 0:65a0ae5578da | 117 | #endif |
yishii | 0:65a0ae5578da | 118 | } else { |
yishii | 0:65a0ae5578da | 119 | //pc.printf("FIFO not enough\r\n"); |
yishii | 0:65a0ae5578da | 120 | } |
yishii | 0:65a0ae5578da | 121 | |
yishii | 0:65a0ae5578da | 122 | Thread::wait(10); |
yishii | 0:65a0ae5578da | 123 | } |
yishii | 0:65a0ae5578da | 124 | |
yishii | 0:65a0ae5578da | 125 | } |
yishii | 0:65a0ae5578da | 126 | |
yishii | 0:65a0ae5578da | 127 | void TaskSensor(void const *argument) |
yishii | 0:65a0ae5578da | 128 | { |
yishii | 0:65a0ae5578da | 129 | init_sensor(); |
yishii | 0:65a0ae5578da | 130 | |
yishii | 0:65a0ae5578da | 131 | while(true){ |
yishii | 0:65a0ae5578da | 132 | sensor_main(); |
yishii | 0:65a0ae5578da | 133 | } |
yishii | 0:65a0ae5578da | 134 | } |