Diff: utils.cpp

Revision:

58:af7e8788f106

Parent:

56:c64bcbaa621a

Child:

62:07e5bdc9f8f7

--- a/utils.cpp	Thu Oct 23 17:18:58 2014 +0000
+++ b/utils.cpp	Thu Oct 23 19:58:09 2014 +0000
@@ -221,7 +221,7 @@
 }
 
 void build_telemetry_report( int ext, int port, char * data ){
-    char aux[ __CB_BUFFER_SIZE__ ];
+    char aux[ __CB_BUFFER_SIZE__ + 6 ];
     char tmp[ 6 ];
     strcpy( aux, itoa( ext, tmp, 10 ) );
     strcat( aux, " " );
@@ -232,16 +232,19 @@
     data += __CLOCK_SYNC_SIZE__;
     //pulando o byte de id ( sequence number )
     data += __SEQ_NUM_SIZE__;
-    for( register uint16_t i = strlen( aux ); i < __TELEMETRY_SIZE__; i++ ) aux[ i ] = data[ i ];
+    
+    uint8_t offset = strlen( aux );
     
-    for( register uint16_t i = __TELEMETRY_SIZE__; i < __CB_BUFFER_SIZE__; i++ ) aux[ i ] = 0;
+    for( register uint16_t i = 0; i < __TELEMETRY_SIZE__; i++ ) aux[ i + offset ] = data[ i ];
+    
+    for( register uint16_t i = __TELEMETRY_SIZE__ + offset; i < __CB_BUFFER_SIZE__; i++ ) aux[ i ] = 0;
     
     if( debug_telemetry ){
         pc.printf("\n\r 1. ");
         uint8_t count = 2;
         for( register int i = 0; i < __CB_BUFFER_SIZE__; i++ ){
-            if( i > 0 && !( i % 15 )  ) pc.printf("\n\r%2u. ", count++ );
-            pc.printf("%3i ", aux[ i ] );
+            if( i > 0 && !( i % 15 )  ) pc.printf("\n\r%2i. ", count++ );
+            pc.printf("%3x ", aux[ i ] );
         }
         pc.printf("\n\r");