From ec40a9641bd6de25dfec52e34c53da9998cc26cb Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 21 Feb 2023 20:35:57 +1100 Subject: [PATCH] AP_HAL_ESP32: add and use a "bool read(c)" method to AP_HAL this is much less likely to not work vs the int16_t equivalent --- libraries/AP_HAL_ESP32/UARTDriver.cpp | 9 ++++----- libraries/AP_HAL_ESP32/UARTDriver.h | 2 +- libraries/AP_HAL_ESP32/WiFiDriver.cpp | 9 ++++----- libraries/AP_HAL_ESP32/WiFiDriver.h | 2 +- libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp | 11 +++++------ libraries/AP_HAL_ESP32/WiFiUdpDriver.h | 2 +- 6 files changed, 16 insertions(+), 19 deletions(-) diff --git a/libraries/AP_HAL_ESP32/UARTDriver.cpp b/libraries/AP_HAL_ESP32/UARTDriver.cpp index 31794ee2e3..1017476323 100644 --- a/libraries/AP_HAL_ESP32/UARTDriver.cpp +++ b/libraries/AP_HAL_ESP32/UARTDriver.cpp @@ -139,16 +139,15 @@ ssize_t IRAM_ATTR UARTDriver::read(uint8_t *buffer, uint16_t count) return ret; } -int16_t IRAM_ATTR UARTDriver::read() +bool IRAM_ATTR UARTDriver::read(uint8_t &byte) { if (!_initialized) { - return -1; + return false; } - uint8_t byte; if (!_readbuf.read_byte(&byte)) { - return -1; + return false; } - return byte; + return true; } void IRAM_ATTR UARTDriver::_timer_tick(void) diff --git a/libraries/AP_HAL_ESP32/UARTDriver.h b/libraries/AP_HAL_ESP32/UARTDriver.h index a52d8408b2..b38dfb2172 100644 --- a/libraries/AP_HAL_ESP32/UARTDriver.h +++ b/libraries/AP_HAL_ESP32/UARTDriver.h @@ -61,7 +61,7 @@ public: uint32_t txspace() override; ssize_t read(uint8_t *buffer, uint16_t count) override; - int16_t read() override; + bool read(uint8_t &b) override WARN_IF_UNUSED; //ssize_t read(uint8_t *buffer, uint16_t count) override; //int16_t read_locked(uint32_t key) override; diff --git a/libraries/AP_HAL_ESP32/WiFiDriver.cpp b/libraries/AP_HAL_ESP32/WiFiDriver.cpp index 6e5a77f948..cc4e3f4a02 100644 --- a/libraries/AP_HAL_ESP32/WiFiDriver.cpp +++ b/libraries/AP_HAL_ESP32/WiFiDriver.cpp @@ -103,16 +103,15 @@ uint32_t WiFiDriver::txspace() return MAX(result, 0); } -int16_t WiFiDriver::read() +bool WiFiDriver::read(uint8_t &byte) { if (_state != CONNECTED) { - return -1; + return false; } - uint8_t byte; if (!_readbuf.read_byte(&byte)) { - return -1; + return false; } - return byte; + return true; } bool WiFiDriver::start_listen() diff --git a/libraries/AP_HAL_ESP32/WiFiDriver.h b/libraries/AP_HAL_ESP32/WiFiDriver.h index 22bbb5b3fc..c3981f5dca 100644 --- a/libraries/AP_HAL_ESP32/WiFiDriver.h +++ b/libraries/AP_HAL_ESP32/WiFiDriver.h @@ -39,7 +39,7 @@ public: uint32_t available() override; uint32_t txspace() override; - int16_t read() override; + bool read(uint8_t &c) override; size_t write(uint8_t c) override; size_t write(const uint8_t *buffer, size_t size) override; diff --git a/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp b/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp index 9d6c66833e..f5d986fea4 100644 --- a/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp +++ b/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp @@ -101,19 +101,18 @@ uint32_t WiFiUdpDriver::txspace() return MAX(result, 0); } -int16_t WiFiUdpDriver::read() +bool WiFiUdpDriver::read(uint8_t &c) { if (!_read_mutex.take_nonblocking()) { - return 0; + return false; } - uint8_t byte; - if (!_readbuf.read_byte(&byte)) { - return -1; + if (!_readbuf.read_byte(&c)) { + return false; } _read_mutex.give(); - return byte; + return true; } bool WiFiUdpDriver::start_listen() diff --git a/libraries/AP_HAL_ESP32/WiFiUdpDriver.h b/libraries/AP_HAL_ESP32/WiFiUdpDriver.h index 4c41d5409e..b5558d8220 100644 --- a/libraries/AP_HAL_ESP32/WiFiUdpDriver.h +++ b/libraries/AP_HAL_ESP32/WiFiUdpDriver.h @@ -38,7 +38,7 @@ public: uint32_t available() override; uint32_t txspace() override; - int16_t read() override; + bool read(uint8_t &c) override; size_t write(uint8_t c) override; size_t write(const uint8_t *buffer, size_t size) override;