Diff: main.cpp

Revision:

1:f02249a6e8bb

Parent:

0:bada2c7bd577

Child:

2:487359a1a439

--- a/main.cpp	Tue Apr 08 10:28:49 2014 +0000
+++ b/main.cpp	Wed Apr 09 11:31:53 2014 +0000
@@ -8,6 +8,9 @@
 #include <iostream>
 #include <string>
 #include <fstream>
+#include <algorithm>
+
+
 DigitalOut myled(LED1);
 MODSERIAL pc(USBTX, USBRX, "modser"); //modified modser lib to be able to input custom stream
 
@@ -17,37 +20,56 @@
 int main() {
     pc.baud(115200);
     status_message Status;
-    gyro_message Gyro;
+    gyro_message GyroOut, GyroIn;
     
     pc.claim();
-    uint8_t buffer[128];
+    uint8_t bufferout[150];
+    uint8_t bufferin[150];
     
-    pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
+    pb_ostream_t streamout = pb_ostream_from_buffer(bufferout, sizeof(bufferout));
+    
     Status.Mode=1;
-    Gyro.X=1.1;
-    Gyro.Y=2.1;
-    Gyro.Z=3.1;
+    GyroOut.X=1.1;
+    GyroOut.Y=2.1;
+    GyroOut.Z=3.1;
+    pc.printf("starting..\r\n");
     while(1){
-        Gyro.X+=0.1;
-        Gyro.Y+=0.2;
-        Gyro.Z+=0.3;
+        GyroOut.X+=0.1;
+        GyroOut.Y+=0.2;
+        GyroOut.Z+=0.3;
         
-        if (pb_encode(&stream, gyro_message_fields, &Gyro)) {
-            pc.putc('$');
+        if (pb_encode(&streamout, gyro_message_fields, &GyroOut)) {
+            /*pc.putc('$');
             //fwrite(buffer, 1, stream.bytes_written, stdout);
             pc.printf("%s", buffer);
-            //pb_write(&stream, &buffer, 20);
-            //memset(&buffer[0], 0, sizeof(buffer)); //empty buffer
             pc.putc('e');
             pc.putc('n');
             pc.putc('d');
+            //pc.printf("x: %4.2f, y: %4.2f, z: %4.2f\r\n", Gyro.X, Gyro.Y, Gyro.Z);
             //stream*=0; //empty stream, not working as of now but creating error on mbed
+            //memset(&buffer[0], 0, sizeof(buffer)); //empty buffer*/
+
+            //new test code:
+            pc.printf("Raw values: x: %4.2f, y: %4.2f, z: %4.2f\r\n", GyroOut.X, GyroOut.Y, GyroOut.Z);
+            pc.printf("%s\r\n", bufferout);
         }
         
         else {
-            pc.printf("Encoding failed: %s\n", PB_GET_ERROR(&stream));
-            return 1;
-        }        
+            pc.printf("Encoding failed: %s\n", PB_GET_ERROR(&streamout));
+            return 0;
+        }
+        pc.printf("decoding...\r\n");
+        pb_istream_t streamin = pb_istream_from_buffer(bufferin, sizeof(bufferin));
+        for(int i=0;i<=streamout.bytes_written;i++) //copy output buffer to input buffer
+            bufferin[i]=bufferout[i];
+        if (pb_decode(&streamin, gyro_message_fields, &GyroIn)) { //decode message
+            pc.printf("Decoded values: x: %4.2f, y: %4.2f, z: %4.2f\r\n", GyroIn.X, GyroIn.Y, GyroIn.Z);
+        }
+        
+        else {
+            pc.printf("Decoding failed: %s\n", PB_GET_ERROR(&streamin));
+            return 0;
+        }
         wait(1);
     }
 }