HAL_QURT: got to link stage

libraries/AP_HAL_QURT/HAL_QURT_Class.cpp

@@ -30,18 +30,16 @@
 
 using namespace QURT;
 
-static UDPDriver uartADriver;
-static UARTDriver uartBDriver("/dev/tty-4");
-static UARTDriver uartCDriver("/dev/tty-2");
-static UARTDriver uartDDriver(nullptr);
-static UARTDriver uartEDriver(nullptr);
+static Empty::UARTDriver serial0Driver;
+static UARTDriver serial1Driver("/dev/tty-4");
+static UARTDriver serial2Driver("/dev/tty-2");
 
 static Empty::SPIDeviceManager spiDeviceManager;
 static Empty::AnalogIn analogIn;
 static Storage storageDriver;
 static Empty::GPIO gpioDriver;
-static RCInput rcinDriver("/dev/tty-1");
-static RCOutput rcoutDriver("/dev/tty-3");
+static Empty::RCInput rcinDriver;
+static RCOutput rcoutDriver;
 static Util utilInstance;
 static Scheduler schedulerInstance;
 static Empty::I2CDeviceManager i2c_mgr_instance;
@@ -50,22 +48,29 @@ bool qurt_ran_overtime;
 
 HAL_QURT::HAL_QURT() :
     AP_HAL::HAL(
-        &uartADriver,
-        &uartBDriver,
-        &uartCDriver,
-        &uartDDriver,
-        &uartEDriver,
-        nullptr, // uartF
+        &serial0Driver,
+        &serial1Driver,
+        &serial2Driver,
+        nullptr,
+        nullptr,
+        nullptr,
+        nullptr,
+        nullptr,
+        nullptr,
+        nullptr,
         &i2c_mgr_instance,
         &spiDeviceManager,
+        nullptr,
         &analogIn,
         &storageDriver,
-        &uartADriver,
+        &serial0Driver,
         &gpioDriver,
         &rcinDriver,
         &rcoutDriver,
         &schedulerInstance,
         &utilInstance,
+        nullptr,
+        nullptr,
         nullptr)
 {
 }
@@ -74,58 +79,15 @@ void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const
 {
     assert(callbacks);
 
-    int opt;
-    const struct GetOptLong::option options[] = {
-        {"uartB",         true,  0, 'B'},
-        {"uartC",         true,  0, 'C'},
-        {"uartD",         true,  0, 'D'},
-        {"uartE",         true,  0, 'E'},
-        {"dsm",           true,  0, 'S'},
-        {"ESC",           true,  0, 'e'},
-        {0, false, 0, 0}
-    };
-
-    GetOptLong gopt(argc, argv, "B:C:D:E:e:S",
-                    options);
-
-    /*
-      parse command line options
-     */
-    while ((opt = gopt.getoption()) != -1) {
-        switch (opt) {
-        case 'B':
-            uartBDriver.set_device_path(gopt.optarg);
-            break;
-        case 'C':
-            uartCDriver.set_device_path(gopt.optarg);
-            break;
-        case 'D':
-            uartDDriver.set_device_path(gopt.optarg);
-            break;
-        case 'E':
-            uartEDriver.set_device_path(gopt.optarg);
-            break;
-        case 'e':
-            rcoutDriver.set_device_path(gopt.optarg);
-            break;
-        case 'S':
-            rcinDriver.set_device_path(gopt.optarg);
-            break;
-        default:
-            printf("Unknown option '%c'\n", (char)opt);
-            exit(1);
-        }
-    }
-
     /* initialize all drivers and private members here.
      * up to the programmer to do this in the correct order.
      * Scheduler should likely come first. */
     scheduler->init();
     schedulerInstance.hal_initialized();
-    uartA->begin(115200);
+    serial0Driver.begin(115200);
     rcinDriver.init();
     callbacks->setup();
-    scheduler->system_initialized();
+    scheduler->set_system_initialized();
 
     for (;;) {
         callbacks->loop();

libraries/AP_HAL_QURT/Scheduler.cpp

@@ -11,8 +11,7 @@
 #include <sched.h>
 #include <errno.h>
 #include <stdio.h>
-#include <dspal/include/pthread.h>
-#include <dspal_types.h>
+#include <pthread.h>
 
 #include "UARTDriver.h"
 #include "Storage.h"
@@ -64,8 +63,7 @@ void Scheduler::init()
 
 void Scheduler::delay_microseconds(uint16_t usec) 
 {
-    //pthread_yield();
-    usleep(usec);
+    qurt_sleep(usec);
 }
 
 void Scheduler::delay(uint16_t ms)
@@ -139,7 +137,7 @@ void Scheduler::resume_timer_procs()
 void Scheduler::reboot(bool hold_in_bootloader) 
 {
     HAP_PRINTF("**** REBOOT REQUESTED ****");
-    usleep(2000000);
+    qurt_sleep(2000000);
     exit(1);
 }
 
@@ -174,7 +172,6 @@ extern bool qurt_ran_overtime;
 void *Scheduler::_timer_thread(void *arg)
 {
     Scheduler *sched = (Scheduler *)arg;
-    uint32_t last_ran_overtime = 0;
 
     while (!sched->_hal_initialized) {
         sched->delay_microseconds(1000);
@@ -184,15 +181,6 @@ void *Scheduler::_timer_thread(void *arg)
 
         // run registered timers
         sched->_run_timers(true);
-
-        // process any pending RC output requests
-        ((RCOutput *)hal.rcout)->timer_update();
-
-        if (qurt_ran_overtime && AP_HAL::millis() - last_ran_overtime > 2000) {
-            last_ran_overtime = AP_HAL::millis();
-            printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
-            hal.console->printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
-        }
     }
     return nullptr;
 }
@@ -228,11 +216,12 @@ void *Scheduler::_uart_thread(void *arg)
 
         // process any pending serial bytes
         //((UARTDriver *)hal.uartA)->timer_tick();
-        hal.uartB->timer_tick();
-        hal.uartC->timer_tick();
-        hal.uartD->timer_tick();
-        hal.uartE->timer_tick();
-        hal.uartF->timer_tick();
+        for (uint8_t i = 0; i < hal.num_serial; i++) {
+            auto *p = hal.serial(i);
+            if (p != nullptr) {
+                p->_timer_tick();
+            }
+        }
     }
     return nullptr;
 }

libraries/AP_HAL_QURT/Scheduler.h

@@ -20,20 +20,23 @@ class QURT::Scheduler : public AP_HAL::Scheduler {
     Scheduler();
     /* AP_HAL::Scheduler methods */
 
-    void     init();
-    void     delay(uint16_t ms);
-    void     delay_microseconds(uint16_t us);
-    void     register_timer_process(AP_HAL::MemberProc);
-    void     register_io_process(AP_HAL::MemberProc);
-    void     register_timer_failsafe(AP_HAL::Proc, uint32_t period_us);
+    void     init() override;
+    void     delay(uint16_t ms) override;
+    void     delay_microseconds(uint16_t us) override;
+    void     register_timer_process(AP_HAL::MemberProc) override;
+    void     register_io_process(AP_HAL::MemberProc) override;
+    void     register_timer_failsafe(AP_HAL::Proc, uint32_t period_us) override;
     void     suspend_timer_procs();
     void     resume_timer_procs();
-    void     reboot(bool hold_in_bootloader);
+    void     reboot(bool hold_in_bootloader) override;
 
     bool     in_main_thread() const override;
     void     system_initialized();
     void     hal_initialized();
-
+
+    void     set_system_initialized() override;
+    bool     is_system_initialized() override;
+
 private:
     bool _initialized;
     volatile bool _hal_initialized;

libraries/AP_HAL_QURT/Semaphores.cpp

@@ -1,19 +1,27 @@
 #include <AP_HAL/AP_HAL.h>
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
-
 #include "Semaphores.h"
 
 extern const AP_HAL::HAL& hal;
 
 using namespace QURT;
 
-bool Semaphore::give() 
+// construct a semaphore
+Semaphore::Semaphore()
+{
+    pthread_mutexattr_t attr;
+    pthread_mutexattr_init(&attr);
+    pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT);
+    pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
+    pthread_mutex_init(&_lock, &attr);
+}
+
+bool Semaphore::give()
 {
     return pthread_mutex_unlock(&_lock) == 0;
 }
 
-bool Semaphore::take(uint32_t timeout_ms) 
+bool Semaphore::take(uint32_t timeout_ms)
 {
     if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) {
         return pthread_mutex_lock(&_lock) == 0;
@@ -31,9 +39,61 @@ bool Semaphore::take(uint32_t timeout_ms)
     return false;
 }
 
-bool Semaphore::take_nonblocking() 
+bool Semaphore::take_nonblocking()
 {
     return pthread_mutex_trylock(&_lock) == 0;
 }
 
-#endif // CONFIG_HAL_BOARD
+/*
+  binary semaphore using pthread condition variables
+ */
+
+BinarySemaphore::BinarySemaphore(bool initial_state) :
+    AP_HAL::BinarySemaphore(initial_state)
+{
+    pthread_cond_init(&cond, NULL);
+    pending = initial_state;
+}
+
+bool BinarySemaphore::wait(uint32_t timeout_us)
+{
+    WITH_SEMAPHORE(mtx);
+    if (!pending) {
+        struct timespec ts;
+        if (clock_gettime(CLOCK_REALTIME, &ts) != 0) {
+            return false;
+        }
+        ts.tv_sec += timeout_us/1000000UL;
+        ts.tv_nsec += (timeout_us % 1000000U) * 1000UL;
+        if (ts.tv_nsec >= 1000000000L) {
+            ts.tv_sec++;
+            ts.tv_nsec -= 1000000000L;
+        }
+        if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) != 0) {
+            return false;
+        }
+    }
+    pending = false;
+    return true;
+}
+
+bool BinarySemaphore::wait_blocking(void)
+{
+    WITH_SEMAPHORE(mtx);
+    if (!pending) {
+        if (pthread_cond_wait(&cond, &mtx._lock) != 0) {
+            return false;
+        }
+    }
+    pending = false;
+    return true;
+}
+
+void BinarySemaphore::signal(void)
+{
+    WITH_SEMAPHORE(mtx);
+    if (!pending) {
+        pending = true;
+        pthread_cond_signal(&cond);
+    }
+}

libraries/AP_HAL_QURT/Semaphores.h

@@ -1,7 +1,9 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL_Boards.h>
+#include <stdint.h>
 #include <AP_HAL/Semaphores.h>
+#include <pthread.h>
 
 namespace QURT {
 
@@ -12,9 +14,8 @@ class Semaphore : public AP_HAL::Semaphore {
     bool give() override;
     bool take(uint32_t timeout_ms) override;
     bool take_nonblocking() override;
-
 protected:
-    int dummy_lock;
+    pthread_mutex_t _lock;
 };
 
 
@@ -28,6 +29,7 @@ class BinarySemaphore : public AP_HAL::BinarySemaphore {
 
 protected:
     Semaphore mtx;
+    pthread_cond_t cond;
     bool pending;
 };