AP_InertialSensor: yield the CPU for the right time in wait_for_sample()

this improves timing performance

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -159,7 +159,13 @@ bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
     }
     uint32_t start = hal.scheduler->millis();
     while ((hal.scheduler->millis() - start) < timeout_ms) {
-        hal.scheduler->delay_microseconds(100);
+        uint64_t tnow = hrt_absolute_time();
+        // we spin for the last timing_lag microseconds. Before that
+        // we yield the CPU to allow IO to happen
+        const uint16_t timing_lag = 400;
+        if (_last_sample_timestamp + _sample_time_usec > tnow+timing_lag) {
+            hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_time_usec - (tnow+timing_lag));
+        }
         if (sample_available()) {
             return true;
         }