Diff: vz_protocol.cpp

Revision:

21:0bd688722e81

Parent:

20:2d6ab23956e5

Child:

23:4a6d631a6298

--- a/vz_protocol.cpp	Sat Sep 20 13:34:52 2014 +0000
+++ b/vz_protocol.cpp	Sat Sep 20 20:28:24 2014 +0000
@@ -23,6 +23,44 @@
     
     if( cb_buffer == NULL ) return( NULL );
     
+     if (cb_buffer[6] == 64) {
+        static uint8_t flood_cnt = 0;
+        static uint8_t first_run_flag = 0;
+        static Timer t;
+        if (first_run_flag == 0) {
+            flood_cnt = cb_buffer[0];
+            first_run_flag++;
+            t.start();
+        } else {
+            if (cb_buffer[0] == 0x00){
+                pc.printf("%d", t.read_us());
+                }
+            flood_cnt++;
+            while (flood_cnt != cb_buffer[0]) {
+                pc.printf("0");
+                flood_cnt++;
+            }
+            int i;
+            int ok = 1;
+            uint8_t cmp;
+            for (i=0, cmp=cb_buffer[0]; (i<300) && (ok==1); i++, cmp++) {
+                if (i != 6) {
+                    if (cb_buffer[i] != cmp) ok = 0;
+                }
+            }
+            if (ok) {
+                pc.printf("+");
+            } else {
+                pc.printf("%02x",cb_buffer[0] );
+                pc.printf("#");
+                pc.printf("\n\r");
+                for (int i = 0; i< 300 ; i++)   {pc.printf("%x", cb_buffer[i]);}
+                
+            }
+        }
+        return( NULL );
+    }
+    
     uint16_t cc = ( uint16_t )cb_buffer[ 4 ] << 8 | cb_buffer[ 5 ];
     
     /*
@@ -42,9 +80,6 @@
     debug.sendTo( debug_server, debug_msg, strlen( debug_msg ) );
     */
     
-    pc.printf("\n\r PKG_CSK OK: %d", pkg_cksok);
-    pc.printf("\n\r PKG_CSK ERR: %d\n\r", pkg_ckserr); 
-    
     if( cc != __checksum__( cb_buffer, __CB_BUFFER_SIZE__ ) ){
             /*pc.printf("\n\r CKS ERROR:  ");
             for (int i=0; i< 300; i++ ){ pc.printf("%x ", cb_buffer[i]); }
@@ -97,7 +132,15 @@
         //__print_clock__( pkg + 8 );
         pkg[ __TIMESLICE_PLACE__ ] = cb_buffer[ __TIMESLICE_PLACE__ ];
         fill = __TIMESLICE_PLACE__ + 1;
-    }else if( type == __TELEMETRY__ ){
+    }
+    
+    if( type == __PROMPT__ ){
+        //__print_clock__( pkg + 8 );
+        xmemcpy((pkg+7), (uint8_t*)cb_buffer, 293);
+        fill = 100;
+    }
+    
+    else if( type == __TELEMETRY__ ){
         pkg[ 7 ] = seq_num;
         //__print_clock__( pkg + 8 );
         pkg[ __TIMESLICE_PLACE__ ] = cb_buffer[ __TIMESLICE_PLACE__ ];