diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c
index 90841a9534..b337f37510 100644
--- a/sw/airborne/boards/ardrone/navdata.c
+++ b/sw/airborne/boards/ardrone/navdata.c
@@ -329,7 +329,11 @@ static void mag_freeze_check(void)
       //DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val);
 
       // wait 40ms to retrieve data
-      usleep(40000);
+      // using 40 times a 1ms wait in case the usleep function
+      // is interupted by a signal
+      for (int i = 0; i < 40; i++) {
+        usleep(1000);
+      }
 
       // restart acquisition
       cmd = 0x01;