HAL_QURT: initial implementation

this provides RC onput, RC output, scheduling, storage, UARTs and all
necessary support routines to fly ArduPilot on QURT

libraries/AP_HAL_QURT/AP_HAL_QURT.h

@@ -0,0 +1,30 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __AP_HAL_QURT_H__
+#define __AP_HAL_QURT_H__
+
+/* Your layer exports should depend on AP_HAL.h ONLY. */
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+
+#include "HAL_QURT_Class.h"
+#include "AP_HAL_QURT_Main.h"
+
+#endif // CONFIG_HAL_BOARD
+
+#endif //__AP_HAL_QURT_H__
+

libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h

@@ -0,0 +1,19 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __AP_HAL_QURT_MAIN_H__
+#define __AP_HAL_QURT_MAIN_H__
+
+#endif // __AP_HAL_QURT_MAIN_H__

libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h

@@ -0,0 +1,29 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+namespace QURT {
+    class UARTDriver;
+    class UDPDriver;
+    class Util;
+    class Scheduler;
+    class Storage;
+    class Util;
+    class Semaphore;
+    class RCInput;
+    class RCOutput;
+}
+

libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h

@@ -0,0 +1,28 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __AP_HAL_QURT_PRIVATE_H__
+#define __AP_HAL_QURT_PRIVATE_H__
+
+/* Umbrella header for all private headers of the AP_HAL_QURT module.
+ * Only import this header from inside AP_HAL_QURT
+ */
+
+#include "UARTDriver.h"
+#include "UDPDriver.h"
+#include "Util.h"
+
+#endif // __AP_HAL_QURT_PRIVATE_H__
+

libraries/AP_HAL_QURT/HAL_QURT_Class.cpp

@@ -0,0 +1,102 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+
+#include "HAL_QURT_Class.h"
+#include "AP_HAL_QURT_Private.h"
+#include "Scheduler.h"
+#include "Storage.h"
+#include "Semaphores.h"
+#include "RCInput.h"
+#include "RCOutput.h"
+#include <AP_HAL_Empty/AP_HAL_Empty.h>
+#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
+#include <assert.h>
+
+using namespace QURT;
+
+static UDPDriver uartADriver;
+static UARTDriver uartBDriver("/dev/tty-4");
+static UARTDriver uartCDriver("/dev/tty-2");
+static Empty::UARTDriver uartDDriver;
+static Semaphore  i2cSemaphore;
+static Empty::I2CDriver  i2cDriver(&i2cSemaphore);
+
+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 Util utilInstance;
+static Scheduler schedulerInstance;
+
+bool qurt_ran_overtime;
+
+HAL_QURT::HAL_QURT() :
+    AP_HAL::HAL(
+        &uartADriver,
+        &uartBDriver,
+        &uartCDriver,
+        &uartDDriver,
+        NULL,            /* no uartE */
+        &i2cDriver,
+        NULL, /* only one i2c */
+        NULL, /* only one i2c */
+        &spiDeviceManager,
+        &analogIn,
+        &storageDriver,
+        &uartADriver,
+        &gpioDriver,
+        &rcinDriver,
+        &rcoutDriver,
+        &schedulerInstance,
+        &utilInstance,
+        NULL)
+{
+}
+
+void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const
+{
+    assert(callbacks);
+
+    /* 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);
+    rcinDriver.init();
+
+    callbacks->setup();
+    scheduler->system_initialized();
+
+    for (;;) {
+        callbacks->loop();
+    }
+}
+
+const AP_HAL::HAL& AP_HAL::get_HAL() {
+    static const HAL_QURT *hal;
+    if (hal == nullptr) {
+        hal = new HAL_QURT;
+        HAP_PRINTF("allocated HAL_QURT of size %u", sizeof(*hal));
+    }
+    return *hal;
+}
+
+#endif

libraries/AP_HAL_QURT/HAL_QURT_Class.h

@@ -0,0 +1,30 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __AP_HAL_QURT_CLASS_H__
+#define __AP_HAL_QURT_CLASS_H__
+
+#include <AP_HAL/AP_HAL.h>
+
+#include "AP_HAL_QURT_Namespace.h"
+
+class HAL_QURT : public AP_HAL::HAL {
+public:
+    HAL_QURT();
+    void run(int argc, char* const* argv, Callbacks* callbacks) const override;
+};
+
+#endif // __AP_HAL_QURT_CLASS_H__
+

libraries/AP_HAL_QURT/RCInput.cpp

@@ -0,0 +1,193 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+#include <stdio.h>
+#include <sys/time.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <stdint.h>
+#include <dev_fs_lib_serial.h>
+
+#include "RCInput.h"
+#include <AP_HAL/utility/dsm.h>
+
+extern const AP_HAL::HAL& hal;
+
+using namespace QURT;
+
+RCInput::RCInput(const char *_device_path) :
+    device_path(_device_path),
+    new_rc_input(false)
+{
+}
+
+extern "C" {
+static void read_callback_trampoline(void *, char *, size_t );
+}
+
+void RCInput::init()
+{
+    if (device_path == nullptr) {
+        return;
+    }
+    fd = open(device_path, O_RDONLY|O_NONBLOCK);
+    if (fd == -1) {
+        AP_HAL::panic("Unable to open RC input %s", device_path);
+    }
+
+    struct dspal_serial_ioctl_data_rate rate;
+    rate.bit_rate = DSPAL_SIO_BITRATE_115200;
+    int ret = ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&rate);
+
+    struct dspal_serial_ioctl_receive_data_callback callback;
+    callback.context = this;
+    callback.rx_data_callback_func_ptr = read_callback_trampoline;
+    ret = ioctl(fd, SERIAL_IOCTL_SET_RECEIVE_DATA_CALLBACK, (void *)&callback);
+}
+
+static void read_callback_trampoline(void *ctx, char *buf, size_t size)
+{
+    ((RCInput *)ctx)->read_callback(buf, size);
+}
+
+/*
+  callback for incoming data
+ */
+void RCInput::read_callback(char *buf, size_t size)
+{
+    add_dsm_input((const uint8_t *)buf, size);
+}
+
+bool RCInput::new_input() 
+{
+    return new_rc_input;
+}
+
+uint8_t RCInput::num_channels() 
+{
+    return _num_channels;
+}
+
+uint16_t RCInput::read(uint8_t ch) 
+{
+    new_rc_input = false;
+    if (_override[ch]) {
+        return _override[ch];
+    }
+    if (ch >= _num_channels) {
+        return 0;
+    }
+    return _pwm_values[ch];
+}
+
+uint8_t RCInput::read(uint16_t* periods, uint8_t len) 
+{
+    uint8_t i;
+    for (i=0; i<len; i++) {
+        if((periods[i] = read(i))){
+            continue;
+        }
+        else{
+            break;
+        }
+    }
+    return (i+1);
+}
+
+bool RCInput::set_overrides(int16_t *overrides, uint8_t len) 
+{
+    bool res = false;
+    if(len > QURT_RC_INPUT_NUM_CHANNELS){
+        len = QURT_RC_INPUT_NUM_CHANNELS;
+    }
+    for (uint8_t i = 0; i < len; i++) {
+        res |= set_override(i, overrides[i]);
+    }
+    return res;
+}
+
+bool RCInput::set_override(uint8_t channel, int16_t override) 
+{
+    if (override < 0) return false; /* -1: no change. */
+    if (channel < QURT_RC_INPUT_NUM_CHANNELS) {
+        _override[channel] = override;
+        if (override != 0) {
+            new_rc_input = true;
+            return true;
+        }
+    }
+    return false;
+}
+
+void RCInput::clear_overrides()
+{
+    for (uint8_t i = 0; i < QURT_RC_INPUT_NUM_CHANNELS; i++) {
+       _override[i] = 0;
+    }
+}
+
+
+/*
+  add some bytes of input in DSM serial stream format, coping with partial packets
+ */
+void RCInput::add_dsm_input(const uint8_t *bytes, size_t nbytes)
+{
+    if (nbytes == 0) {
+        return;
+    }
+    const uint8_t dsm_frame_size = sizeof(dsm.frame);
+
+    uint32_t now = AP_HAL::millis();    
+    if (now - dsm.last_input_ms > 5) {
+        // resync based on time
+        dsm.partial_frame_count = 0;
+    }
+    dsm.last_input_ms = now;
+    
+    while (nbytes > 0) {
+        size_t n = nbytes;
+        if (dsm.partial_frame_count + n > dsm_frame_size) {
+            n = dsm_frame_size - dsm.partial_frame_count;
+        }
+        if (n > 0) {
+            memcpy(&dsm.frame[dsm.partial_frame_count], bytes, n);
+            dsm.partial_frame_count += n;
+            nbytes -= n;
+            bytes += n;
+        }
+
+	if (dsm.partial_frame_count == dsm_frame_size) {
+            dsm.partial_frame_count = 0;
+            uint16_t values[16] {};
+            uint16_t num_values=0;
+            if (dsm_decode(AP_HAL::micros64(), dsm.frame, values, &num_values, 16) &&
+                num_values >= 5) {
+                for (uint8_t i=0; i<num_values; i++) {
+                    if (values[i] != 0) {
+                        _pwm_values[i] = values[i];
+                    }
+                }
+                /*
+                  the apparent number of channels can change on DSM,
+                  as they are spread across multiple frames. We just
+                  use the max num_values we get
+                 */
+                if (num_values > _num_channels) {
+                    _num_channels = num_values;
+                }
+                new_rc_input = true;
+#if 0
+                HAP_PRINTF("Decoded DSM %u channels %u %u %u %u %u %u %u %u\n",
+                           (unsigned)num_values,
+                           values[0], values[1], values[2], values[3], values[4], values[5], values[6], values[7]);
+#endif
+            }
+        }
+    }
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_QURT/RCInput.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#include "AP_HAL_QURT.h"
+
+#define QURT_RC_INPUT_NUM_CHANNELS 16
+
+class QURT::RCInput : public AP_HAL::RCInput {
+public:
+    RCInput(const char *device_path);
+
+    static RCInput *from(AP_HAL::RCInput *rcinput) {
+        return static_cast<RCInput*>(rcinput);
+    }
+
+    void init();
+    bool new_input();
+    uint8_t num_channels();
+    uint16_t read(uint8_t ch);
+    uint8_t read(uint16_t* periods, uint8_t len);
+
+    bool set_overrides(int16_t *overrides, uint8_t len);
+    bool set_override(uint8_t channel, int16_t override);
+    void clear_overrides();
+
+    void read_callback(char *buf, size_t size);
+
+ private:
+    volatile bool new_rc_input;
+
+    uint16_t _pwm_values[QURT_RC_INPUT_NUM_CHANNELS];    
+    uint8_t  _num_channels;
+
+    /* override state */
+    uint16_t _override[QURT_RC_INPUT_NUM_CHANNELS];
+
+    // add some DSM input bytes, for RCInput over a serial port
+    void add_dsm_input(const uint8_t *bytes, size_t nbytes);
+        
+    const char *device_path;
+    int32_t fd = -1;
+    
+    // state of add_dsm_input
+    struct {
+        uint8_t frame[16];
+        uint8_t partial_frame_count;
+        uint32_t last_input_ms;
+    } dsm;
+};
+

libraries/AP_HAL_QURT/RCOutput.cpp

@@ -0,0 +1,113 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+
+#include "RCOutput.h"
+#include <GCS_MAVLink/include/mavlink/v1.0/checksum.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <dev_fs_lib_serial.h>
+
+extern const AP_HAL::HAL& hal;
+
+using namespace QURT;
+
+void RCOutput::init()
+{
+}
+
+void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
+{
+    // no support for changing frequency yet
+}
+
+uint16_t RCOutput::get_freq(uint8_t ch)
+{
+    // return fixed fake value
+    return 490;
+}
+
+void RCOutput::enable_ch(uint8_t ch)
+{
+    if (ch >= channel_count) {
+        return;
+    }
+    enable_mask |= 1U<<ch;
+}
+
+void RCOutput::disable_ch(uint8_t ch)
+{
+    if (ch >= channel_count) {
+        return;
+    }
+    enable_mask &= ~1U<<ch;
+}
+
+void RCOutput::write(uint8_t ch, uint16_t period_us)
+{
+    if (ch >= channel_count) {
+        return;
+    }
+    period[ch] = period_us;
+    need_write = true;
+}
+
+uint16_t RCOutput::read(uint8_t ch)
+{
+    if (ch >= channel_count) {
+        return 0;
+    }
+    return period[ch];
+}
+
+void RCOutput::read(uint16_t *period_us, uint8_t len)
+{
+    for (int i = 0; i < len; i++) {
+        period_us[i] = read(i);
+    }
+}
+
+extern "C" {
+    // discard incoming data
+    static void read_callback_trampoline(void *, char *, size_t ) {}
+}
+
+void RCOutput::timer_update(void)
+{
+    if (fd == -1 && device_path != nullptr) {
+        HAP_PRINTF("Opening RCOutput %s", device_path);
+        fd = open(device_path, O_RDWR|O_NONBLOCK);
+        if (fd == -1) {
+            AP_HAL::panic("Unable to open %s", device_path);
+        }
+        HAP_PRINTF("Opened ESC UART %s fd=%d\n", device_path, fd);
+        if (fd != -1) {
+            struct dspal_serial_ioctl_data_rate rate;
+            rate.bit_rate = DSPAL_SIO_BITRATE_115200;
+            ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&rate);
+
+            struct dspal_serial_ioctl_receive_data_callback callback;
+            callback.context = this;
+            callback.rx_data_callback_func_ptr = read_callback_trampoline;
+            ioctl(fd, SERIAL_IOCTL_SET_RECEIVE_DATA_CALLBACK, (void *)&callback);
+        }
+    }
+    if (!need_write || fd == -1) {
+        return;
+    }
+    struct PACKED {
+        uint8_t magic = 0xF7;
+        uint16_t period[channel_count];
+        uint16_t crc;
+    } frame;
+    memcpy(frame.period, period, sizeof(period));
+    frame.crc = crc_calculate((uint8_t*)frame.period, channel_count*2);
+    need_write = false;
+    ::write(fd, (uint8_t *)&frame, sizeof(frame));
+}
+
+
+#endif // CONFIG_HAL_BOARD_SUBTYPE
+

libraries/AP_HAL_QURT/RCOutput.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_HAL_QURT.h"
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+
+class QURT::RCOutput : public AP_HAL::RCOutput {
+public:
+    
+    RCOutput(const char *_device_path) {
+        device_path = _device_path;
+    }
+    void init();
+    void set_freq(uint32_t chmask, uint16_t freq_hz);
+    uint16_t get_freq(uint8_t ch);
+    void enable_ch(uint8_t ch);
+    void disable_ch(uint8_t ch);
+    void write(uint8_t ch, uint16_t period_us);
+    uint16_t read(uint8_t ch);
+    void read(uint16_t *period_us, uint8_t len);
+
+    void timer_update(void);
+    
+private:
+    const char *device_path;
+    const uint32_t baudrate = 115200;    
+    static const uint8_t channel_count = 4;
+    
+    int fd = -1;
+    uint16_t enable_mask;
+    uint16_t period[channel_count];
+    volatile bool need_write;
+};
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_QURT/Scheduler.cpp

@@ -0,0 +1,291 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL/AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+
+#include "AP_HAL_QURT.h"
+#include "Scheduler.h"
+
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <sched.h>
+#include <errno.h>
+#include <stdio.h>
+#include <dspal/include/pthread.h>
+#include <dspal_types.h>
+
+#include "UARTDriver.h"
+//#include "AnalogIn.h"
+#include "Storage.h"
+#include "RCOutput.h"
+#include <AP_Scheduler/AP_Scheduler.h>
+
+using namespace QURT;
+
+extern const AP_HAL::HAL& hal;
+
+Scheduler::Scheduler()
+{
+}
+
+void Scheduler::init()
+{
+    _main_task_pid = getpid();
+
+    // setup the timer thread - this will call tasks at 1kHz
+	pthread_attr_t thread_attr;
+	struct sched_param param;
+
+	pthread_attr_init(&thread_attr);
+	pthread_attr_setstacksize(&thread_attr, 40960);
+
+	param.sched_priority = APM_TIMER_PRIORITY;
+	(void)pthread_attr_setschedparam(&thread_attr, &param);
+
+	pthread_create(&_timer_thread_ctx, &thread_attr, &Scheduler::_timer_thread, this);
+
+    // the UART thread runs at a medium priority
+	pthread_attr_init(&thread_attr);
+	pthread_attr_setstacksize(&thread_attr, 40960);
+
+	param.sched_priority = APM_UART_PRIORITY;
+	(void)pthread_attr_setschedparam(&thread_attr, &param);
+
+	pthread_create(&_uart_thread_ctx, &thread_attr, &Scheduler::_uart_thread, this);
+
+    // the IO thread runs at lower priority
+	pthread_attr_init(&thread_attr);
+	pthread_attr_setstacksize(&thread_attr, 40960);
+
+	param.sched_priority = APM_IO_PRIORITY;
+	(void)pthread_attr_setschedparam(&thread_attr, &param);
+
+	pthread_create(&_io_thread_ctx, &thread_attr, &Scheduler::_io_thread, this);
+}
+
+void Scheduler::delay_microseconds(uint16_t usec) 
+{
+    //pthread_yield();
+    usleep(usec);
+}
+
+void Scheduler::delay(uint16_t ms)
+{
+    if (in_timerprocess()) {
+        ::printf("ERROR: delay() from timer process\n");
+        return;
+    }
+	uint64_t start = AP_HAL::micros64();
+    uint64_t now;
+    
+    while (((now=AP_HAL::micros64()) - start)/1000 < ms) {
+        delay_microseconds(1000);
+        if (_min_delay_cb_ms <= ms) {
+            if (_delay_cb) {
+                _delay_cb();
+            }
+        }
+    }
+}
+
+void Scheduler::register_delay_callback(AP_HAL::Proc proc,
+                                            uint16_t min_time_ms) 
+{
+    _delay_cb = proc;
+    _min_delay_cb_ms = min_time_ms;
+}
+
+void Scheduler::register_timer_process(AP_HAL::MemberProc proc) 
+{
+    for (uint8_t i = 0; i < _num_timer_procs; i++) {
+        if (_timer_proc[i] == proc) {
+            return;
+        }
+    }
+
+    if (_num_timer_procs < QURT_SCHEDULER_MAX_TIMER_PROCS) {
+        _timer_proc[_num_timer_procs] = proc;
+        _num_timer_procs++;
+    } else {
+        hal.console->printf("Out of timer processes\n");
+    }
+}
+
+void Scheduler::register_io_process(AP_HAL::MemberProc proc) 
+{
+    for (uint8_t i = 0; i < _num_io_procs; i++) {
+        if (_io_proc[i] == proc) {
+            return;
+        }
+    }
+
+    if (_num_io_procs < QURT_SCHEDULER_MAX_TIMER_PROCS) {
+        _io_proc[_num_io_procs] = proc;
+        _num_io_procs++;
+    } else {
+        hal.console->printf("Out of IO processes\n");
+    }
+}
+
+void Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us) 
+{
+    _failsafe = failsafe;
+}
+
+void Scheduler::suspend_timer_procs() 
+{
+    _timer_suspended = true;
+}
+
+void Scheduler::resume_timer_procs() 
+{
+    _timer_suspended = false;
+    if (_timer_event_missed == true) {
+        _run_timers(false);
+        _timer_event_missed = false;
+    }
+}
+
+void Scheduler::reboot(bool hold_in_bootloader) 
+{
+    HAP_PRINTF("**** REBOOT REQUESTED ****");
+    usleep(2000000);
+    exit(1);
+}
+
+void Scheduler::_run_timers(bool called_from_timer_thread)
+{
+    if (_in_timer_proc) {
+        return;
+    }
+    _in_timer_proc = true;
+
+    if (!_timer_suspended) {
+        // now call the timer based drivers
+        for (int i = 0; i < _num_timer_procs; i++) {
+            if (_timer_proc[i]) {
+                _timer_proc[i]();
+            }
+        }
+    } else if (called_from_timer_thread) {
+        _timer_event_missed = true;
+    }
+
+    // and the failsafe, if one is setup
+    if (_failsafe != NULL) {
+        _failsafe();
+    }
+
+    // process analog input
+    // ((QURTAnalogIn *)hal.analogin)->_timer_tick();
+
+    _in_timer_proc = false;
+}
+
+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);
+    }
+    while (true) {
+        sched->delay_microseconds(1000);
+
+        // 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 NULL;
+}
+
+void Scheduler::_run_io(void)
+{
+    if (_in_io_proc) {
+        return;
+    }
+    _in_io_proc = true;
+
+    if (!_timer_suspended) {
+        // now call the IO based drivers
+        for (int i = 0; i < _num_io_procs; i++) {
+            if (_io_proc[i]) {
+                _io_proc[i]();
+            }
+        }
+    }
+
+    _in_io_proc = false;
+}
+
+void *Scheduler::_uart_thread(void *arg)
+{
+    Scheduler *sched = (Scheduler *)arg;
+
+    while (!sched->_hal_initialized) {
+        sched->delay_microseconds(1000);
+    }
+    while (true) {
+        sched->delay_microseconds(1000);
+
+        // process any pending serial bytes
+        //((UARTDriver *)hal.uartA)->timer_tick();
+        ((UARTDriver *)hal.uartB)->timer_tick();
+        ((UARTDriver *)hal.uartC)->timer_tick();
+        //((UARTDriver *)hal.uartD)->timer_tick();
+    }
+    return NULL;
+}
+
+void *Scheduler::_io_thread(void *arg)
+{
+    Scheduler *sched = (Scheduler *)arg;
+
+    while (!sched->_hal_initialized) {
+        sched->delay_microseconds(1000);
+    }
+    while (true) {
+        sched->delay_microseconds(1000);
+
+        // run registered IO processes
+        sched->_run_io();
+    }
+    return NULL;
+}
+
+bool Scheduler::in_timerprocess() 
+{
+    return getpid() != _main_task_pid;
+}
+
+bool Scheduler::system_initializing() {
+    return !_initialized;
+}
+
+void Scheduler::system_initialized() {
+    if (_initialized) {
+        AP_HAL::panic("PANIC: scheduler::system_initialized called"
+                   "more than once");
+    }
+    _initialized = true;
+}
+
+void Scheduler::hal_initialized(void)
+{
+    HAP_PRINTF("HAL is initialised");
+    _hal_initialized = true;
+}
+#endif

libraries/AP_HAL_QURT/Scheduler.h

@@ -0,0 +1,81 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+#include "AP_HAL_QURT_Namespace.h"
+#include <sys/time.h>
+#include <signal.h>
+#include <pthread.h>
+
+#define QURT_SCHEDULER_MAX_TIMER_PROCS 8
+
+#define APM_MAIN_PRIORITY       180
+#define APM_TIMER_PRIORITY      181
+#define APM_UART_PRIORITY        60
+#define APM_IO_PRIORITY          58
+#define APM_SHELL_PRIORITY       57
+#define APM_OVERTIME_PRIORITY    10
+#define APM_STARTUP_PRIORITY     10
+
+#define APM_MAIN_THREAD_STACK_SIZE 16384
+
+/* Scheduler implementation: */
+class QURT::Scheduler : public AP_HAL::Scheduler {
+public:
+    Scheduler();
+    /* AP_HAL::Scheduler methods */
+
+    void     init();
+    void     delay(uint16_t ms);
+    void     delay_microseconds(uint16_t us);
+    void     register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
+    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     suspend_timer_procs();
+    void     resume_timer_procs();
+    void     reboot(bool hold_in_bootloader);
+
+    bool     in_timerprocess();
+    bool     system_initializing();
+    void     system_initialized();
+    void     hal_initialized();
+    
+private:
+    bool _initialized;
+    volatile bool _hal_initialized;
+    AP_HAL::Proc _delay_cb;
+    uint16_t _min_delay_cb_ms;
+    AP_HAL::Proc _failsafe;
+    volatile bool _timer_pending;
+
+    volatile bool _timer_suspended;
+
+    AP_HAL::MemberProc _timer_proc[QURT_SCHEDULER_MAX_TIMER_PROCS];
+    uint8_t _num_timer_procs;
+    volatile bool _in_timer_proc;
+
+    AP_HAL::MemberProc _io_proc[QURT_SCHEDULER_MAX_TIMER_PROCS];
+    uint8_t _num_io_procs;
+    volatile bool _in_io_proc;
+
+    volatile bool _timer_event_missed;
+
+    pid_t _main_task_pid;
+    pthread_t _timer_thread_ctx;
+    pthread_t _io_thread_ctx;
+    pthread_t _storage_thread_ctx;
+    pthread_t _uart_thread_ctx;
+
+    static void *_timer_thread(void *arg);
+    static void *_io_thread(void *arg);
+    static void *_storage_thread(void *arg);
+    static void *_uart_thread(void *arg);
+
+    void _run_timers(bool called_from_timer_thread);
+    void _run_io(void);
+};
+#endif
+
+
+

libraries/AP_HAL_QURT/Semaphores.cpp

@@ -0,0 +1,39 @@
+#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() 
+{
+    return pthread_mutex_unlock(&_lock) == 0;
+}
+
+bool Semaphore::take(uint32_t timeout_ms) 
+{
+    if (timeout_ms == 0) {
+        return pthread_mutex_lock(&_lock) == 0;
+    }
+    if (take_nonblocking()) {
+        return true;
+    }
+    uint64_t start = AP_HAL::micros64();
+    do {
+        hal.scheduler->delay_microseconds(200);
+        if (take_nonblocking()) {
+            return true;
+        }
+    } while ((AP_HAL::micros64() - start) < timeout_ms*1000);
+    return false;
+}
+
+bool Semaphore::take_nonblocking() 
+{
+    return pthread_mutex_trylock(&_lock) == 0;
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_QURT/Semaphores.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+#include "AP_HAL_QURT.h"
+#include <pthread.h>
+
+class QURT::Semaphore : public AP_HAL::Semaphore {
+public:
+    Semaphore() {
+        HAP_PRINTF("%s constructor", __FUNCTION__);
+        pthread_mutex_init(&_lock, NULL);
+        HAP_PRINTF("%s constructor2", __FUNCTION__);
+    }
+    bool give();
+    bool take(uint32_t timeout_ms);
+    bool take_nonblocking();
+private:
+    pthread_mutex_t _lock;
+};
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_QURT/Storage.cpp

@@ -0,0 +1,49 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+
+#include <assert.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <stdio.h>
+#include "Storage.h"
+
+using namespace QURT;
+
+/*
+  This stores 'eeprom' data on the filesystem, with a 16k size
+  
+  Data is written on the ARM frontend via a RPC call
+ */
+
+extern const AP_HAL::HAL& hal;
+
+volatile bool Storage::dirty;
+uint8_t Storage::buffer[QURT_STORAGE_SIZE];
+Semaphore Storage::lock;
+
+void Storage::read_block(void *dst, uint16_t loc, size_t n) 
+{
+    if (loc >= sizeof(buffer)-(n-1)) {
+        return;
+    }
+    memcpy(dst, &buffer[loc], n);
+}
+
+void Storage::write_block(uint16_t loc, const void *src, size_t n) 
+{
+    if (loc >= sizeof(buffer)-(n-1)) {
+        return;
+    }
+    if (memcmp(src, &buffer[loc], n) != 0) {
+        lock.take(0);
+        memcpy(&buffer[loc], src, n);
+        dirty = true;
+        lock.give();
+    }
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_QURT/Storage.h

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_HAL_QURT_Namespace.h"
+#include "Semaphores.h"
+#include <stdio.h>
+
+#define QURT_STORAGE_SIZE HAL_STORAGE_SIZE
+
+class QURT::Storage : public AP_HAL::Storage
+{
+public:
+    Storage() {}
+
+    static Storage *from(AP_HAL::Storage *storage) {
+        return static_cast<Storage*>(storage);
+    }
+
+    void init() {}
+    void read_block(void *dst, uint16_t src, size_t n);
+    void write_block(uint16_t dst, const void* src, size_t n);
+
+    static volatile bool dirty;
+    static uint8_t buffer[QURT_STORAGE_SIZE];
+    static Semaphore lock;
+};
+

libraries/AP_HAL_QURT/UARTDriver.cpp

@@ -0,0 +1,297 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+#include <stdlib.h>
+#include <unistd.h>
+#include "UARTDriver.h"
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <dev_fs_lib_serial.h>
+#include <AP_HAL/utility/RingBuffer.h>
+
+using namespace QURT;
+
+extern const AP_HAL::HAL& hal;
+
+UARTDriver::UARTDriver(const char *name) :
+    devname(name)
+{
+}
+
+void UARTDriver::begin(uint32_t b) 
+{
+    begin(b, 16384, 16384);
+}
+
+static const struct {
+    uint32_t baudrate;
+    enum DSPAL_SERIAL_BITRATES arg;
+} baudrate_table[] = {
+    {    9600, DSPAL_SIO_BITRATE_9600 },
+    {   14400, DSPAL_SIO_BITRATE_14400 },
+    {   19200, DSPAL_SIO_BITRATE_19200 },
+    {   38400, DSPAL_SIO_BITRATE_38400 },
+    {   57600, DSPAL_SIO_BITRATE_57600 },
+    {   76800, DSPAL_SIO_BITRATE_76800 },
+    {  115200, DSPAL_SIO_BITRATE_115200 },
+    {  230400, DSPAL_SIO_BITRATE_230400 },
+    {  250000, DSPAL_SIO_BITRATE_250000 },
+    {  460800, DSPAL_SIO_BITRATE_460800 },
+    {  921600, DSPAL_SIO_BITRATE_921600 },
+    { 2000000, DSPAL_SIO_BITRATE_2000000 },
+};
+
+extern "C" {
+static void read_callback_trampoline(void *, char *, size_t );
+}
+
+static void read_callback_trampoline(void *ctx, char *buf, size_t size)
+{
+    ((UARTDriver *)ctx)->_read_callback(buf, size);
+}
+
+/*
+  callback for incoming data
+ */
+void UARTDriver::_read_callback(char *buf, size_t size)
+{
+    if (readbuf == nullptr) {
+        return;
+    }
+    uint32_t n = readbuf->write((const uint8_t *)buf, size);
+    if (n != size) {
+        HAP_PRINTF("read_callback lost %u %u", n, size);
+    }
+}
+
+void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) 
+{
+    if (rxS < 4096) {
+        rxS = 4096;
+    }
+    if (txS < 4096) {
+        txS = 4096;
+    }
+    if (fd == -1) {
+        fd = open(devname, O_RDWR | O_NONBLOCK);
+        if (fd == -1) {
+            AP_HAL::panic("Unable to open %s", devname);
+        }
+    }
+
+    /*
+      allocate the read buffer
+      we allocate buffers before we successfully open the device as we
+      want to allocate in the early stages of boot, and cause minimum
+      thrashing of the heap once we are up. The ttyACM0 driver may not
+      connect for some time after boot
+     */
+    if (rxS != 0 && (readbuf == nullptr || rxS != readbuf->get_size())) {
+        initialised = false;
+        if (readbuf != nullptr) {
+            delete readbuf;
+        }
+        readbuf = new ByteBuffer(rxS);
+    }
+
+    /*
+      allocate the write buffer
+    */
+    if (txS != 0 && (writebuf == nullptr || txS != writebuf->get_size())) {
+        initialised = false;
+        if (writebuf != nullptr) {
+            delete writebuf;
+        }
+        writebuf = new ByteBuffer(txS);
+    }
+    
+    struct dspal_serial_ioctl_receive_data_callback callback;
+    callback.context = this;
+    callback.rx_data_callback_func_ptr = read_callback_trampoline;
+    int ret = ioctl(fd, SERIAL_IOCTL_SET_RECEIVE_DATA_CALLBACK, (void *)&callback);
+
+    if (b != 0) {
+        for (uint8_t i=0; i<ARRAY_SIZE(baudrate_table); i++) {
+            if (b <= baudrate_table[i].baudrate) {
+                struct dspal_serial_ioctl_data_rate rate;
+                rate.bit_rate = baudrate_table[i].arg;
+                ret = ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&rate);
+                break;
+            }
+        }
+    }
+
+    if (readbuf && writebuf && fd != -1) {
+        initialised = true;
+    }
+}
+
+void UARTDriver::end() 
+{
+    initialised = false;
+    if (fd != -1) {
+        ::close(fd);
+        fd = -1;
+    }
+    if (readbuf) {
+        delete readbuf;
+        readbuf = nullptr;
+    }
+    if (writebuf) {
+        delete writebuf;
+        writebuf = nullptr;
+    }
+
+}
+
+void UARTDriver::flush() 
+{
+}
+
+bool UARTDriver::is_initialized() 
+{ 
+    return fd != -1 && initialised; 
+}
+
+void UARTDriver::set_blocking_writes(bool blocking) 
+{
+    nonblocking_writes = !blocking;
+}
+
+bool UARTDriver::tx_pending()
+{
+    return false;
+}
+
+/* QURT implementations of Stream virtual methods */
+int16_t UARTDriver::available() 
+{ 
+    if (!initialised) {
+        return 0;
+    }
+    return readbuf->available();
+}
+
+int16_t UARTDriver::txspace() 
+{ 
+    if (!initialised) {
+        return 0;
+    }
+    return writebuf->space();
+}
+
+int16_t UARTDriver::read() 
+{ 
+    uint8_t c;
+    if (!initialised) {
+        return -1;
+    }
+    if (!lock.take(0)) {
+        return 0;
+    }
+    if (readbuf->empty()) {
+        lock.give();
+        return -1;
+    }
+    readbuf->read(&c, 1);
+    lock.give();
+    return c;
+}
+
+/* QURT implementations of Print virtual methods */
+size_t UARTDriver::write(uint8_t c) 
+{
+    if (!initialised) {
+        return 0;
+    }
+    if (!lock.take(0)) {
+        return 0;
+    }
+
+    while (writebuf->space() == 0) {
+        if (nonblocking_writes) {
+            lock.give();
+            return 0;
+        }
+        hal.scheduler->delay_microseconds(1000);
+    }
+    writebuf->write(&c, 1);
+    lock.give();
+    return 1;
+}
+
+size_t UARTDriver::write(const uint8_t *buffer, size_t size)
+{
+    if (!initialised) {
+        return 0;
+    }
+    if (!nonblocking_writes) {
+        /*
+          use the per-byte delay loop in write() above for blocking writes
+         */
+        size_t ret = 0;
+        while (size--) {
+            if (write(*buffer++) != 1) break;
+            ret++;
+        }
+        return ret;
+    }
+
+    if (!lock.take(0)) {
+        return 0;
+    }
+    size_t ret = writebuf->write(buffer, size);
+    lock.give();
+    return ret;
+}
+
+/*
+  push any pending bytes to/from the serial port. This is called at
+  1kHz in the timer thread
+ */
+void UARTDriver::timer_tick(void)
+{
+    uint16_t n;
+
+    if (fd == -1 || !initialised || !lock.take_nonblocking()) {
+        return;
+    }
+
+    in_timer = true;
+    
+    // write any pending bytes
+    n = writebuf->available();
+    if (n == 0) {
+        in_timer = false;
+        lock.give();
+        return;
+    }
+    if (n > 64) {
+        n = 64;
+    }
+    uint8_t buf[n];
+    writebuf->read(buf, n);
+    ::write(fd, buf, n);
+    lock.give();
+    
+    in_timer = false;
+}
+
+#endif

libraries/AP_HAL_QURT/UARTDriver.h

@@ -0,0 +1,63 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "AP_HAL_QURT.h"
+#include "Semaphores.h"
+#include <AP_HAL/utility/RingBuffer.h>
+
+class QURT::UARTDriver : public AP_HAL::UARTDriver {
+public:
+    UARTDriver(const char *name);
+
+    /* QURT implementations of UARTDriver virtual methods */
+    void begin(uint32_t b);
+    void begin(uint32_t b, uint16_t rxS, uint16_t txS);
+    void end();
+    void flush();
+    bool is_initialized();
+    void set_blocking_writes(bool blocking);
+    bool tx_pending();
+
+    /* QURT implementations of Stream virtual methods */
+    int16_t available();
+    int16_t txspace();
+    int16_t read();
+
+    /* QURT implementations of Print virtual methods */
+    size_t write(uint8_t c);
+    size_t write(const uint8_t *buffer, size_t size);
+
+    void _read_callback(char *buf, size_t size);
+
+    void timer_tick(void);
+    
+private:
+    const char *devname;
+    int fd = -1;
+    Semaphore lock;
+
+    ByteBuffer *readbuf;
+    ByteBuffer *writebuf;
+
+    bool nonblocking_writes = false;
+    volatile bool in_timer = false;
+    volatile bool initialised = false;
+
+    uint64_t last_write_time_us;
+
+    int write_fd(const uint8_t *buf, uint16_t n);
+};

libraries/AP_HAL_QURT/UDPDriver.cpp

@@ -0,0 +1,254 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+#include <stdlib.h>
+#include <unistd.h>
+#include "UDPDriver.h"
+#include <stdio.h>
+#include <AP_HAL/utility/RingBuffer.h>
+#include <AP_Math/AP_Math.h>
+
+using namespace QURT;
+
+extern const AP_HAL::HAL& hal;
+
+void UDPDriver::begin(uint32_t b) 
+{
+    begin(b, 16384, 16384);
+}
+
+void UDPDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) 
+{
+    rxS = constrain_int32(rxS, 16384, 30000);
+    txS = constrain_int32(txS, 16384, 30000);
+
+    /*
+      allocate the read buffer
+     */
+    if (rxS != 0 && (readbuf == nullptr || rxS != readbuf->get_size())) {
+        initialised = false;
+        if (readbuf != nullptr) {
+            delete readbuf;
+        }
+        readbuf = new ByteBuffer(rxS);
+    }
+
+    /*
+      allocate the write buffer
+    */
+    if (txS != 0 && (writebuf == nullptr || txS != writebuf->get_size())) {
+        initialised = false;
+        if (writebuf != nullptr) {
+            delete writebuf;
+        }
+        writebuf = new ByteBuffer(txS);
+    }
+    
+    if (readbuf && writebuf) {
+        initialised = true;
+    }
+}
+
+void UDPDriver::end() 
+{
+    initialised = false;
+    if (readbuf) {
+        delete readbuf;
+        readbuf = nullptr;
+    }
+    if (writebuf) {
+        delete writebuf;
+        writebuf = nullptr;
+    }
+
+}
+
+void UDPDriver::flush() 
+{
+}
+
+bool UDPDriver::is_initialized() 
+{ 
+    return initialised; 
+}
+
+void UDPDriver::set_blocking_writes(bool blocking) 
+{
+    nonblocking_writes = !blocking;
+}
+
+bool UDPDriver::tx_pending()
+{
+    return false;
+}
+
+/* QURT implementations of Stream virtual methods */
+int16_t UDPDriver::available() 
+{ 
+    if (!initialised) {
+        return 0;
+    }
+    return readbuf->available();
+}
+
+int16_t UDPDriver::txspace() 
+{ 
+    if (!initialised) {
+        return 0;
+    }
+    return writebuf->space();
+}
+
+int16_t UDPDriver::read() 
+{ 
+    uint8_t c;
+    if (!initialised) {
+        return -1;
+    }
+    if (!lock.take(0)) {
+        return 0;
+    }
+    if (readbuf->empty()) {
+        lock.give();
+        return -1;
+    }
+    readbuf->read(&c, 1);
+    lock.give();
+    return c;
+}
+
+/* QURT implementations of Print virtual methods */
+size_t UDPDriver::write(uint8_t c) 
+{
+    if (!initialised) {
+        return 0;
+    }
+    if (!lock.take(0)) {
+        return 0;
+    }
+
+    while (writebuf->space() == 0) {
+        if (nonblocking_writes) {
+            lock.give();
+            return 0;
+        }
+        hal.scheduler->delay_microseconds(1000);
+    }
+    writebuf->write(&c, 1);
+    lock.give();
+    return 1;
+}
+
+size_t UDPDriver::write(const uint8_t *buffer, size_t size)
+{
+    if (!initialised) {
+        return 0;
+    }
+    if (!nonblocking_writes) {
+        /*
+          use the per-byte delay loop in write() above for blocking writes
+         */
+        size_t ret = 0;
+        while (size--) {
+            if (write(*buffer++) != 1) break;
+            ret++;
+        }
+        return ret;
+    }
+
+    if (!lock.take(0)) {
+        return 0;
+    }
+    size_t ret = writebuf->write(buffer, size);
+    lock.give();
+    return ret;
+}
+
+uint32_t UDPDriver::socket_check(uint8_t *buf, int len, uint32_t *nbytes)
+{
+    if (!initialised) {
+        return 1;
+    }
+    if (!writebuf) {
+        return 1;
+    }
+    *nbytes = writebuf->available();
+    if (*nbytes == 0) {
+        return 1;
+    }
+    if (*nbytes > len) {
+        *nbytes = len;
+    }
+    uint32_t n = *nbytes;
+    if (writebuf->peek(0) != 254) {
+        /*
+          we have a non-mavlink packet at the start of the
+          buffer. Look ahead for a MAVLink start byte, up to 256 bytes
+          ahead
+         */
+        uint16_t limit = n>256?256:n;
+        uint16_t i;
+        for (i=0; i<limit; i++) {
+            if (writebuf->peek(i) == 254) {
+                n = i;
+                break;
+            }
+        }
+        // if we didn't find a MAVLink marker then limit the send size to 256
+        if (i == limit) {
+            n = limit;
+        }
+    } else {
+        // this looks like a MAVLink packet - try to write on
+        // packet boundaries when possible
+        if (n < 8) {
+            n = 0;
+        } else {
+            // the length of the packet is the 2nd byte, and mavlink
+            // packets have a 6 byte header plus 2 byte checksum,
+            // giving len+8 bytes
+            uint8_t len = writebuf->peek(1);
+            if (n < len+8) {
+                // we don't have a full packet yet
+                n = 0;
+            } else if (n > len+8) {
+                // send just 1 packet at a time (so MAVLink packets
+                // are aligned on UDP boundaries)
+                n = len+8;
+            }
+        }        
+    }
+
+    *nbytes = n;
+    writebuf->read(buf, *nbytes);
+    return 0;
+}
+
+uint32_t UDPDriver::socket_input(const uint8_t *buf, int len, uint32_t *nbytes)
+{
+    if (!initialised) {
+        return 1;
+    }
+    if (!readbuf) {
+        return 1;
+    }
+    *nbytes = readbuf->write(buf, len);
+    return 0;
+}
+#endif // CONFIG_HAL_BOARD
+

libraries/AP_HAL_QURT/UDPDriver.h

@@ -0,0 +1,55 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "AP_HAL_QURT.h"
+#include "Semaphores.h"
+#include <AP_HAL/utility/RingBuffer.h>
+
+class QURT::UDPDriver : public AP_HAL::UARTDriver {
+public:
+    static UDPDriver *from(AP_HAL::UARTDriver *d) {
+        return static_cast<UDPDriver*>(d);
+    }
+
+    void begin(uint32_t b);
+    void begin(uint32_t b, uint16_t rxS, uint16_t txS);
+    void end();
+    void flush();
+    bool is_initialized();
+    void set_blocking_writes(bool blocking);
+    bool tx_pending();
+
+    int16_t available();
+    int16_t txspace();
+    int16_t read();
+
+    size_t write(uint8_t c);
+    size_t write(const uint8_t *buffer, size_t size);
+
+    uint32_t socket_check(uint8_t *buf, int len, uint32_t *nbytes);
+    uint32_t socket_input(const uint8_t *buf, int len, uint32_t *nbytes);
+
+    enum flow_control get_flow_control(void) { return FLOW_CONTROL_ENABLE; };
+    
+private:
+    Semaphore lock;
+    bool initialised;
+    bool nonblocking_writes = true;
+    
+    ByteBuffer *readbuf;
+    ByteBuffer *writebuf;
+};

libraries/AP_HAL_QURT/Util.cpp

@@ -0,0 +1,27 @@
+#include <AP_HAL/AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+#include "Semaphores.h"
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+
+#include "Util.h"
+
+using namespace QURT;
+
+/*
+  always report 256k of free memory. I don't know how to query
+  available memory on QURT
+ */
+uint32_t Util::available_memory(void)
+{
+    return 256*1024;
+}
+
+// create a new semaphore
+Semaphore *Util::new_semaphore(void)
+{
+    return new Semaphore;
+}
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_QURT

libraries/AP_HAL_QURT/Util.h

@@ -0,0 +1,33 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_HAL_QURT_Namespace.h"
+
+class QURT::Util : public AP_HAL::Util {
+public:
+    Util(void) {
+        HAP_PRINTF("%s constructor", __FUNCTION__);
+    }
+    bool run_debug_shell(AP_HAL::BetterStream *stream) override { return false; }
+
+    uint32_t available_memory(void) override;
+
+    // create a new semaphore
+    Semaphore *new_semaphore(void) override;
+};
+

libraries/AP_HAL_QURT/dsp_main.cpp

@@ -0,0 +1,88 @@
+#include <AP_HAL/AP_HAL.h>
+#include "UDPDriver.h"
+#include <stdio.h>
+#include <stdint.h>
+#include <unistd.h>
+#include "Scheduler.h"
+#include "Storage.h"
+#include "replace.h"
+#include <qurt_dsp.h>
+
+extern const AP_HAL::HAL& hal;
+
+extern "C" {
+    int ArduPilot_main(int argc, const char *argv[]);
+}
+
+volatile int _last_dsp_line = __LINE__;
+volatile const char *_last_dsp_file = __FILE__;
+volatile uint32_t _last_counter;
+
+static void *main_thread(void *)
+{
+    HAP_PRINTF("Ardupilot main_thread started");
+    ArduPilot_main(0, NULL);
+    return NULL;
+}
+
+
+uint32_t ardupilot_start()
+{
+    HAP_PRINTF("Starting Ardupilot");
+    pthread_attr_t thread_attr;
+    struct sched_param param;
+    pthread_t thread_ctx;
+
+    pthread_attr_init(&thread_attr);
+    pthread_attr_setstacksize(&thread_attr, 80000);
+
+    param.sched_priority = APM_MAIN_PRIORITY;
+    (void)pthread_attr_setschedparam(&thread_attr, &param);
+
+    pthread_create(&thread_ctx, &thread_attr, main_thread, NULL);
+    return 0;
+}
+
+uint32_t ardupilot_heartbeat()
+{
+    HAP_PRINTF("tick %u %s:%d", (unsigned)_last_counter, _last_dsp_file, _last_dsp_line);
+    return 0;
+}
+
+uint32_t ardupilot_set_storage(const uint8_t *buf, int len)
+{
+    if (len != sizeof(QURT::Storage::buffer)) {
+        HAP_PRINTF("Incorrect storage size %u", len);
+        return 1;
+    }
+    QURT::Storage::lock.take(0);
+    memcpy(QURT::Storage::buffer, buf, len);
+    QURT::Storage::lock.give();
+    return 0;
+}
+
+uint32_t ardupilot_get_storage(uint8_t *buf, int len)
+{
+    if (len != sizeof(QURT::Storage::buffer)) {
+        HAP_PRINTF("Incorrect storage size %u", len);
+        return 1;
+    }
+    if (!QURT::Storage::dirty) {
+        return 1;
+    }
+    QURT::Storage::lock.take(0);
+    memcpy(buf, QURT::Storage::buffer, len);
+    QURT::Storage::lock.give();
+    return 0;
+}
+
+
+uint32_t ardupilot_socket_check(uint8_t *buf, int len, uint32_t *nbytes)
+{
+    return QURT::UDPDriver::from(hal.uartA)->socket_check(buf, len, nbytes);
+}
+
+uint32_t ardupilot_socket_input(const uint8_t *buf, int len, uint32_t *nbytes)
+{
+    return QURT::UDPDriver::from(hal.uartA)->socket_input(buf, len, nbytes);
+}

libraries/AP_HAL_QURT/mainapp/mainapp.cpp

@@ -0,0 +1,133 @@
+/*
+  main program for HAL_QURT port
+ */
+#include <stdio.h>
+#include <stdint.h>
+#include <sys/types.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <time.h>
+#include <errno.h>
+#include <AP_HAL/utility/Socket.h>
+#include <qurt_dsp.h>
+
+static SocketAPM sock{true};
+static bool connected;
+static uint32_t last_get_storage_us;
+static uint64_t start_time;
+
+// location of virtual eeprom in Linux filesystem
+#define STORAGE_DIR "/var/APM"
+#define STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
+
+// time since startup in microseconds
+static uint64_t micros64()
+{
+    struct timespec ts;
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    uint64_t ret = ts.tv_sec*1000*1000ULL + ts.tv_nsec/1000U;
+    if (start_time == 0) {
+        start_time = ret;
+    }
+    ret -= start_time;
+    return ret;
+}
+
+/*
+  send storage file to DSPs
+ */
+static void send_storage(void)
+{
+    int fd = open(STORAGE_FILE, O_RDWR|O_CREAT, 0644);
+    if (fd == -1) {
+        printf("Unable to open %s", STORAGE_FILE);
+        exit(1);
+    }
+    uint8_t buf[16384];
+    memset(buf, 0, sizeof(buf));
+    read(fd, buf, sizeof(buf));
+    if (ardupilot_set_storage(buf, sizeof(buf)) != 0) {
+        printf("Failed to send initial storage");
+        exit(1);        
+    }
+    close(fd);
+}
+
+/*
+  get updated storage file from DSPs
+ */
+static void get_storage(void)
+{
+    uint8_t buf[16384];
+    if (ardupilot_get_storage(buf, sizeof(buf)) != 0) {
+        return;
+    }
+    int fd = open(STORAGE_FILE ".new", O_WRONLY);
+    if (fd == -1) {
+        printf("Unable to open %s - %s\n", STORAGE_FILE ".new", strerror(errno));
+    }
+    write(fd, buf, sizeof(buf));
+    close(fd);
+    // atomic rename
+    if (rename(STORAGE_FILE ".new", STORAGE_FILE) != 0) {
+        printf("Unable to rename to %s - %s\n", STORAGE_FILE, strerror(errno));
+    }
+}
+
+/*
+  handle any incoming or outgoing UDP socket data on behalf of the DSPs
+ */
+static void socket_check(void)
+{
+    uint8_t buf[300];
+    ssize_t ret = sock.recv(buf, sizeof(buf), 0);
+    if (ret > 0) {
+        uint32_t nbytes;
+        ardupilot_socket_input(buf, ret, &nbytes);
+        if (!connected) {
+            const char *ip;
+            uint16_t port;
+            sock.last_recv_address(ip, port);
+            connected = sock.connect(ip, port);
+            if (connected) {
+                printf("Connected to UDP %s:%u\n", ip, (unsigned)port);
+            }
+            sock.set_blocking(false);
+        }
+    }
+    uint32_t nbytes;
+    if (ardupilot_socket_check(buf, sizeof(buf), &nbytes) == 0) {
+        if (!connected) {
+            sock.sendto(buf, nbytes, "255.255.255.255", 14550);
+        } else {
+            sock.send(buf, nbytes);
+        }
+    }
+}
+
+/*
+  main program
+ */
+int main(int argc, const char *argv[])
+{
+    sock.set_broadcast();
+
+    printf("Starting DSP code\n");
+    send_storage();
+        
+    ardupilot_start();
+    while (true) {
+        uint64_t now = micros64();
+        if (now - last_get_storage_us > 1000*1000) {
+            printf("tick t=%.6f\n", now*1.0e-6f);
+            ardupilot_heartbeat();
+            get_storage();
+            last_get_storage_us = now;
+        }
+        socket_check();
+        usleep(5000);
+    }
+}

libraries/AP_HAL_QURT/qurt_dsp.idl

@@ -0,0 +1,17 @@
+#include "AEEStdDef.idl"
+
+interface ardupilot {
+    // start main thread
+    uint32 start();
+
+    // a heartbeat for debugging
+    uint32 heartbeat();
+
+    // get eeprom updates
+    uint32 set_storage(in sequence<uint8> eeprom);    
+    uint32 get_storage(rout sequence<uint8> eeprom);
+
+    // handle socket data
+    uint32 socket_check(rout sequence<uint8> buf, rout uint32 nbytes);
+    uint32 socket_input(in sequence<uint8> buf, rout uint32 nbytes);
+};

libraries/AP_HAL_QURT/replace.cpp

@@ -0,0 +1,76 @@
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <dirent.h>
+#include "replace.h"
+
+extern "C" {
+
+// this is not declared in qurt headers
+void HAP_debug(const char *msg, int level, const char *filename, int line);
+    
+void HAP_printf(const char *file, int line, const char *format, ...)
+{
+	va_list ap;
+        char buf[300];
+        
+	va_start(ap, format);
+	vsnprintf(buf, sizeof(buf), format, ap);
+	va_end(ap);
+        HAP_debug(buf, 0, file, line);
+        //usleep(20000);
+}
+    
+/**
+   QURT doesn't have strnlen()
+**/
+size_t strnlen(const char *s, size_t max)
+{
+        size_t len;
+  
+        for (len = 0; len < max; len++) {
+                if (s[len] == '\0') {
+                        break;
+                }
+        }
+        return len;  
+}
+
+int vasprintf(char **ptr, const char *format, va_list ap)
+{
+	int ret;
+	va_list ap2;
+
+	va_copy(ap2, ap);
+	ret = vsnprintf(NULL, 0, format, ap2);
+	va_end(ap2);
+	if (ret < 0) return ret;
+
+	(*ptr) = (char *)malloc(ret+1);
+	if (!*ptr) return -1;
+
+	va_copy(ap2, ap);
+	ret = vsnprintf(*ptr, ret+1, format, ap2);
+	va_end(ap2);
+
+	return ret;
+}
+
+int asprintf(char **ptr, const char *format, ...)
+{
+	va_list ap;
+	int ret;
+	
+	*ptr = NULL;
+	va_start(ap, format);
+	ret = vasprintf(ptr, format, ap);
+	va_end(ap);
+
+	return ret;
+}
+
+}

libraries/AP_HAL_QURT/replace.h

@@ -0,0 +1,39 @@
+#pragma once
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+#include <stdlib.h>
+#include <stdint.h>
+#include <types.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <dirent.h>
+
+#include <types.h>
+#include <dirent.h>
+extern "C" {
+
+    /*
+      work around broken headers
+     */
+    size_t strnlen(const char *s, size_t maxlen);
+    int asprintf(char **, const char *, ...);
+    off_t lseek(int, off_t, int);
+    DIR *opendir (const char *);
+    int unlink(const char *pathname);
+
+    //typedef int32_t pid_t;
+    pid_t getpid (void);
+    
+    void HAP_printf(const char *file, int line, const char *fmt, ...);
+}
+
+#define HAP_PRINTF(...) HAP_printf(__FILE__, __LINE__, __VA_ARGS__)
+
+extern volatile int _last_dsp_line;
+extern volatile const char *_last_dsp_file;
+extern volatile uint32_t _last_counter;
+
+#define HAP_LINE() do { _last_dsp_line = __LINE__; _last_dsp_file = __FILE__; _last_counter++; } while (0)
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_QURT/system.cpp

@@ -0,0 +1,66 @@
+#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 <sys/timespec.h>
+#include <dspal_time.h>
+#include "replace.h"
+#include <fenv.h>
+
+extern const AP_HAL::HAL& hal;
+
+namespace AP_HAL {
+
+static struct {
+    uint64_t start_time;
+} state;
+
+void init()
+{
+    state.start_time = micros64();
+    // we don't want exceptions in flight code. That is the job of SITL
+    feclearexcept(FE_OVERFLOW | FE_DIVBYZERO | FE_INVALID);
+}
+
+void panic(const char *errormsg, ...)
+{
+    char buf[200];
+    va_list ap;
+    va_start(ap, errormsg);
+    vsnprintf(buf, sizeof(buf), errormsg, ap);
+    va_end(ap);
+    HAP_PRINTF(buf);
+    usleep(2000000);
+    hal.rcin->deinit();
+    exit(1);
+}
+
+uint32_t micros()
+{
+    return micros64() & 0xFFFFFFFF;
+}
+
+uint32_t millis()
+{
+    return millis64() & 0xFFFFFFFF;
+}
+
+uint64_t micros64()
+{
+    struct timespec ts;
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    uint64_t ret = ts.tv_sec*1000*1000ULL + ts.tv_nsec/1000U;
+    ret -= state.start_time;
+    return ret;
+}
+
+uint64_t millis64()
+{
+    return micros64() / 1000;
+}
+
+} // namespace AP_HAL