Diff: main.cpp

Revision:

19:3fc0ecfc723a

Parent:

18:a4d18f4eb968

Child:

20:01bba4071ffc

--- a/main.cpp	Tue Jun 28 06:18:52 2016 +0000
+++ b/main.cpp	Tue Jun 28 13:41:15 2016 +0000
@@ -10,7 +10,7 @@
 #include "mbed.h"
 #include "MPU9250.h"
 #include "MadgwickAHRS.h"
-
+#define DEBUG_putc  //Wada
 
 /* MPU9250 Library
  *
@@ -45,7 +45,7 @@
 
 void init(void)
 {
-    pc.baud(9600); //921600
+    pc.baud(115200); //921600
 
     imu[0] = new mpu9250_spi(spi, p8);
     imu[1] = new mpu9250_spi(spi, p9);
@@ -72,6 +72,23 @@
     }
 }
 
+// Wada June 17, 2016
+void float2byte(float gq, float k)//k = gyro_divider 32.8 or q_divider 32800
+{
+    float omegaF = gq * k;
+    int16_t omegaI = (int16_t)omegaF;
+    if(omegaI < 0){//1の補数
+        omegaI = 0xFFFF + omegaI;
+        }
+    for(i=0;i<4;i++){
+        tmp = (0xF & (omegaI >> (i*4)));
+        #ifdef DEBUG_putc
+        pc.putc(tmp);
+        #endif      
+    }        
+}//float2byte
+
+
 void eventFunc(void)
 {    
     for(int i=0; i<2; i++) {
@@ -95,14 +112,17 @@
         imu[i]->Magnetometer[1],
         imu[i]->Magnetometer[2]
         );
-        
-    for(int i=0; i<2; i++) 
+    #ifdef DEBUG_putc
+    pc.putc(0x34);  //STX
+    #endif    
+    for(int i=0; i<2; i++) {
             /*
             printf("%+1.3f,%+1.3f,%+1.3f,",
                 imu[i]->accelerometer_data[0],
                 imu[i]->accelerometer_data[1],
                 imu[i]->accelerometer_data[2]);
             */
+            /*
             printf("%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,",
                imu[i]->gyroscope_data[0]*DEGREE2RAD,
                imu[i]->gyroscope_data[1]*DEGREE2RAD,
@@ -111,8 +131,19 @@
                ahrs[i]->q1,
                ahrs[i]->q2,
                ahrs[i]->q3);
-
-    printf("\n");
+            */
+            float2byte(imu[i]->gyroscope_data[0],32.8);
+            float2byte(imu[i]->gyroscope_data[1],32.8);
+            float2byte(imu[i]->gyroscope_data[2],32.8);
+            float2byte(ahrs[i]->q0,32800);
+            float2byte(ahrs[i]->q1,32800);
+            float2byte(ahrs[i]->q2,32800);
+            float2byte(ahrs[i]->q3,32800);
+    }
+    #ifdef DEBUG_putc
+    pc.putc(0x12);  //ETX
+    #endif    
+    //printf("\n");
 }
 
 int main()