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,38 @@ bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_
 
     return true;
 }
+
+/*
+  create a new task
+*/
+bool Scheduler::task_create(
+    AP_HAL::MemberProc proc_init,
+    AP_HAL::Scheduler::TaskBodyMemberProc proc_body,
+    const char *name,
+    uint32_t stack_size,
+    priority_base base,
+    int8_t priority)
+{
+    Task *task = new Task{(Thread::task_t)proc_init, (Task::task_body_t)proc_body};
+    if (!task) {
+        return false;
+    }
+
+    const uint8_t thread_priority = calculate_thread_priority(base, priority);
+
+    // Add 256k to HAL-independent requested stack size
+    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.
+     */
+    task->set_auto_free(true);
+
+    if (!task->start(name, SCHED_FIFO, thread_priority)) {
+        delete task;
+        return false;
+    }
+
+    return true;
+}

libraries/AP_HAL_Linux/Scheduler.h

@@ -60,6 +60,17 @@ 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::MemberProc proc_init,
+        AP_HAL::Scheduler::TaskBodyMemberProc proc_body,
+        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,26 @@
+#include "Task.h"
+#include <AP_HAL/AP_HAL.h>
+
+extern const AP_HAL::HAL &hal;
+
+namespace Linux {
+
+bool Task::_run()
+{
+    if (!_task_init) {
+        return false;
+    }
+    if (!_task_body) {
+        return false;
+    }
+
+    _task_init();
+    while (true) {
+        const uint32_t delay_us = _task_body();
+        hal.scheduler->delay_microseconds(delay_us);
+    }
+
+    return true;
+}
+
+}

libraries/AP_HAL_Linux/Task.h

@@ -0,0 +1,23 @@
+#include "Thread.h"
+
+namespace Linux {
+class Task : public Thread {
+public:
+    FUNCTOR_TYPEDEF(task_body_t, uint32_t);
+
+    Task(task_t t_init, task_body_t t_body) :
+        Thread(t_init),  // t_init not used from here
+        _task_init{t_init},
+        _task_body{t_body}
+         { }
+
+protected:
+
+    bool _run() override;
+
+private:
+    task_t _task_init;
+    task_body_t _task_body;
+
+};
+}