Don't use failed reads. Add debugging.

conf/airframes/ardrone2_sdk.xml

@@ -6,6 +6,7 @@
 		<target name="ap" board="ardrone2_sdk">
 			<define name="USE_INS_NAV_INIT" />
 			<define name="ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED"/>
+			<!-- <define name="ARDRONE2_DEBUG" /> -->
 			<!-- <define name="USE_GPS_HEIGHT" /> -->
 			<!-- <define name="ARDRONE_FLIGHT_INDOOR" /> -->
 			<!-- <define name="ARDRONE_WITHOUT_SHELL" /> -->

sw/airborne/boards/ardrone/at_com.c

@@ -133,10 +133,14 @@ void init_at_config(void) {
 }
 
 //Recieve a navdata packet
-void at_com_recieve_navdata(unsigned char* buffer) {
-  int l;
-  recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0,
+int at_com_recieve_navdata(unsigned char* buffer) {
+  int l = sizeof(from);
+  int n;
+  // FIXME(ben): not clear why recvfrom() and not recv() is used.
+  n = recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0,
 	       (struct sockaddr *) &from, (socklen_t *) &l);
+
+  return n;
 }
 
 //Send an AT command

sw/airborne/boards/ardrone/at_com.h

@@ -202,7 +202,7 @@ typedef struct _navdata_gps_t {
 
 //External functions
 extern void init_at_com(void);
-extern void at_com_recieve_navdata(unsigned char* buffer);
+extern int at_com_recieve_navdata(unsigned char* buffer);
 extern void at_com_send_config(char* key, char* value);
 extern void at_com_send_ftrim(void);
 extern void at_com_send_ref(int bits);

sw/airborne/subsystems/ahrs/ahrs_ardrone2.c

@@ -27,6 +27,11 @@
  * and also sets battery level.
  */
 
+#ifdef ARDRONE2_DEBUG
+# include <errno.h>
+# include <stdio.h>
+#endif
+
 #include "ahrs_ardrone2.h"
 #include "state.h"
 #include "math/pprz_algebra_float.h"
@@ -55,15 +60,42 @@ void ahrs_align(void) {
 
 }
 
+#ifdef ARDRONE2_DEBUG
+static void dump(const void *_b, size_t s) {
+  const unsigned char *b = _b;
+  size_t n;
+
+  for(n = 0; n < s; ++n) {
+    printf("%02x ", b[n]);
+    if (n%16 == 15)
+      printf("\n");
+  }
+  if (n%16 != 0)
+    printf("\n");
+}
+#endif
+
 void ahrs_propagate(void) {
+  int l;
+
   //Recieve the main packet
-  at_com_recieve_navdata(buffer);
+  l = at_com_recieve_navdata(buffer);
   navdata_t* main_packet = (navdata_t*) &buffer;
 
+#ifdef ARDRONE2_DEBUG
+  if (l < 0)
+    printf("errno = %d\n", errno);
+#endif
+
   //When this isn't a valid packet return
-  if(main_packet->header != NAVDATA_HEADER)
+  if(l < 0 || main_packet->header != NAVDATA_HEADER)
     return;
 
+#ifdef ARDRONE2_DEBUG
+  printf("Read %d\n", l);
+  dump(buffer, l);
+#endif
+
   //Set the state
   ahrs_impl.state = main_packet->ardrone_state;
 
@@ -78,6 +110,9 @@ void ahrs_propagate(void) {
 
   //Read the navdata until packet is fully readed
   while(!full_read && navdata_option->size > 0) {
+#ifdef ARDRONE2_DEBUG
+    printf ("tag = %d\n", navdata_option->tag);
+#endif
     //Check the tag for the right option
     switch(navdata_option->tag) {
     case 0: //NAVDATA_DEMO
@@ -112,6 +147,9 @@ void ahrs_propagate(void) {
       break;
 #ifdef USE_GPS_ARDRONE2
     case 27: //NAVDATA_GPS
+# ifdef ARDRONE2_DEBUG
+      dump(navdata_option, navdata_option->size);
+# endif
       navdata_gps = (navdata_gps_t*) navdata_option;
 
       // Send the data to the gps parser
@@ -123,7 +161,9 @@ void ahrs_propagate(void) {
       full_read = TRUE;
       break;
     default:
-      //printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
+#ifdef ARDRONE2_DEBUG
+      printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
+#endif
       break;
     }
     navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size);

sw/airborne/subsystems/gps/gps_ardrone2.c

@@ -26,6 +26,10 @@
  * ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2.
  */
 
+#ifdef ARDRONE2_DEBUG
+# include <stdio.h>
+#endif
+
 #include "subsystems/gps.h"
 #include "math/pprz_geodetic_double.h"
 
@@ -37,6 +41,10 @@ void gps_impl_init( void ) {
 
 void gps_ardrone2_parse(navdata_gps_t *navdata_gps) {
   int i;
+
+#ifdef ARDRONE2_DEBUG
+  printf("state = %d\n", navdata_gps->gps_state);
+#endif
   // Set the lla double struct from the navdata
   struct LlaCoor_d gps_lla_d;
   gps_lla_d.lat = RadOfDeg(navdata_gps->lat);