yasuo katano / Mbed 2 deprecated Neon_F303K8_04

arranged by katano

Dependencies:   MPU9150_DMP Neon_F303K8 QuaternionMath iSDIO mbed-rtos mbed

Fork of Neon_F303K8 by Yasuhiro ISHII

taskSensor.cpp@4:636bb3f66dcd, 2016-08-05 (annotated)

Committer:
yakatano
Date:
Fri Aug 05 15:05:20 2016 +0000
Revision:
4:636bb3f66dcd
Parent:
1:73543a1fbe62 
??????

Who changed what in which revision?

UserRevisionLine numberNew 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 1:73543a1fbe62 21 printf("MPU9150 initialization failured\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 1:73543a1fbe62 35 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 1:73543a1fbe62 43 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 // 加速度センサー生値
yakatano 4:636bb3f66dcd 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
yakatano 4:636bb3f66dcd 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
yakatano 4:636bb3f66dcd 83 #if 1
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 1:73543a1fbe62 101 putc('$'); //packet start
yishii 1:73543a1fbe62 102 putc((char)2); //assume packet type constant
yishii 0:65a0ae5578da 103
yishii 1:73543a1fbe62 104 putc(buffer[0]);
yishii 1:73543a1fbe62 105 putc(buffer[1]);
yishii 1:73543a1fbe62 106 putc(buffer[4]);
yishii 1:73543a1fbe62 107 putc(buffer[5]);
yishii 1:73543a1fbe62 108 putc(buffer[8]);
yishii 1:73543a1fbe62 109 putc(buffer[9]);
yishii 1:73543a1fbe62 110 putc(buffer[12]);
yishii 1:73543a1fbe62 111 putc(buffer[13]);
yishii 0:65a0ae5578da 112
yishii 1:73543a1fbe62 113 putc((char)n++);
yishii 1:73543a1fbe62 114 putc((char)0);
yishii 1:73543a1fbe62 115 putc('\r');
yishii 1:73543a1fbe62 116 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 }