AP_HAL_SITL: implement new AP_HAL functions

Implement the new AP_HAL functions and use them in the Scheduler when
possible.

The '_sketch_start_time' was renamed and moved as a detail of
implementation of the functions code. It allows the code to return time
starting from zero.

The 'stopped_clock_usec' was renamed to follow convention in the file
and add a getter so that AP_HAL functions can reach it. It's not a
problem this getter is public because in practice, regular code
shouldn't even access the SITLScheduler directly -- only code that
should is from SITL itself.

libraries/AP_HAL_SITL/Scheduler.cpp

@@ -27,66 +27,47 @@ AP_HAL::MemberProc SITLScheduler::_io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NU
 uint8_t SITLScheduler::_num_io_procs = 0;
 bool SITLScheduler::_in_io_proc = false;
 
-struct timeval SITLScheduler::_sketch_start_time;
-
 SITLScheduler::SITLScheduler(SITL_State *sitlState) :
     _sitlState(sitlState),
-    stopped_clock_usec(0)
+    _stopped_clock_usec(0)
 {
 }
 
 void SITLScheduler::init(void *unused)
 {
-    gettimeofday(&_sketch_start_time,NULL);
 }
 
 uint64_t SITLScheduler::_micros64()
 {
-    struct timeval tp;
-    gettimeofday(&tp,NULL);
-    uint64_t ret = 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
-                          (_sketch_start_time.tv_sec +
-                           (_sketch_start_time.tv_usec*1.0e-6)));
-    return ret;
+    return AP_HAL::micros64();
 }
 
 uint64_t SITLScheduler::micros64()
 {
-    if (stopped_clock_usec) {
-        return stopped_clock_usec;
-    }
-    return _micros64();
+    return AP_HAL::micros64();
 }
 
 uint32_t SITLScheduler::micros()
 {
-    return micros64() & 0xFFFFFFFF;
+    return AP_HAL::micros();
 }
 
 uint64_t SITLScheduler::millis64()
 {
-    if (stopped_clock_usec) {
-        return stopped_clock_usec/1000;
-    }
-    struct timeval tp;
-    gettimeofday(&tp,NULL);
-    uint64_t ret = 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
-                          (_sketch_start_time.tv_sec +
-                           (_sketch_start_time.tv_usec*1.0e-6)));
-    return ret;
+    return AP_HAL::millis64();
 }
 
 uint32_t SITLScheduler::millis()
 {
-    return millis64() & 0xFFFFFFFF;
+    return AP_HAL::millis();
 }
 
 void SITLScheduler::delay_microseconds(uint16_t usec)
 {
-    uint64_t start = micros64();
+    uint64_t start = AP_HAL::micros64();
     uint64_t dtime;
-    while ((dtime=(micros64() - start) < usec)) {
-        if (stopped_clock_usec) {
+    while ((dtime=(AP_HAL::micros64() - start) < usec)) {
+        if (_stopped_clock_usec) {
             _sitlState->wait_clock(start+usec);
         } else {
             usleep(usec - dtime);
@@ -171,7 +152,7 @@ bool SITLScheduler::system_initializing() {
 
 void SITLScheduler::system_initialized() {
     if (_initialized) {
-        panic(
+        AP_HAL::panic(
             "PANIC: scheduler system initialized called more than once");
     }
     int exceptions = FE_OVERFLOW | FE_DIVBYZERO;
@@ -276,7 +257,7 @@ void SITLScheduler::panic(const char *errormsg, ...)
  */
 void SITLScheduler::stop_clock(uint64_t time_usec)
 {
-    stopped_clock_usec = time_usec;
+    _stopped_clock_usec = time_usec;
     _run_io_procs(false);
 }
 

libraries/AP_HAL_SITL/Scheduler.h

@@ -59,12 +59,13 @@ public:
         _run_io_procs(true);
     }
 
+    uint64_t stopped_clock_usec() const { return _stopped_clock_usec; }
+
 private:
     SITL_State *_sitlState;
     uint8_t _nested_atomic_ctr;
     AP_HAL::Proc _delay_cb;
     uint16_t _min_delay_cb_ms;
-    static struct timeval _sketch_start_time;
     static AP_HAL::Proc _failsafe;
 
     static void _run_timer_procs(bool called_from_isr);
@@ -82,7 +83,7 @@ private:
     void stop_clock(uint64_t time_usec);
 
     bool _initialized;
-    uint64_t stopped_clock_usec;
+    uint64_t _stopped_clock_usec;
 };
 #endif
 #endif // __AP_HAL_SITL_SCHEDULER_H__

libraries/AP_HAL_SITL/system.cpp

@@ -1,9 +1,78 @@
+#include <stdarg.h>
+#include <stdio.h>
+
+#include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/system.h>
 
+#include "Scheduler.h"
+
+extern const AP_HAL::HAL& hal;
+
+using HALSITL::SITLScheduler;
+
 namespace AP_HAL {
 
+static struct {
+    struct timeval start_time;
+} state;
+
 void init()
 {
+    gettimeofday(&state.start_time, NULL);
+}
+
+void panic(const char *errormsg, ...)
+{
+    va_list ap;
+
+    va_start(ap, errormsg);
+    vprintf(errormsg, ap);
+    va_end(ap);
+    printf("\n");
+
+    for(;;);
+}
+
+uint32_t micros()
+{
+    return micros64() & 0xFFFFFFFF;
+}
+
+uint32_t millis()
+{
+    return millis64() & 0xFFFFFFFF;
+}
+
+uint64_t micros64()
+{
+    const SITLScheduler* scheduler = SITLScheduler::from(hal.scheduler);
+    uint64_t stopped_usec = scheduler->stopped_clock_usec();
+    if (stopped_usec) {
+        return stopped_usec;
+    }
+
+    struct timeval tp;
+    gettimeofday(&tp, NULL);
+    uint64_t ret = 1.0e6 * ((tp.tv_sec + (tp.tv_usec * 1.0e-6)) -
+                            (state.start_time.tv_sec +
+                             (state.start_time.tv_usec * 1.0e-6)));
+    return ret;
+}
+
+uint64_t millis64()
+{
+    const SITLScheduler* scheduler = SITLScheduler::from(hal.scheduler);
+    uint64_t stopped_usec = scheduler->stopped_clock_usec();
+    if (stopped_usec) {
+        return stopped_usec / 1000;
+    }
+
+    struct timeval tp;
+    gettimeofday(&tp, NULL);
+    uint64_t ret = 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) -
+                          (state.start_time.tv_sec +
+                           (state.start_time.tv_usec*1.0e-6)));
+    return ret;
 }
 
 } // namespace AP_HAL