[paparazzi-commits] [4239] Fix onboard_logger (UDP logger)...

[Allen Ibara]

Revision: 4239
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4239
Author:   aibara
Date:     2009-10-08 22:51:02 +0000 (Thu, 08 Oct 2009)
Log Message:
-----------
Fix onboard_logger (UDP logger)... was badly parsing packets/skipping messages

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/fms/onboard_logger.c

Modified: paparazzi3/trunk/sw/airborne/fms/onboard_logger.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/onboard_logger.c   2009-10-08 07:04:13 UTC 
(rev 4238)
+++ paparazzi3/trunk/sw/airborne/fms/onboard_logger.c   2009-10-08 22:51:02 UTC 
(rev 4239)
@@ -67,19 +67,19 @@
   udp_length = htons(udp->uh_len);
   //printf ("Got udp packet length %i\n", udp_length);
   //for ( i = 0; i < udp_length; i++)
-  //printf ("%02x ", payload[i]);
+  //printf ("[%02i] \n", payload[i]);
 
   i = 0;
   while (i < udp_length) {
-    if (payload[i++] == 0x99) {
-      //printf("got startbyte at %i\n", i);
-      if (i + payload[i] + 2 <= udp_length) {
-       got_pprz_message(payload + i, &header->ts);
-       i += payload[i] + 2;
-      } else {
-       return; // truncated
-      }
-    }
+    if (payload[i] != 0x99) {
+      ///printf("missing start byte\n");
+      break;
+    } 
+    i++;
+    if (i + payload[i] <= udp_length) {
+      got_pprz_message(payload + i, &header->ts);
+      i += payload[i] - 1;
+    } 
   }
 }