Diff: taskSensor.cpp

Revision:

0:65a0ae5578da

Child:

1:73543a1fbe62

diff -r 000000000000 -r 65a0ae5578da taskSensor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/taskSensor.cpp	Sat May 14 07:11:48 2016 +0000
@@ -0,0 +1,134 @@
+#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 initialisation failure\r\n");
+        while(1){
+            Thread::wait(1000);
+        }
+    }
+    imu.initialiseDMP();
+    imu_initialized = true;
+}
+
+static char buffer[200];
+
+void sensor_main(void)
+{
+    Quaternion q1;
+    pc.printf("Entered to sensor_main\r\n");
+    float q[4];
+    uint8_t n = 0;
+    uint16_t val;
+    
+    imu.setFifoReset(true);    
+    imu.setDMPEnabled(true);    
+        
+    pc.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 0
+            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
+            
+            pc.putc('$'); //packet start
+            pc.putc((char)2); //assume packet type constant
+
+            pc.putc(buffer[0]);
+            pc.putc(buffer[1]);
+            pc.putc(buffer[4]);
+            pc.putc(buffer[5]);
+            pc.putc(buffer[8]);
+            pc.putc(buffer[9]);
+            pc.putc(buffer[12]);
+            pc.putc(buffer[13]);
+
+            pc.putc((char)n++);
+            pc.putc((char)0);
+            pc.putc('\r');
+            pc.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();
+    }
+}