Diff: main.cpp

Revision:

22:10de7ce8fee3

Parent:

21:06a9547359dc

Child:

23:2e1ee10d4ee0

diff -r 06a9547359dc -r 10de7ce8fee3 main.cpp
--- a/main.cpp	Wed Jun 29 02:27:17 2016 +0000
+++ b/main.cpp	Wed Jun 29 04:35:10 2016 +0000
@@ -43,6 +43,9 @@
 
 Ticker          ticker;
 
+float putg[3] = {0};
+float putq[4] = {0};
+
 void init(void)
 {
     pc.baud(115200); //921600
@@ -119,22 +122,33 @@
     #endif
     wait_us(150);
     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]);
-            */
+            putg[0] = imu[i]->gyroscope_data[0];
+            putg[1] = imu[i]->gyroscope_data[1];
+            putg[2] = imu[i]->gyroscope_data[2];
+            putq[0] = ahrs[i]->q0;
+            putq[1] = ahrs[i]->q1;
+            putq[2] = ahrs[i]->q2;
+            putq[3] = ahrs[i]->q3;
+            
             /*
             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,
-               imu[i]->gyroscope_data[2]*DEGREE2RAD,
-               ahrs[i]->q0,
-               ahrs[i]->q1,
-               ahrs[i]->q2,
-               ahrs[i]->q3);
+                putg[0]*DEGREE2RAD,
+                putg[1]*DEGREE2RAD,
+                putg[2]*DEGREE2RAD,
+                putq[0],
+                putq[1],
+                putq[2],
+                putq[3]);
             */
+            
+            float2byte(putg[0],32.8);
+            float2byte(putg[1],32.8);
+            float2byte(putg[2],32.8);
+            float2byte(putq[0],32800);
+            float2byte(putq[1],32800);
+            float2byte(putq[2],32800);
+            float2byte(putq[3],32800);      
+            
             /*
             //test signal 29,June wada
             float2byte(-200,32.8);
@@ -145,20 +159,12 @@
             float2byte( 0.4,32800);
             float2byte( 0.8,32800);
             */
-            
-            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");
+    printf("\n");
 }
 
 int main()