HAL_Linux: added qflight board subtype

libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h

@@ -30,6 +30,7 @@ namespace Linux {
     class RCInput_ZYNQ;
     class RCInput_UART;
     class RCInput_UDP;
+    class RCInput_DSM;
     class RCOutput_PRU;
     class RCOutput_AioPRU;
     class RCOutput_PCA9685;
@@ -37,6 +38,7 @@ namespace Linux {
     class RCOutput_ZYNQ;
     class RCOutput_Bebop;
     class RCOutput_Sysfs;
+    class RCOutput_QFLIGHT;
     class Semaphore;
     class Scheduler;
     class Util;

libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h

@@ -22,6 +22,7 @@
 #include "RCInput_UART.h"
 #include "RCInput_UDP.h"
 #include "RCInput_Raspilot.h"
+#include "RCInput_DSM.h"
 #include "RCOutput_PRU.h"
 #include "RCOutput_AioPRU.h"
 #include "RCOutput_PCA9685.h"
@@ -29,6 +30,7 @@
 #include "RCOutput_Bebop.h"
 #include "RCOutput_Raspilot.h"
 #include "RCOutput_Sysfs.h"
+#include "RCOutput_qflight.h"
 #include "Semaphores.h"
 #include "Scheduler.h"
 #include "ToneAlarmDriver.h"

libraries/AP_HAL_Linux/GPIO_Sysfs.cpp

@@ -2,7 +2,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
 
 #include <fcntl.h>
 #include <linux/limits.h>

libraries/AP_HAL_Linux/HAL_Linux_Class.cpp

@@ -38,6 +38,7 @@ static RPIOUARTDriver uartCDriver;
 #else
 static UARTDriver uartCDriver(false);
 #endif
+static UARTDriver uartDDriver(false);
 static UARTDriver uartEDriver(false);
 
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
@@ -63,6 +64,9 @@ static I2CDriver  i2cDriver0(&i2cSemaphore0, i2c_devpaths);
 /* One additional emulated bus */
 static Semaphore  i2cSemaphore1;
 static I2CDriver  i2cDriver1(&i2cSemaphore1, "/dev/i2c-10");
+#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+static Semaphore  i2cSemaphore0;
+static Empty::I2CDriver i2cDriver0(&i2cSemaphore0);
 #else
 static Semaphore  i2cSemaphore0;
 static I2CDriver  i2cDriver0(&i2cSemaphore0, "/dev/i2c-1");
@@ -74,6 +78,8 @@ static SPIDeviceManager spiDeviceManager;
 static ADS1115AnalogIn analogIn;
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
 static RaspilotAnalogIn analogIn;
+#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+static Empty::AnalogIn analogIn;
 #else
 static AnalogIn analogIn;
 #endif
@@ -125,6 +131,8 @@ static RCInput_ZYNQ rcinDriver;
 static RCInput_UDP  rcinDriver;
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
 static RCInput_UART rcinDriver("/dev/ttyS2");
+#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+static RCInput_DSM rcinDriver;
 #else
 static RCInput rcinDriver;
 #endif
@@ -154,6 +162,8 @@ static RCOutput_ZYNQ rcoutDriver;
 static RCOutput_Bebop rcoutDriver;
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
 static RCOutput_PCA9685 rcoutDriver(PCA9685_PRIMARY_ADDRESS, false, 0, MINNOW_GPIO_S5_1);
+#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+static RCOutput_QFLIGHT rcoutDriver;
 #else
 static Empty::RCOutput rcoutDriver;
 #endif
@@ -171,7 +181,7 @@ HAL_Linux::HAL_Linux() :
         &uartADriver,
         &uartBDriver,
         &uartCDriver,
-        NULL,            /* no uartD */
+        &uartDDriver,
         &uartEDriver,
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
         &i2cDriver0,
@@ -200,7 +210,7 @@ HAL_Linux::HAL_Linux() :
 
 void _usage(void)
 {
-    printf("Usage: -A uartAPath -B uartBPath -C uartCPath\n");
+    printf("Usage: -A uartAPath -B uartBPath -C uartCPath -D uartDPath -E uartEPath\n");
     printf("Options:\n");
     printf("\t-serial:          -A /dev/ttyO4\n");
     printf("\t                  -B /dev/ttyS1\n");    
@@ -224,14 +234,19 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const
         {"uartA",         true,  0, 'A'},
         {"uartB",         true,  0, 'B'},
         {"uartC",         true,  0, 'C'},
+        {"uartD",         true,  0, 'D'},
         {"uartE",         true,  0, 'E'},
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+        {"dsm",           true,  0, 'S'},
+        {"ESC",           true,  0, 'e'},
+#endif
         {"log-directory",       true,  0, 'l'},
         {"terrain-directory",   true,  0, 't'},
         {"help",                false,  0, 'h'},
         {0, false, 0, 0}
     };
 
-    GetOptLong gopt(argc, argv, "A:B:C:E:l:t:h",
+    GetOptLong gopt(argc, argv, "A:B:C:D:E:l:t:he:S",
                     options);
 
     /*
@@ -248,9 +263,20 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const
         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;
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+        case 'e':
+            rcoutDriver.set_device_path(gopt.optarg);
+            break;
+        case 'S':
+            rcinDriver.set_device_path(gopt.optarg);
+            break;
+#endif // CONFIG_HAL_BOARD_SUBTYPE
         case 'l':            
             utilInstance.set_custom_log_directory(gopt.optarg);
             break;

libraries/AP_HAL_Linux/RCInput.cpp

@@ -16,7 +16,7 @@
 
 #include "RCInput.h"
 #include "sbus.h"
-#include "dsm.h"
+#include <AP_HAL/utility/dsm.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -374,4 +374,64 @@ void RCInput::_update_periods(uint16_t *periods, uint8_t len)
     new_rc_input = true;
 }
 
+
+/*
+  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
+                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_Linux/RCInput.h

@@ -31,6 +31,10 @@ public:
     // specific implementations
     virtual void _timer_tick() {}
 
+    // add some DSM input bytes, for RCInput over a serial port
+    void add_dsm_input(const uint8_t *bytes, size_t nbytes);
+    
+    
  protected:
     void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1,
                            uint16_t channel = LINUX_RC_INPUT_CHANNEL_INVALID);
@@ -67,6 +71,13 @@ public:
         uint16_t bytes[16]; // including start bit and stop bit
         uint16_t bit_ofs;
     } dsm_state;
+
+    // state of add_dsm_input
+    struct {
+        uint8_t frame[16];
+        uint8_t partial_frame_count;
+        uint32_t last_input_ms;
+    } dsm;
 };
 
 #include "RCInput_PRU.h"

libraries/AP_HAL_Linux/RCInput_DSM.cpp

@@ -0,0 +1,74 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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/>.
+ */
+/*
+  this is a driver for DSM input in the QFLIGHT board. It could be
+  extended to other boards in future by providing an open/read/write
+  abstraction
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+#include "RCInput_DSM.h"
+#include <AP_HAL_Linux/qflight/qflight_util.h>
+#include <AP_HAL_Linux/qflight/qflight_dsp.h>
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+
+using namespace Linux;
+
+void RCInput_DSM::init()
+{
+}
+
+void RCInput_DSM::set_device_path(const char *path)
+{
+    device_path = path;
+    printf("Set DSM device path %s\n", path);
+}
+
+void RCInput_DSM::_timer_tick(void)
+{
+    if (device_path == nullptr) {
+        return;
+    }
+    int ret;
+    /*
+      we defer the open to the timer tick to ensure all RPC calls are
+      made in the same thread
+     */
+    if (fd == -1) {
+        ret = qflight_UART_open(device_path, &fd);
+        if (ret == 0) {
+            printf("Opened DSM input %s fd=%d\n", device_path, (int)fd);
+            fflush(stdout);
+            qflight_UART_set_baudrate(fd, 115200);
+        }
+    }
+    if (fd != -1) {
+        uint8_t bytes[16];
+        int32_t nread;
+        ret = qflight_UART_read(fd, bytes, sizeof(bytes), &nread);
+        if (ret == 0 && nread > 0) {
+            // printf("Read %u DSM bytes at %u\n", (unsigned)nread, AP_HAL::millis());
+            fflush(stdout);
+            add_dsm_input(bytes, nread);
+        }
+    }
+}
+#endif // CONFIG_HAL_BOARD_SUBTYPE
+

libraries/AP_HAL_Linux/RCInput_DSM.h

@@ -0,0 +1,36 @@
+/*
+   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>
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+
+#include "RCInput.h"
+#include "RCInput_DSM.h"
+
+class Linux::RCInput_DSM : public Linux::RCInput
+{
+public:
+    void init() override;
+    void _timer_tick(void) override;
+    void set_device_path(const char *path);
+
+private:
+    const char *device_path;
+    int32_t fd = -1;
+};
+#endif // CONFIG_HAL_BOARD_SUBTYPE
+

libraries/AP_HAL_Linux/RCOutput_qflight.cpp

@@ -0,0 +1,176 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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/>.
+ */
+/*
+  this is a driver for RC output in the QFLIGHT board. Output goes via
+  a UART with a CRC. See libraries/RC_Channel/examples/RC_UART for an
+  example of the other end of this protocol
+ */
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+
+#include "RCOutput_qflight.h"
+#include <AP_HAL_Linux/qflight/qflight_util.h>
+#include <AP_HAL_Linux/qflight/qflight_dsp.h>
+#include <GCS_MAVLink/include/mavlink/v1.0/checksum.h>
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+
+using namespace Linux;
+
+void RCOutput_QFLIGHT::init()
+{
+    hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&RCOutput_QFLIGHT::timer_update, void));
+}
+
+void RCOutput_QFLIGHT::set_device_path(const char *_device)
+{
+    device = _device;
+}
+
+
+void RCOutput_QFLIGHT::set_freq(uint32_t chmask, uint16_t freq_hz)
+{
+    // no support for changing frequency yet
+}
+
+uint16_t RCOutput_QFLIGHT::get_freq(uint8_t ch)
+{
+    // return fixed fake value - no control of frequency over the UART
+    return 490;
+}
+
+void RCOutput_QFLIGHT::enable_ch(uint8_t ch)
+{
+    if (ch >= channel_count) {
+        return;
+    }
+    enable_mask |= 1U<<ch;
+}
+
+void RCOutput_QFLIGHT::disable_ch(uint8_t ch)
+{
+    if (ch >= channel_count) {
+        return;
+    }
+    enable_mask &= ~1U<<ch;
+}
+
+void RCOutput_QFLIGHT::write(uint8_t ch, uint16_t period_us)
+{
+    if (ch >= channel_count) {
+        return;
+    }
+    period[ch] = period_us;
+    need_write = true;
+}
+
+uint16_t RCOutput_QFLIGHT::read(uint8_t ch)
+{
+    if (ch >= channel_count) {
+        return 0;
+    }
+    return period[ch];
+}
+
+void RCOutput_QFLIGHT::read(uint16_t *period_us, uint8_t len)
+{
+    for (int i = 0; i < len; i++) {
+        period_us[i] = read(i);
+    }
+}
+
+void RCOutput_QFLIGHT::timer_update(void)
+{
+    /*
+      we defer the open to the time to ensure all RPC calls are made
+      from the same thread
+     */
+    if (fd == -1 && device != nullptr) {
+        int ret = qflight_UART_open(device, &fd);
+        printf("Opened ESC UART %s ret=%d fd=%d\n",
+               device, ret, (int)fd);
+        if (fd != -1) {
+            qflight_UART_set_baudrate(fd, baudrate);
+        }
+    }
+    if (!need_write || fd == -1) {
+        return;
+    }
+    /*
+      this implements the PWM over UART prototocol. 
+     */
+    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);
+    int32_t nwritten;
+    qflight_UART_write(fd, (uint8_t *)&frame, sizeof(frame), &nwritten);
+    need_write = false;
+
+    check_rc_in();
+}
+
+/*
+  we accept RC input from the UART and treat it as RC overrides. This
+  is an lazy way to allow an RCOutput driver to do RCInput. See the
+  RC_UART example for the other end of this protocol
+ */
+void RCOutput_QFLIGHT::check_rc_in(void)
+{
+    const uint8_t magic = 0xf6;
+    while (nrcin_bytes != sizeof(rcu.bytes)) {
+        int32_t nread;
+        if (qflight_UART_read(fd, rcu.bytes, sizeof(rcu.bytes)-nrcin_bytes, &nread) != 0 || nread <= 0) {
+            return;
+        }
+        nrcin_bytes += nread;
+        if (rcu.rcin.magic != magic) {
+            for (uint8_t i=1; i<nrcin_bytes; i++) {
+                if (rcu.bytes[i] == magic) {
+                    memmove(&rcu.bytes[0], &rcu.bytes[i], nrcin_bytes-i);
+                    nrcin_bytes = nrcin_bytes - i;
+                    return;
+                }
+            }
+            nrcin_bytes = 0;
+            return;
+        }
+    }
+    if (nrcin_bytes == sizeof(rcu.bytes)) {
+        if (rcu.rcin.magic == 0xf6 &&
+            crc_calculate((uint8_t*)rcu.rcin.rcin, sizeof(rcu.rcin.rcin)) == rcu.rcin.crc) {
+            bool have_data = false;
+            for (uint8_t i=0; i<8; i++) {
+                if (rcu.rcin.rcin[i] != 0) {
+                    have_data = true;
+                    break;
+                }
+            }
+            if (have_data) {
+                hal.rcin->set_overrides((int16_t*)rcu.rcin.rcin, 8);
+            }
+        }
+        nrcin_bytes = 0;
+    }
+}
+
+#endif // CONFIG_HAL_BOARD_SUBTYPE
+

libraries/AP_HAL_Linux/RCOutput_qflight.h

@@ -0,0 +1,47 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_HAL_Linux.h"
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+
+class Linux::RCOutput_QFLIGHT : public AP_HAL::RCOutput {
+public:
+    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 set_device_path(const char *device);
+
+private:
+    const char *device = nullptr;
+    const uint32_t baudrate = 115200;    
+    static const uint8_t channel_count = 4;
+    
+    int32_t fd = -1;
+    uint16_t enable_mask;
+    uint16_t period[channel_count];
+    volatile bool need_write;
+    
+    void timer_update(void);
+
+    void check_rc_in(void);
+    
+    uint32_t last_read_check_ms;
+    struct PACKED rcin_frame {
+        uint8_t magic;
+        uint16_t rcin[8];
+        uint16_t crc;
+    };
+    union {
+        struct rcin_frame rcin;
+        uint8_t bytes[19];
+    } rcu;
+    uint8_t nrcin_bytes;
+};
+
+#endif // CONFIG_HAL_BOARD_SUBTYPE

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -16,6 +16,14 @@
 #include <errno.h>
 #include <sys/mman.h>
 
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+#include <rpcmem.h>
+#include <AP_HAL_Linux/qflight/qflight_util.h>
+#include <AP_HAL_Linux/qflight/qflight_dsp.h>
+#include <AP_HAL_Linux/qflight/qflight_buffer.h>
+#endif
+
+
 using namespace Linux;
 
 extern const AP_HAL::HAL& hal;
@@ -261,7 +269,13 @@ void *Scheduler::_timer_thread(void* arg)
     while (sched->system_initializing()) {
         poll(NULL, 0, 1);
     }
-    /*
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+    printf("Initialising rpcmem\n");
+    rpcmem_init();
+#endif
+    
+/*
       this aims to run at an average of 1kHz, so that it can be used
       to drive 1kHz processes without drift
      */
@@ -277,6 +291,16 @@ void *Scheduler::_timer_thread(void* arg)
         next_run_usec += 1000;
         // run registered timers
         sched->_run_timers(true);
+
+#if HAL_LINUX_UARTS_ON_TIMER_THREAD
+        /*
+          some boards require that UART calls happen on the same
+          thread as other calls of the same time. This impacts the
+          QFLIGHT calls where UART output is an RPC call to the DSPs
+         */
+        _run_uarts();
+        RCInput::from(hal.rcin)->_timer_tick();
+#endif
     }
     return NULL;
 }
@@ -306,11 +330,33 @@ void *Scheduler::_rcin_thread(void *arg)
     }
     while (true) {
         sched->_microsleep(APM_LINUX_RCIN_PERIOD);
+#if !HAL_LINUX_UARTS_ON_TIMER_THREAD
         RCInput::from(hal.rcin)->_timer_tick();
+#endif
     }
     return NULL;
 }
 
+
+/*
+  run timers for all UARTs
+ */
+void Scheduler::_run_uarts(void)
+{
+    // process any pending serial bytes
+    UARTDriver::from(hal.uartA)->_timer_tick();
+    UARTDriver::from(hal.uartB)->_timer_tick();
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
+    //SPI UART not use SPI
+    if (RPIOUARTDriver::from(hal.uartC)->isExternal()) {
+        RPIOUARTDriver::from(hal.uartC)->_timer_tick();
+    }
+#else
+    UARTDriver::from(hal.uartC)->_timer_tick();
+#endif
+    UARTDriver::from(hal.uartE)->_timer_tick();
+}
+
 void *Scheduler::_uart_thread(void* arg)
 {
     Scheduler* sched = (Scheduler *)arg;
@@ -320,19 +366,9 @@ void *Scheduler::_uart_thread(void* arg)
     }
     while (true) {
         sched->_microsleep(APM_LINUX_UART_PERIOD);
-
-        // process any pending serial bytes
-        UARTDriver::from(hal.uartA)->_timer_tick();
-        UARTDriver::from(hal.uartB)->_timer_tick();
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
-        //SPI UART not use SPI
-        if (RPIOUARTDriver::from(hal.uartC)->isExternal()) {
-            RPIOUARTDriver::from(hal.uartC)->_timer_tick();
-        }
-#else
-        UARTDriver::from(hal.uartC)->_timer_tick();
+#if !HAL_LINUX_UARTS_ON_TIMER_THREAD
+        _run_uarts();
 #endif
-        UARTDriver::from(hal.uartE)->_timer_tick();
     }
     return NULL;
 }

libraries/AP_HAL_Linux/Scheduler.h

@@ -82,6 +82,7 @@ private:
     static void *_io_thread(void* arg);
     static void *_rcin_thread(void* arg);
     static void *_uart_thread(void* arg);
+    static void _run_uarts(void);
     static void *_tonealarm_thread(void* arg);
 
     void _run_timers(bool called_from_timer_thread);

libraries/AP_HAL_Linux/UARTDriver.cpp

@@ -28,6 +28,7 @@
 #include "UDPDevice.h"
 #include "ConsoleDevice.h"
 #include "TCPServerDevice.h"
+#include "UARTQFlight.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -87,6 +88,15 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
             break;
         }
 
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+        case DEVICE_QFLIGHT:
+        {
+            _qflight_start_connection();
+            _flow_control = FLOW_CONTROL_DISABLE;
+            break;
+        }
+#endif
+        
         case DEVICE_SERIAL:
         {
             if (!_serial_start_connection()) {
@@ -192,6 +202,10 @@ UARTDriver::device_type UARTDriver::_parseDevicePath(const char *arg)
 
     if (stat(arg, &st) == 0 && S_ISCHR(st.st_mode)) {
         return DEVICE_SERIAL;
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+    } else if (strncmp(arg, "qflight:", 8) == 0) {
+        return DEVICE_QFLIGHT;
+#endif
     } else if (strncmp(arg, "tcp:", 4) != 0 && 
                strncmp(arg, "udp:", 4) != 0) {
         return DEVICE_UNKNOWN;
@@ -259,6 +273,17 @@ bool UARTDriver::_serial_start_connection()
     return true;
 }
 
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+bool UARTDriver::_qflight_start_connection()
+{
+    _device = new QFLIGHTDevice(device_path);
+    _connected = _device->open();
+    _flow_control = FLOW_CONTROL_DISABLE;
+
+    return true;
+}
+#endif
+
 /*
   start a UDP connection for the serial port
  */

libraries/AP_HAL_Linux/UARTDriver.h

@@ -56,11 +56,15 @@ private:
     void _udp_start_connection(void);
     void _tcp_start_connection(void);
     bool _serial_start_connection(void);
+    bool _qflight_start_connection(void);
 
     enum device_type {
         DEVICE_TCP,
         DEVICE_UDP,
         DEVICE_SERIAL,
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+        DEVICE_QFLIGHT,
+#endif
         DEVICE_UNKNOWN
     };
 

libraries/AP_HAL_Linux/UARTQFlight.cpp

@@ -0,0 +1,104 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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/>.
+ */
+/*
+  This is a UART driver for the QFLIGHT port. Actual UART output
+  happens via RPC calls. See the qflight/ subdirectory for details
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+
+#include <stdio.h>
+#include <unistd.h>
+
+#include "UARTQFlight.h"
+
+#include <AP_HAL_Linux/qflight/qflight_util.h>
+#include <AP_HAL_Linux/qflight/qflight_dsp.h>
+#include <stdio.h>
+
+QFLIGHTDevice::QFLIGHTDevice(const char *_device_path)
+{
+    device_path = _device_path;
+    if (strncmp(device_path, "qflight:", 8) == 0) {
+        device_path += 8;
+    }
+}
+
+QFLIGHTDevice::~QFLIGHTDevice()
+{
+    close();
+}
+
+bool QFLIGHTDevice::close()
+{
+    if (fd != -1) {
+        if (qflight_UART_close(fd) != 0) {
+            return false;
+        }
+    }
+
+    fd = -1;
+    return true;
+}
+
+bool QFLIGHTDevice::open()
+{
+    int ret = qflight_UART_open(device_path, &fd);
+
+    if (ret != 0 || fd == -1) {
+        printf("Failed to open UART device %s ret=%d fd=%d\n",
+               device_path, ret, (int)fd);
+        return false;
+    }
+    printf("opened QFLIGHT UART device %s ret=%d fd=%d\n",
+           device_path, ret, (int)fd);
+    return true;
+}
+
+ssize_t QFLIGHTDevice::read(uint8_t *buf, uint16_t n)
+{
+    int32_t nread = 0;
+    int ret = qflight_UART_read(fd, buf, n, &nread);
+    if (ret != 0) {
+        return 0;
+    }
+    return nread;
+}
+
+ssize_t QFLIGHTDevice::write(const uint8_t *buf, uint16_t n)
+{
+    int32_t nwritten = 0;
+    int ret = qflight_UART_write(fd, buf, n, &nwritten);
+    if (ret != 0) {
+        return 0;
+    }
+    return nwritten;
+}
+
+void QFLIGHTDevice::set_blocking(bool blocking)
+{
+    // no implementation yet
+}
+
+void QFLIGHTDevice::set_speed(uint32_t baudrate)
+{
+    qflight_UART_set_baudrate(fd, baudrate);
+}
+
+#endif // CONFIG_HAL_BOARD_SUBTYPE
+

libraries/AP_HAL_Linux/UARTQFlight.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include "SerialDevice.h"
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
+
+class QFLIGHTDevice: public SerialDevice {
+public:
+    QFLIGHTDevice(const char *device_path);
+    virtual ~QFLIGHTDevice();
+
+    virtual bool open() override;
+    virtual bool close() override;
+    virtual ssize_t write(const uint8_t *buf, uint16_t n) override;
+    virtual ssize_t read(uint8_t *buf, uint16_t n) override;
+    virtual void set_blocking(bool blocking) override;
+    virtual void set_speed(uint32_t speed) override;
+
+private:
+    int32_t fd = -1;
+    const char *device_path;
+};
+
+#endif

libraries/AP_HAL_Linux/qflight/qflight_buffer.h

@@ -0,0 +1,38 @@
+#pragma once
+
+/*
+  shared memory structures for sensor data and peripheral control on Qualcomm flight board
+ */
+struct DSPBuffer {
+    // IMU data
+    struct IMU {
+        static const uint32_t max_samples = 10;
+        uint32_t num_samples;
+        struct BUF {
+            uint64_t timestamp;
+            float accel[3];
+            float gyro[3];
+        } buf[max_samples];
+    } imu;
+
+    // MAG data
+    struct MAG {
+        static const uint64_t max_samples = 10;
+        uint32_t num_samples;
+        struct BUF {
+            uint64_t timestamp;
+            int16_t mag_raw[3];
+        } buf[max_samples];
+    } mag;
+
+    // baro data
+    struct BARO {
+        static const uint32_t max_samples = 10;
+        uint32_t num_samples;
+        struct BUF {
+            uint64_t timestamp;
+            float pressure_pa;
+            float temperature_C;
+        } buf[max_samples];
+    } baro;
+};

libraries/AP_HAL_Linux/qflight/qflight_dsp.idl

@@ -0,0 +1,15 @@
+#include "AEEStdDef.idl"
+
+interface qflight {
+    // sensor calls
+    long get_imu_data(rout sequence<uint8> outdata);
+    long get_mag_data(rout sequence<uint8> outdata);
+    long get_baro_data(rout sequence<uint8> outdata);
+
+    // UART control
+    long UART_open(in string device, rout int32 fd);
+    long UART_set_baudrate(in int32 fd, in uint32 baudrate);
+    long UART_read(in int32 fd, rout sequence<uint8> buf, rout int32 nread);
+    long UART_write(in int32 fd, in sequence<uint8> buf, rout int32 nwritten);
+    long UART_close(in int32 fd);
+};

libraries/AP_HAL_Linux/qflight/qflight_util.h

@@ -0,0 +1,3 @@
+#include <rpcmem.h>
+
+#define QFLIGHT_RPC_ALLOCATE(type) (type *)rpcmem_alloc_def(sizeof(type))