AP_HAL_ESP32: add control options to UART driver

libraries/AP_HAL_ESP32/UARTDriver.cpp

@@ -13,66 +13,100 @@
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <AP_HAL_ESP32/UARTDriver.h>
+#include <AP_HAL/AP_HAL.h>
+
 #include <AP_Math/AP_Math.h>
+#include "Semaphores.h"
+#include "Scheduler.h"
+#include "UARTDriver.h"
 
 #include "esp_log.h"
 
 extern const AP_HAL::HAL& hal;
 
-namespace ESP32
-{
+#include "rom/ets_sys.h" //for ets_printf
+extern int ets_printf(const char* format, ...); //for ets_printf in rom
+
+using namespace ESP32;
+
 
-UARTDesc uart_desc[] = {HAL_ESP32_UART_DEVICES};
+const UARTDriver::SerialDef UARTDriver::_serial_tab[] = {HAL_ESP32_UART_DEVICES};
+
+// table to find UARTDrivers from serial number, used for event handling
+UARTDriver *UARTDriver::serial_drivers[UART_MAX_DRIVERS];
+
+UARTDriver::UARTDriver(uint8_t serial_num) :
+    AP_HAL::UARTDriver(),
+    sdef(_serial_tab[_serial_num]),
+    _serial_num(serial_num)
+{
+    _initialized = false;
+    if(_serial_num > UART_MAX_DRIVERS) {
+        // ets is very low level and can't print floats etc
+        ets_printf("too many UART drivers");
+    }
+    serial_drivers[_serial_num] = this;
+    //no printf allowed in static constructor, not allowed
+}
 
 void UARTDriver::vprintf(const char *fmt, va_list ap)
 {
+    // the idea is that no other thread can printf to the console etc till the current one finishes its line/action/etc.
+    WITH_SEMAPHORE(_sem);
 
-    uart_port_t p = uart_desc[uart_num].port;
+    uart_port_t p = sdef.port;
     if (p == 0) {
         esp_log_writev(ESP_LOG_INFO, "", fmt, ap);
     } else {
         AP_HAL::UARTDriver::vprintf(fmt, ap);
     }
+    //hal.scheduler->delay_microseconds(10000);// time for hw to flush while holding sem ? todo get rid of this?
+}
+
+// disable TX/RX pins for unusued uart
+void UARTDriver::disable_rxtx(void) const
+{
+    // nop on esp32. todo?
 }
 
 void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
 {
-    if (uart_num < ARRAY_SIZE(uart_desc)) {
-        uart_port_t p = uart_desc[uart_num].port;
-        if (!_initialized) {
-
-            uart_config_t config = {
-                .baud_rate = (int)b,
-                .data_bits = UART_DATA_8_BITS,
-                .parity = UART_PARITY_DISABLE,
-                .stop_bits = UART_STOP_BITS_1,
-                .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
-            };
-            uart_param_config(p, &config);
-            uart_set_pin(p,
-                         uart_desc[uart_num].tx,
-                         uart_desc[uart_num].rx,
-                         UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
-            //uart_driver_install(p, 2*UART_FIFO_LEN, 0, 0, nullptr, 0);
-            uart_driver_install(p, 2*UART_HW_FIFO_LEN(p), 0, 0, nullptr, 0);
-            _readbuf.set_size(RX_BUF_SIZE);
-            _writebuf.set_size(TX_BUF_SIZE);
-
-            _initialized = true;
-        } else {
-            flush();
-            uart_set_baudrate(p, b);
+    // hal.console->printf("%s:%d UART num:%d\n", __PRETTY_FUNCTION__, __LINE__, sdef.port);
+
+    uart_port_t p = sdef.port;
+    if (!_initialized) {
+
+        uart_config_t config = {
+            .baud_rate = (int)b,
+            .data_bits = UART_DATA_8_BITS,
+            .parity = UART_PARITY_DISABLE,
+            .stop_bits = UART_STOP_BITS_1,
+            .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
+        };
+        uart_param_config(p, &config);
+        uart_set_pin(p,
+                      sdef.tx,
+                      sdef.rx,
+                      UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
+        //uart_driver_install(p, 2*UART_FIFO_LEN, 0, 0, nullptr, 0);
+        uart_driver_install(p, 2*UART_HW_FIFO_LEN(p), 0, 0, nullptr, 0);
+        _readbuf.set_size(RX_BUF_SIZE);
+        _writebuf.set_size(TX_BUF_SIZE);
+
+        _initialized = true;
+    } else {
+        flush();
+        uart_set_baudrate(p, b);
 
-        }
     }
+
     _baudrate = b;
 }
 
 void UARTDriver::_end()
 {
     if (_initialized) {
-        uart_driver_delete(uart_desc[uart_num].port);
+        uart_driver_delete(sdef.port);
         _readbuf.set_size(0);
         _writebuf.set_size(0);
     }
@@ -81,7 +115,7 @@ void UARTDriver::_end()
 
 void UARTDriver::_flush()
 {
-    uart_port_t p = uart_desc[uart_num].port;
+    uart_port_t p = sdef.port;
     uart_flush(p);
 }
 
@@ -115,6 +149,18 @@ uint32_t UARTDriver::txspace()
 
 }
 
+// set optional features, return true on success
+bool UARTDriver::set_options(uint16_t options)
+{
+    return false;
+}
+
+// get optional features
+uint16_t UARTDriver::get_options(void) const
+{
+    return _last_options;
+}
+
 ssize_t IRAM_ATTR UARTDriver::_read(uint8_t *buffer, uint16_t count)
 {
     if (!_initialized) {
@@ -143,7 +189,7 @@ void IRAM_ATTR UARTDriver::_timer_tick(void)
 
 void IRAM_ATTR UARTDriver::read_data()
 {
-    uart_port_t p = uart_desc[uart_num].port;
+    uart_port_t p = sdef.port;
     int count = 0;
     do {
         count = uart_read_bytes(p, _buffer, sizeof(_buffer), 0);
@@ -155,7 +201,7 @@ void IRAM_ATTR UARTDriver::read_data()
 
 void IRAM_ATTR UARTDriver::write_data()
 {
-    uart_port_t p = uart_desc[uart_num].port;
+    uart_port_t p = sdef.port;
     int count = 0;
     _write_mutex.take_blocking();
     do {
@@ -184,7 +230,7 @@ size_t IRAM_ATTR UARTDriver::_write(const uint8_t *buffer, size_t size)
 
 bool UARTDriver::_discard_input()
 {
-    //uart_port_t p = uart_desc[uart_num].port;
+    //uart_port_t p = sdef.port;
     //return uart_flush_input(p) == ESP_OK;
     return false;
 }
@@ -219,4 +265,3 @@ uint64_t UARTDriver::receive_time_constraint_us(uint16_t nbytes)
     return last_receive_us;
 }
 
-}

libraries/AP_HAL_ESP32/UARTDriver.h

@@ -15,49 +15,87 @@
 
 #pragma once
 
+#include "AP_HAL_ESP32.h"
+
 #include <AP_HAL/UARTDriver.h>
 #include <AP_HAL/utility/RingBuffer.h>
-#include <AP_HAL_ESP32/AP_HAL_ESP32.h>
-#include <AP_HAL_ESP32/Semaphores.h>
+#include "Semaphores.h" 
 
 #include "driver/gpio.h"
 #include "driver/uart.h"
 
-namespace ESP32
-{
-
-struct UARTDesc {
-    uart_port_t port;
-    gpio_num_t rx;
-    gpio_num_t tx;
-};
+#define UART_MAX_DRIVERS 11
 
-class UARTDriver : public AP_HAL::UARTDriver
+class ESP32::UARTDriver : public AP_HAL::UARTDriver
 {
 public:
+    UARTDriver(uint8_t serial_num);
 
-    UARTDriver(uint8_t serial_num)
-        : AP_HAL::UARTDriver()
-    {
-        _initialized = false;
-        uart_num = serial_num;
-    }
-
-    virtual ~UARTDriver() = default;
-
-    void vprintf(const char *fmt, va_list ap) override;
+    /* Do not allow copies */
+    CLASS_NO_COPY(UARTDriver);
 
     bool is_initialized() override;
     bool tx_pending() override;
 
+    // disable TX/RX pins for unusued uart?
+    void disable_rxtx(void) const override;
+
     uint32_t txspace() override;
 
+    // control optional features
+    bool set_options(uint16_t options) override;
+    uint16_t get_options(void) const override;
+
     void _timer_tick(void) override;
 
-    uint32_t bw_in_bytes_per_second() const override
-    {
-        return 10*1024;
-    }
+    // unused stuff from chibios - do we want it in the future?
+    // struct SerialDef {
+    //     void* serial;
+    //     uint8_t instance;
+    //     bool is_usb;
+    //     uint8_t port;
+    //     uint8_t rx;
+    //     uint8_t tx;
+    //     uint8_t rts_line;
+    //     uint8_t cts_line;
+    //     uint32_t _baudrate;
+    //     uint8_t get_index(void) const {
+    //         return uint8_t(this - &_serial_tab[0]);
+    //     }
+    // };
+    struct SerialDef {
+        uart_port_t port;
+        gpio_num_t rx;
+        gpio_num_t tx;
+        // available in chibios but not currently required for esp32
+        // uint8_t get_index(void) const {
+        //     return uint8_t(this - &_serial_tab[0]);
+        // }
+    };
+
+    //! @todo enable wait_timeout override
+#if 0
+    bool wait_timeout(uint16_t n, uint32_t timeout_ms) override;
+#endif
+
+    //! @todo enable flow control
+#if 0
+    void set_flow_control(enum flow_control flow_control) override;
+    enum flow_control get_flow_control(void) override { return _flow_control; }
+#endif
+
+    //! @todo enable parity and stop bits
+#if 0
+    void configure_parity(uint8_t v) override;
+    void set_stop_bits(int n) override;
+
+    /*
+      software control of the CTS/RTS pins if available. Return false if
+      not available
+     */
+    bool set_RTS_pin(bool high) override;
+    bool set_CTS_pin(bool high) override;
+#endif
 
     /*
       return timestamp estimate in microseconds for when the start of
@@ -72,9 +110,18 @@ class UARTDriver : public AP_HAL::UARTDriver
 
     uint64_t receive_time_constraint_us(uint16_t nbytes) override; 
 
+    uint32_t bw_in_bytes_per_second() const override
+    {
+        return 10*1024;
+    }
+
     uint32_t get_baud_rate() const override { return _baudrate; }
 
+    void vprintf(const char *fmt, va_list ap) override;
+
 private:
+    const SerialDef &sdef;
+
     bool _initialized;
     const size_t TX_BUF_SIZE = 1024;
     const size_t RX_BUF_SIZE = 1024;
@@ -85,12 +132,53 @@ class UARTDriver : public AP_HAL::UARTDriver
     void read_data();
     void write_data();
 
-    uint8_t uart_num;
+    HAL_Semaphore _sem;
+
+    // table to find UARTDrivers from serial number, used for event handling
+    static UARTDriver *serial_drivers[UART_MAX_DRIVERS];
+
+    // index into serial_drivers table
+    uint8_t _serial_num;
+
+    uint32_t _baudrate;
+
+    static const SerialDef _serial_tab[];
 
     // timestamp for receiving data on the UART, avoiding a lock
     uint64_t _receive_timestamp[2];
     uint8_t _receive_timestamp_idx;
-    uint32_t _baudrate;
+
+    //! @todo enable flow control
+#if 0
+    // handling of flow control
+    enum flow_control _flow_control = FLOW_CONTROL_DISABLE;
+    bool _rts_is_active;
+    uint32_t _last_write_completed_us;
+    uint32_t _first_write_started_us;
+    uint32_t _total_written;
+
+    // statistics
+    uint32_t _tx_stats_bytes;
+    uint32_t _rx_stats_bytes;
+
+    // we remember config options from set_options to apply on sdStart()
+    uint32_t _cr1_options;
+    uint32_t _cr2_options;
+    uint32_t _cr3_options;
+#endif
+    uint16_t _last_options;
+
+    //! @todo enable half-duplex control
+#if 0
+    // half duplex control. After writing we throw away bytes for 4 byte widths to
+    // prevent reading our own bytes back
+#if CH_CFG_USE_EVENTS == TRUE
+    bool half_duplex;
+    event_listener_t hd_listener;
+    eventflags_t hd_tx_active;
+    void half_duplex_setup_tx(void);
+#endif
+#endif
 
     void _receive_timestamp_update(void);
 
@@ -104,4 +192,3 @@ class UARTDriver : public AP_HAL::UARTDriver
     bool _discard_input() override; // discard all bytes available for reading
 };
 
-}