HAL_Linux: improved latency of timed semaphores

libraries/AP_HAL_Linux/Semaphores.cpp

@@ -18,11 +18,17 @@ bool LinuxSemaphore::take(uint32_t timeout_ms)
     if (timeout_ms == 0) {
         return pthread_mutex_lock(&_lock) == 0;
     }
-    while (timeout_ms-- > 0) {
-        if (take_nonblocking()) return true;
-        hal.scheduler->delay(1);
+    if (take_nonblocking()) {
+        return true;
     }
-    return take_nonblocking();
+    uint32_t start = hal.scheduler->micros();
+    do {
+        hal.scheduler->delay_microseconds(200);
+        if (take_nonblocking()) {
+            return true;
+        }
+    } while ((hal.scheduler->micros() - start) < timeout_ms*1000);
+    return false;
 }
 
 bool LinuxSemaphore::take_nonblocking()