AP_HAL: added TIME_CHECK() macro

this can be used to find places where we use more time than
expected. It works similarly to WITH_SEMAPHORE()

libraries/AP_HAL/Scheduler.cpp

@@ -1,5 +1,6 @@
 #include "Scheduler.h"
 #include "AP_HAL.h"
+#include <stdio.h>
 
 using namespace AP_HAL;
 
@@ -35,3 +36,23 @@ ExpectDelay::~ExpectDelay()
 {
     hal.scheduler->expect_delay_ms(0);
 }
+
+/*
+  implement TimeCheck class for TIME_CHECK() support
+ */
+TimeCheck::TimeCheck(uint32_t _limit_ms, const char *_file, uint32_t _line) :
+    limit_ms(_limit_ms),
+    file(_file),
+    line(_line)
+{
+    start_ms = AP_HAL::millis();
+}
+
+TimeCheck::~TimeCheck()
+{
+    const uint32_t end_ms = AP_HAL::millis();
+    const uint32_t delta_ms = end_ms - start_ms;
+    if (delta_ms > limit_ms) {
+        ::printf("Delta %u at %s:%u\n", unsigned(delta_ms), file, unsigned(line));
+    }
+}

libraries/AP_HAL/Scheduler.h

@@ -143,3 +143,24 @@ public:
 #define EXPECT_DELAY_MS(ms) DELAY_JOIN( ms, __COUNTER__ )
 #define DELAY_JOIN( ms, counter) _DO_DELAY_JOIN( ms, counter )
 #define _DO_DELAY_JOIN( ms, counter ) ExpectDelay _getdelay ## counter(ms)
+
+
+/*
+  TIME_CHECK() can be used to unexpected detect long delays. Scatter
+  them in likely places and any long delays will be printed
+ */
+
+class TimeCheck {
+public:
+    TimeCheck(uint32_t limit_ms, const char *file, uint32_t line);
+    ~TimeCheck();
+private:
+    const uint32_t limit_ms;
+    const uint32_t line;
+    const char *file;
+    uint32_t start_ms;
+};
+
+#define TIME_CHECK(limit_ms) JOIN_TC(limit_ms, __FILE__, __LINE__, __COUNTER__ )
+#define JOIN_TC(limit_ms, file, line, counter ) _DO_JOIN_TC( limit_ms, file, line, counter )
+#define _DO_JOIN_TC(limit_ms, file, line, counter ) TimeCheck _gettc ## counter(limit_ms, file, line)