AP_HAL_Linux: 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 Linux::Scheduler directly -- only code that
should is from Linux implementation.

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -9,7 +9,6 @@
 #include "Util.h"
 #include "SPIUARTDriver.h"
 #include "RPIOUARTDriver.h"
-#include <sys/time.h>
 #include <poll.h>
 #include <unistd.h>
 #include <stdlib.h>
@@ -69,7 +68,7 @@ void Scheduler::_create_realtime_thread(pthread_t *ctx, int rtprio,
     if (r != 0) {
         hal.console->printf("Error creating thread '%s': %s\n",
                             name, strerror(r));
-        panic("Failed to create thread");
+        AP_HAL::panic("Failed to create thread");
     }
     pthread_attr_destroy(&attr);
 
@@ -82,8 +81,6 @@ void Scheduler::init(void* machtnichts)
 {
     mlockall(MCL_CURRENT|MCL_FUTURE);
 
-    clock_gettime(CLOCK_MONOTONIC, &_sketch_start_time);
-
     struct sched_param param = { .sched_priority = APM_LINUX_MAIN_PRIORITY };
     sched_setscheduler(0, SCHED_FIFO, &param);
 
@@ -140,12 +137,12 @@ void Scheduler::_microsleep(uint32_t usec)
 
 void Scheduler::delay(uint16_t ms)
 {
-    if (stopped_clock_usec) {
+    if (_stopped_clock_usec) {
         return;
     }
-    uint64_t start = millis64();
+    uint64_t start = AP_HAL::millis64();
 
-    while ((millis64() - start) < ms) {
+    while ((AP_HAL::millis64() - start) < ms) {
         // this yields the CPU to other apps
         _microsleep(1000);
         if (_min_delay_cb_ms <= ms) {
@@ -158,41 +155,27 @@ void Scheduler::delay(uint16_t ms)
 
 uint64_t Scheduler::millis64()
 {
-    if (stopped_clock_usec) {
-        return stopped_clock_usec/1000;
-    }
-    struct timespec ts;
-    clock_gettime(CLOCK_MONOTONIC, &ts);
-    return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) -
-                  (_sketch_start_time.tv_sec +
-                   (_sketch_start_time.tv_nsec*1.0e-9)));
+    return AP_HAL::millis64();
 }
 
 uint64_t Scheduler::micros64()
 {
-    if (stopped_clock_usec) {
-        return stopped_clock_usec;
-    }
-    struct timespec ts;
-    clock_gettime(CLOCK_MONOTONIC, &ts);
-    return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) -
-                  (_sketch_start_time.tv_sec +
-                   (_sketch_start_time.tv_nsec*1.0e-9)));
+    return AP_HAL::micros64();
 }
 
 uint32_t Scheduler::millis()
 {
-    return millis64() & 0xFFFFFFFF;
+    return AP_HAL::millis();
 }
 
 uint32_t Scheduler::micros()
 {
-    return micros64() & 0xFFFFFFFF;
+    return AP_HAL::micros();
 }
 
 void Scheduler::delay_microseconds(uint16_t us)
 {
-    if (stopped_clock_usec) {
+    if (_stopped_clock_usec) {
         return;
     }
     _microsleep(us);
@@ -300,12 +283,12 @@ void *Scheduler::_timer_thread(void* arg)
       this aims to run at an average of 1kHz, so that it can be used
       to drive 1kHz processes without drift
      */
-    uint64_t next_run_usec = sched->micros64() + 1000;
+    uint64_t next_run_usec = AP_HAL::micros64() + 1000;
     while (true) {
-        uint64_t dt = next_run_usec - sched->micros64();
+        uint64_t dt = next_run_usec - AP_HAL::micros64();
         if (dt > 2000) {
             // we've lost sync - restart
-            next_run_usec = sched->micros64();
+            next_run_usec = AP_HAL::micros64();
         } else {
             sched->_microsleep(dt);
         }
@@ -439,7 +422,7 @@ bool Scheduler::system_initializing() {
 void Scheduler::system_initialized()
 {
     if (_initialized) {
-        panic("PANIC: scheduler::system_initialized called more than once");
+        AP_HAL::panic("PANIC: scheduler::system_initialized called more than once");
     }
     _initialized = true;
 }
@@ -451,8 +434,8 @@ void Scheduler::reboot(bool hold_in_bootloader)
 
 void Scheduler::stop_clock(uint64_t time_usec)
 {
-    if (time_usec >= stopped_clock_usec) {
-        stopped_clock_usec = time_usec;
+    if (time_usec >= _stopped_clock_usec) {
+        _stopped_clock_usec = time_usec;
         _run_io();
     }
 }

libraries/AP_HAL_Linux/Scheduler.h

@@ -18,6 +18,11 @@ typedef void *(*pthread_startroutine_t)(void *);
 
 public:
     Scheduler();
+
+    static Scheduler *from(AP_HAL::Scheduler *scheduler) {
+        return static_cast<Scheduler*>(scheduler);
+    }
+
     void     init(void* machtnichts);
     void     delay(uint16_t ms);
     uint32_t millis();
@@ -48,8 +53,9 @@ public:
 
     void     stop_clock(uint64_t time_usec);
 
+    uint64_t stopped_clock_usec() const { return _stopped_clock_usec; }
+
 private:
-    struct timespec _sketch_start_time;    
     void _timer_handler(int signum);
     void _microsleep(uint32_t usec);
 
@@ -88,7 +94,7 @@ private:
     void _create_realtime_thread(pthread_t *ctx, int rtprio, const char *name,
                                  pthread_startroutine_t start_routine);
 
-    uint64_t stopped_clock_usec;
+    uint64_t _stopped_clock_usec;
 
     Semaphore _timer_semaphore;
     Semaphore _io_semaphore;

libraries/AP_HAL_Linux/system.cpp

@@ -1,9 +1,77 @@
+#include <stdarg.h>
+#include <stdio.h>
+#include <sys/time.h>
+#include <unistd.h>
+
+#include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/system.h>
+#include <AP_HAL_Linux/Scheduler.h>
+
+extern const AP_HAL::HAL& hal;
 
 namespace AP_HAL {
 
+static struct {
+    struct timespec start_time;
+} state;
+
 void init()
 {
+    clock_gettime(CLOCK_MONOTONIC, &state.start_time);
+}
+
+void panic(const char *errormsg, ...)
+{
+    va_list ap;
+
+    va_start(ap, errormsg);
+    vdprintf(1, errormsg, ap);
+    va_end(ap);
+    write(1, "\n", 1);
+
+    hal.rcin->deinit();
+    hal.scheduler->delay_microseconds(10000);
+    exit(1);
+}
+
+uint32_t micros()
+{
+    return micros64() & 0xFFFFFFFF;
+}
+
+uint32_t millis()
+{
+    return millis64() & 0xFFFFFFFF;
+}
+
+uint64_t micros64()
+{
+    const Linux::Scheduler* scheduler = Linux::Scheduler::from(hal.scheduler);
+    uint64_t stopped_usec = scheduler->stopped_clock_usec();
+    if (stopped_usec) {
+        return stopped_usec;
+    }
+
+    struct timespec ts;
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) -
+                  (state.start_time.tv_sec +
+                   (state.start_time.tv_nsec*1.0e-9)));
+}
+
+uint64_t millis64()
+{
+    const Linux::Scheduler* scheduler = Linux::Scheduler::from(hal.scheduler);
+    uint64_t stopped_usec = scheduler->stopped_clock_usec();
+    if (stopped_usec) {
+        return stopped_usec / 1000;
+    }
+
+    struct timespec ts;
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) -
+                  (state.start_time.tv_sec +
+                   (state.start_time.tv_nsec*1.0e-9)));
 }
 
 } // namespace AP_HAL