AP_HAL: add timeout helper functions

add tests for timeout functions

libraries/AP_HAL/system.h

@@ -22,4 +22,32 @@ uint64_t millis64();
 void dump_stack_trace();
 void dump_core_file();
 
+// Function to reliably determine whether a timeout has expired using any unsigned time type and interval
+// The template makes sure that callers do not mix up different types for storing time or assessing the
+// current time.
+// The comparison is guaranteed to work even if the present and the past cross a wrap boundary
+template <typename T, typename S, typename R>
+inline bool timeout_expired(const T past_time, const S now, const R timeout)
+{
+    static_assert(std::is_same<T, S>::value, "timeout_expired() must compare values of the same unsigned type");
+    static_assert(std::is_unsigned<T>::value, "timeout_expired() must use unsigned times");
+    static_assert(std::is_unsigned<R>::value, "timeout_expired() must use unsigned timeouts");
+    const T dt = now - past_time;
+    return (dt >= timeout);
+}
+
+// Function to reliably determine whether how much of a timeout is left using any unsigned time type and interval
+// The template makes sure that callers do not mix up different types for storing time or assessing the
+// current time.
+// The comparison is guaranteed to work even if the present and the past cross a wrap boundary
+template <typename T, typename S, typename R>
+inline T timeout_remaining(const T past_time, const S now, const R timeout)
+{
+    static_assert(std::is_same<T, S>::value, "timeout_remaining() must compare values of the same unsigned type");
+    static_assert(std::is_unsigned<T>::value, "timeout_remaining() must use unsigned times");
+    static_assert(std::is_unsigned<R>::value, "timeout_remaining() must use unsigned timeouts");
+    const T dt = now - past_time;
+    return (dt >= timeout) ? T(0) : (timeout - dt);
+}
+
 } // namespace AP_HAL

libraries/AP_HAL/tests/test_timeouts.cpp

@@ -0,0 +1,72 @@
+#include <AP_gtest.h>
+#include <AP_HAL/HAL.h>
+#include <AP_HAL/Util.h>
+#include <AP_Math/AP_Math.h>
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+
+template <typename T>
+static void timeout_test(T timenow, T trigger)
+{
+    T ticks = 2000U;
+    T time_past = timenow;
+    bool timeout_triggered = false;
+
+    while (ticks-- != 0) {
+        timenow++;
+        if (AP_HAL::timeout_expired(time_past, timenow, 1000U)) {
+            if (!timeout_triggered) {
+                ASSERT_EQ(timenow, trigger);
+                timeout_triggered = true;
+            }
+        }
+    }
+
+    ASSERT_TRUE(timeout_triggered);
+}
+
+TEST(timeout_wrap_Test, Basic)
+{
+    timeout_test(0xFFFFFFFFU - 499U, 500U);
+}
+
+TEST(timeout_Test, Basic)
+{
+    timeout_test(500U, 1500U);
+}
+
+template <typename T>
+static void timeout_remaining(T timenow, T incr)
+{
+    T ticks = 2000U;
+    T time_past = timenow;
+    T last_remaining = 1000;
+
+    while (ticks-- != 0) {
+        timenow+=incr;
+        const uint32_t new_remaining = AP_HAL::timeout_remaining(time_past, timenow, 1000U);
+        ASSERT_LT(new_remaining, MAX(last_remaining, 1U));   // time remaining must be going down
+        last_remaining = new_remaining;
+        ASSERT_LT(new_remaining, 1001U);
+    }
+
+    ASSERT_EQ(AP_HAL::timeout_remaining(time_past, timenow, 1000U), 0U);
+}
+
+TEST(timeout_remaining_wrap_Test, Basic)
+{
+    timeout_remaining(0xFFFFFFFFU - 500U, 1U);
+}
+
+TEST(timeout_remaining_Test, Basic)
+{
+    timeout_remaining(500U, 1U);
+}
+
+TEST(timeout_remaining_wrap_Test_5us, Basic)
+{
+    timeout_remaining(0xFFFFFFFF - 500U, 5U);
+}
+
+AP_GTEST_MAIN()