AP_HAL_Linux: Thread: add PeriodicThread helper

This is a helper class to run a single periodic function like the ones
used in the scheduler.

libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h

@@ -19,6 +19,7 @@ namespace Linux {
     class Storage_FRAM;
     class DigitalSource;
     class DigitalSource_Sysfs;
+    class PeriodicThread;
     class PWM_Sysfs;
     class PWM_Sysfs_Bebop;
     class PWM_Sysfs_Base;

libraries/AP_HAL_Linux/Thread.cpp

@@ -96,4 +96,35 @@ bool Thread::is_current_thread()
     return pthread_equal(pthread_self(), _ctx);
 }
 
+bool PeriodicThread::set_rate(uint32_t rate_hz)
+{
+    if (_started || rate_hz == 0) {
+        return false;
+    }
+
+    _period_usec = hz_to_usec(rate_hz);
+
+    return true;
+}
+
+bool PeriodicThread::_run()
+{
+    uint64_t next_run_usec = AP_HAL::micros64() + _period_usec;
+
+    while (true) {
+        uint64_t dt = next_run_usec - AP_HAL::micros64();
+        if (dt > _period_usec) {
+            // we've lost sync - restart
+            next_run_usec = AP_HAL::micros64();
+        } else {
+            Scheduler::from(hal.scheduler)->microsleep(dt);
+        }
+        next_run_usec += _period_usec;
+
+        _task();
+    }
+
+    return true;
+}
+
 }

libraries/AP_HAL_Linux/Thread.h

@@ -57,4 +57,18 @@ protected:
     pthread_t _ctx;
 };
 
+class PeriodicThread : public Thread {
+public:
+    PeriodicThread(Thread::task_t t)
+        : Thread(t)
+    { }
+
+    bool set_rate(uint32_t rate_hz);
+
+protected:
+    bool _run() override;
+
+    uint64_t _period_usec;
+};
+
 }