Rover: use ins.wait_for_sample()

APMrover2/APMrover2.pde

@@ -604,10 +604,12 @@ void setup() {
  */
 void loop()
 {
+    // wait for an INS sample
+    if (!ins.wait_for_sample(1000)) {
+        return;
+    }
     uint32_t timer = millis();
 
-    // We want this to execute at 50Hz, synchronised with the gyro/accel
-    if (ins.sample_available()) {
     delta_ms_fast_loop	= timer - fast_loopTimer;
     G_Dt                = (float)delta_ms_fast_loop / 1000.f;
     fast_loopTimer      = timer;
@@ -623,9 +625,6 @@ void loop()
     fast_loopTimeStamp = millis();
 
     scheduler.run(19000U);
-    } else {
-        hal.scheduler->delay(1);
-    }
 }
 
 // Main loop 50Hz