HAL_ChibiOS: moved UART thread to UART driver

use an event model for triggering sends from tx complete DMA handler
for unbuffered uarts

libraries/AP_HAL_ChibiOS/Scheduler.cpp

@@ -37,13 +37,9 @@ THD_WORKING_AREA(_timer_thread_wa, 2048);
 THD_WORKING_AREA(_rcin_thread_wa, 512);
 THD_WORKING_AREA(_io_thread_wa, 2048);
 THD_WORKING_AREA(_storage_thread_wa, 2048);
-THD_WORKING_AREA(_uart_thread_wa, 2048);
 #if HAL_WITH_UAVCAN
 THD_WORKING_AREA(_uavcan_thread_wa, 4096);
 #endif
-#if HAL_WITH_IO_MCU
-extern ChibiOS::UARTDriver uart_io;
-#endif
 
 Scheduler::Scheduler()
 {}
@@ -72,13 +68,6 @@ void Scheduler::init()
                      _rcin_thread,             /* Thread function.     */
                      this);                     /* Thread parameter.    */
 
-    // the UART thread runs at a medium priority
-    _uart_thread_ctx = chThdCreateStatic(_uart_thread_wa,
-                     sizeof(_uart_thread_wa),
-                     APM_UART_PRIORITY,        /* Initial priority.    */
-                     _uart_thread,             /* Thread function.     */
-                     this);                    /* Thread parameter.    */
-
     // the IO thread runs at lower priority
     _io_thread_ctx = chThdCreateStatic(_io_thread_wa,
                      sizeof(_io_thread_wa),
@@ -326,29 +315,6 @@ void Scheduler::_run_io(void)
     _in_io_proc = false;
 }
 
-void Scheduler::_uart_thread(void* arg)
-{
-    Scheduler *sched = (Scheduler *)arg;
-    sched->_uart_thread_ctx->name = "apm_uart";
-    while (!sched->_hal_initialized) {
-        sched->delay_microseconds(1000);
-    }
-    while (true) {
-        sched->delay_microseconds(1000);
-
-        // process any pending serial bytes
-        hal.uartA->_timer_tick();
-        hal.uartB->_timer_tick();
-        hal.uartC->_timer_tick();
-        hal.uartD->_timer_tick();
-        hal.uartE->_timer_tick();
-        hal.uartF->_timer_tick();
-#if HAL_WITH_IO_MCU
-        uart_io._timer_tick();
-#endif
-    }
-}
-
 void Scheduler::_io_thread(void* arg)
 {
     Scheduler *sched = (Scheduler *)arg;

libraries/AP_HAL_ChibiOS/Scheduler.h

@@ -111,7 +111,6 @@ private:
     thread_t* _rcin_thread_ctx;
     thread_t* _io_thread_ctx;
     thread_t* _storage_thread_ctx;
-    thread_t* _uart_thread_ctx;
 #if HAL_WITH_UAVCAN
     thread_t* _uavcan_thread_ctx;
 #endif

libraries/AP_HAL_ChibiOS/UARTDriver.cpp

@@ -22,6 +22,7 @@
 #include <usbcfg.h>
 #include "shared_dma.h"
 #include <AP_Math/AP_Math.h>
+#include "Scheduler.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -32,23 +33,90 @@ using namespace ChibiOS;
 #define HAVE_USB_SERIAL
 #endif
 
+#if HAL_WITH_IO_MCU
+extern ChibiOS::UARTDriver uart_io;
+#endif
+
 const UARTDriver::SerialDef UARTDriver::_serial_tab[] = { HAL_UART_DEVICE_LIST };
 
-// event used to wake up waiting thread
+// handle for UART handling thread
+thread_t *UARTDriver::uart_thread_ctx;
+
+// table to find UARTDrivers from serial number, used for event handling
+UARTDriver *UARTDriver::uart_drivers[UART_MAX_DRIVERS];
+
+// last time we did a 1kHz run of uarts
+uint32_t UARTDriver::last_thread_run_us;
+
+// event used to wake up waiting thread. This event number is for
+// caller threads
 #define EVT_DATA EVENT_MASK(0)
 
-UARTDriver::UARTDriver(uint8_t serial_num) :
+UARTDriver::UARTDriver(uint8_t _serial_num) :
 tx_bounce_buf_ready(true),
-sdef(_serial_tab[serial_num]),
+serial_num(_serial_num),
+sdef(_serial_tab[_serial_num]),
 _baudrate(57600),
 _in_timer(false),
 _initialised(false)
 {
+    osalDbgAssert(serial_num < UART_MAX_DRIVERS, "too many UART drivers");
+    uart_drivers[serial_num] = this;
     chMtxObjectInit(&_write_mutex);
 }
 
+/*
+  thread for handling UART send/receive
+
+  We use events indexed by serial_num to trigger a more rapid send for
+  unbuffered_write uarts, and run at 1kHz for general UART handling
+ */
+void UARTDriver::uart_thread(void* arg)
+{
+    uart_thread_ctx = chThdGetSelfX();
+    while (true) {
+        eventmask_t mask = chEvtWaitAnyTimeout(~0, MS2ST(1));
+        uint32_t now = AP_HAL::micros();
+        if (now - last_thread_run_us >= 1000) {
+            // run them all if it's been more than 1ms since we ran
+            // them all
+            mask = ~0;
+            last_thread_run_us = now;
+        }
+        for (uint8_t i=0; i<UART_MAX_DRIVERS; i++) {
+            if (uart_drivers[i] == nullptr) {
+                continue;
+            }
+            if ((mask & EVENT_MASK(i)) &&
+                uart_drivers[i]->_initialised) {
+                uart_drivers[i]->_timer_tick();
+            }
+        }
+    }
+}
+
+/*
+  initialise UART thread
+ */
+void UARTDriver::thread_init(void)
+{
+    if (uart_thread_ctx) {
+        // already initialised
+        return;
+    }
+    uart_thread_ctx = chThdCreateFromHeap(NULL,
+                                          THD_WORKING_AREA_SIZE(2048),
+                                          "apm_uart",
+                                          APM_UART_PRIORITY,
+                                          uart_thread,
+                                           this);
+}
+
+
 void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
 {
+    thread_init();
+    
     hal.gpio->pinMode(2, HAL_GPIO_OUTPUT);
     hal.gpio->pinMode(3, HAL_GPIO_OUTPUT);
     if (sdef.serial == nullptr) {
@@ -211,20 +279,22 @@ void UARTDriver::dma_tx_deallocate(void)
     chSysUnlock();
 }
 
+/*
+  DMA transmit complettion interrupt handler
+ */
 void UARTDriver::tx_complete(void* self, uint32_t flags)
 {
     UARTDriver* uart_drv = (UARTDriver*)self;
     if (!uart_drv->tx_bounce_buf_ready) {
         uart_drv->_last_write_completed_us = AP_HAL::micros();
         uart_drv->tx_bounce_buf_ready = true;
-        if (uart_drv->unbuffered_writes) {
+        if (uart_drv->unbuffered_writes && uart_drv->_writebuf.available()) {
-            //kickstart the next write
+            // trigger a rapid send of next bytes
-            uart_drv->write_pending_bytes_DMA_from_irq();
+            chSysLockFromISR();
-        }
+            chEvtSignalI(uart_thread_ctx, EVENT_MASK(uart_drv->serial_num));
-        if (uart_drv->tx_bounce_buf_ready) {
+            chSysUnlockFromISR();
-            // we didn't launch a new DMA
-            uart_drv->dma_handle->unlock_from_IRQ();
         }
+        uart_drv->dma_handle->unlock_from_IRQ();
     }
 }
 
@@ -475,34 +545,6 @@ void UARTDriver::write_pending_bytes_DMA(uint32_t n)
     dmaStreamEnable(txdma);
 }
 
-/*
-  write out pending bytes with DMA from IRQ
- */
-void UARTDriver::write_pending_bytes_DMA_from_irq()
-{
-    if (!tx_bounce_buf_ready) {
-        return;
-    }
-
-    /* TX DMA channel preparation.*/
-    _writebuf.advance(tx_len);
-    tx_len = _writebuf.peekbytes(tx_bounce_buf, TX_BOUNCE_BUFSIZE);
-    if (tx_len == 0) {
-        return;
-    }
-    tx_bounce_buf_ready = false;
-    osalDbgAssert(txdma != nullptr, "UART TX DMA allocation failed");
-    dmaStreamSetMemory0(txdma, tx_bounce_buf);
-    dmaStreamSetTransactionSize(txdma, tx_len);
-    uint32_t dmamode = STM32_DMA_CR_DMEIE | STM32_DMA_CR_TEIE;
-    dmamode |= STM32_DMA_CR_CHSEL(STM32_DMA_GETCHANNEL(sdef.dma_tx_stream_id,
-                                                       sdef.dma_tx_channel_id));
-    dmamode |= STM32_DMA_CR_PL(0);
-    dmaStreamSetMode(txdma, dmamode | STM32_DMA_CR_DIR_M2P |
-                     STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
-    dmaStreamEnable(txdma);
-}
-
 /*
   write any pending bytes to the device, non-DMA method
  */

libraries/AP_HAL_ChibiOS/UARTDriver.h

@@ -24,6 +24,8 @@
 #define RX_BOUNCE_BUFSIZE 128
 #define TX_BOUNCE_BUFSIZE 64
 
+#define UART_MAX_DRIVERS 7
+
 class ChibiOS::UARTDriver : public AP_HAL::UARTDriver {
 public:
     UARTDriver(uint8_t serial_num);
@@ -73,6 +75,18 @@ public:
 private:
     bool tx_bounce_buf_ready;
     const SerialDef &sdef;
+
+    // thread used for all UARTs
+    static thread_t *uart_thread_ctx;
+
+    // last time we ran the uart thread
+    static uint32_t last_thread_run_us;
+    
+    // table to find UARTDrivers from serial number, used for event handling
+    static UARTDriver *uart_drivers[UART_MAX_DRIVERS];
+
+    // index into uart_drivers table
+    uint8_t serial_num;
     
     uint32_t _baudrate;
     uint16_t tx_len;
@@ -121,6 +135,8 @@ private:
 
     void write_pending_bytes_DMA(uint32_t n);
     void write_pending_bytes_NODMA(uint32_t n);
-    void write_pending_bytes_DMA_from_irq(void);
     void write_pending_bytes(void);
+
+    void thread_init();
+    static void uart_thread(void *);
 };