From b2938be9d5cc4edb4c7327d3893eebc9dfba34ac Mon Sep 17 00:00:00 2001
From: OpenUAS <noreply@openuas.org>
Date: Mon, 8 Dec 2014 22:41:21 +0100
Subject: [PATCH] Fix for magneto fix that still kept hanging sometimes

---
 sw/airborne/boards/ardrone/navdata.c | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c
index 244ddbbed0..079a5c70fa 100644
--- a/sw/airborne/boards/ardrone/navdata.c
+++ b/sw/airborne/boards/ardrone/navdata.c
@@ -332,6 +332,7 @@ static void mag_freeze_check(void) {
 
         // do the navboard reset via GPIOs
         gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA);
+        usleep(20000); //Otherwise set sometime does not work
         gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA);
 
         //// wait 20ms to retrieve data
@@ -339,7 +340,12 @@ static void mag_freeze_check(void) {
 
         //// restart acquisition
         cmd = 0x01;
-        navdata_write(&cmd, 1);
+        usleep(5000);
+        for (int i=0;i<10;i++)
+        {
+          usleep(1000);
+          navdata_write(&cmd, 1);
+        }
       }
 
       MagFreezeCounter = 0; // reset counter back to zero
@@ -499,6 +505,7 @@ void navdata_update()
   // Check if initialized
   if (!nav_port.isInitialized) {
     navdata_init();
+    mag_freeze_check();
     return;
   }