AP_HAL_Linux: implement task_create

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -19,6 +19,8 @@
 #include "UARTDriver.h"
 #include "Util.h"
 
+#include "Task.h"
+
 using namespace Linux;
 
 extern const AP_HAL::HAL& hal;
@@ -423,3 +425,37 @@ bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_
 
     return true;
 }
+
+/*
+  create a new task
+*/
+bool Scheduler::task_create(
+    AP_HAL::Scheduler::Task &task,
+    const char *name,
+    uint32_t stack_size,
+    priority_base base,
+    int8_t priority)
+{
+    Linux::Task *new_task = new Linux::Task{task};
+    if (!new_task) {
+        return false;
+    }
+
+    const uint8_t thread_priority = calculate_thread_priority(base, priority);
+
+    // Add 256k to HAL-independent requested stack size
+    new_task->set_stack_size(256 * 1024 + stack_size);
+
+    /*
+     * We should probably store the thread handlers and join() when exiting,
+     * but let's the thread manage itself for now.
+     */
+    new_task->set_auto_free(true);
+
+    if (!new_task->start(name, SCHED_FIFO, thread_priority)) {
+        delete new_task;
+        return false;
+    }
+
+    return true;
+}

libraries/AP_HAL_Linux/Scheduler.h

@@ -60,6 +60,16 @@ class Scheduler : public AP_HAL::Scheduler {
      */
     void set_cpu_affinity(const cpu_set_t &cpu_affinity) { _cpu_affinity = cpu_affinity; }
 
+    /*
+      create a new task (may create a thread, may run on main loop)
+     */
+    bool task_create(
+        AP_HAL::Scheduler::Task &task,
+        const char *name,
+        uint32_t stack_size,
+        priority_base base,
+        int8_t priority) override;
+
 private:
     class SchedulerThread : public PeriodicThread {
     public:

libraries/AP_HAL_Linux/Task.cpp

@@ -0,0 +1,18 @@
+#include "Task.h"
+#include <AP_HAL/AP_HAL.h>
+
+extern const AP_HAL::HAL &hal;
+
+namespace Linux {
+
+bool Task::_run()
+{
+    while (true) {
+        const uint32_t delay_us = task.update();
+        hal.scheduler->delay_microseconds(delay_us);
+    }
+
+    return true;
+}
+
+}

libraries/AP_HAL_Linux/Task.h

@@ -0,0 +1,22 @@
+#include "Thread.h"
+
+#include <AP_HAL/Scheduler.h>
+
+namespace Linux {
+class Task : public Thread {
+public:
+
+    Task(AP_HAL::Scheduler::Task &new_task) :
+        Thread(nullptr),  // this passed-in task_t not called
+        task{new_task}
+         { }
+
+protected:
+
+    bool _run() override;
+
+private:
+    AP_HAL::Scheduler::Task &task;
+
+};
+}