AP_HAL_ESP32: check UART thread ownership

Signed-off-by: Rhys Mainwaring <[email protected]>
ArduPilot · Jan 4, 2025 · 19978df · 19978df
1 parent 5f8a655
commit 19978df
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 4 deletions.
diff --git a/libraries/AP_HAL_ESP32/UARTDriver.cpp b/libraries/AP_HAL_ESP32/UARTDriver.cpp
@@ -38,6 +38,12 @@ void UARTDriver::vprintf(const char *fmt, va_list ap)
 
 void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
 {
+    if (b == 0 && txS == 0 && rxS == 0 && _initialized) {
+        // the thread owning this port has changed
+        _uart_owner_thd = xTaskGetCurrentTaskHandle();
+        return;
+    }
+
     if (uart_num < ARRAY_SIZE(uart_desc)) {
         uart_port_t p = uart_desc[uart_num].port;
         if (!_initialized) {
@@ -58,6 +64,7 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
             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);
+            _uart_owner_thd = xTaskGetCurrentTaskHandle();
 
             _initialized = true;
         } else {
@@ -98,7 +105,7 @@ bool UARTDriver::tx_pending()
 
 uint32_t UARTDriver::_available()
 {
-    if (!_initialized) {
+    if (!_initialized || _uart_owner_thd != xTaskGetCurrentTaskHandle()) {
         return 0;
     }
     return _readbuf.available();
@@ -117,6 +124,10 @@ uint32_t UARTDriver::txspace()
 
 ssize_t IRAM_ATTR UARTDriver::_read(uint8_t *buffer, uint16_t count)
 {
+    if (_uart_owner_thd != xTaskGetCurrentTaskHandle()) {
+        return -1;
+    }
+
     if (!_initialized) {
         return -1;
     }
@@ -184,9 +195,16 @@ 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;
-    //return uart_flush_input(p) == ESP_OK;
-    return false;
+    if (_uart_owner_thd != xTaskGetCurrentTaskHandle()) {
+        return false;
+    }
+    if (!_initialized) {
+        return false;
+    }
+
+    _readbuf.clear();
+
+    return true;
 }
 
 // record timestamp of new incoming data

diff --git a/libraries/AP_HAL_ESP32/UARTDriver.h b/libraries/AP_HAL_ESP32/UARTDriver.h
@@ -23,6 +23,9 @@
 #include "driver/gpio.h"
 #include "driver/uart.h"
 
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
 namespace ESP32
 {
 
@@ -92,6 +95,8 @@ class UARTDriver : public AP_HAL::UARTDriver
     uint8_t _receive_timestamp_idx;
     uint32_t _baudrate;
 
+    const tskTaskControlBlock* _uart_owner_thd;
+
     void _receive_timestamp_update(void);
 
 protected: