From 8b029777e2cb6824950701919c34ec818462112a Mon Sep 17 00:00:00 2001
From: Daniel Tombeil <dt@admindu.de>
Date: Tue, 2 May 2023 09:26:57 +1000
Subject: [PATCH] AP_HAL_ESP32: Adding receive_time_constraint_us() to
 UARTDriver to address GPS init

  and health issues

- more work on receive_time_constraint_us()
---
 libraries/AP_HAL_ESP32/UARTDriver.cpp | 39 +++++++++++++++++++++++++++
 libraries/AP_HAL_ESP32/UARTDriver.h   | 20 ++++++++++++++
 2 files changed, 59 insertions(+)

diff --git a/libraries/AP_HAL_ESP32/UARTDriver.cpp b/libraries/AP_HAL_ESP32/UARTDriver.cpp
index 1017476323..fb051800a6 100644
--- a/libraries/AP_HAL_ESP32/UARTDriver.cpp
+++ b/libraries/AP_HAL_ESP32/UARTDriver.cpp
@@ -72,6 +72,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
 
         }
     }
+    _baudrate = b;
 }
 
 void UARTDriver::end()
@@ -136,18 +137,26 @@ ssize_t IRAM_ATTR UARTDriver::read(uint8_t *buffer, uint16_t count)
         return 0;
     }
 
+
+    _receive_timestamp_update();
+
     return ret;
 }
 
 bool IRAM_ATTR UARTDriver::read(uint8_t &byte)
 {
+
     if (!_initialized) {
         return false;
     }
     if (!_readbuf.read_byte(&byte)) {
         return false;
     }
+
+    _receive_timestamp_update();
+
     return true;
+
 }
 
 void IRAM_ATTR UARTDriver::_timer_tick(void)
@@ -212,4 +221,34 @@ bool UARTDriver::discard_input()
     return false;
 }
 
+// record timestamp of new incoming data
+void IRAM_ATTR UARTDriver::_receive_timestamp_update(void)
+{
+    _receive_timestamp[_receive_timestamp_idx^1] = AP_HAL::micros64();
+    _receive_timestamp_idx ^= 1;
+}
+
+
+/*
+  return timestamp estimate in microseconds for when the start of
+  a nbytes packet arrived on the uart. This should be treated as a
+  time constraint, not an exact time. It is guaranteed that the
+  packet did not start being received after this time, but it
+  could have been in a system buffer before the returned time.
+  This takes account of the baudrate of the link. For transports
+  that have no baudrate (such as USB) the time estimate may be
+  less accurate.
+  A return value of zero means the HAL does not support this API
+*/
+uint64_t UARTDriver::receive_time_constraint_us(uint16_t nbytes)
+{
+    uint64_t last_receive_us = _receive_timestamp[_receive_timestamp_idx];
+    if (_baudrate > 0) {
+        // assume 10 bits per byte. For USB we assume zero transport delay
+        uint32_t transport_time_us = (1000000UL * 10UL / _baudrate) * (nbytes + available());
+        last_receive_us -= transport_time_us;
+    }
+    return last_receive_us;
+}
+
 }
diff --git a/libraries/AP_HAL_ESP32/UARTDriver.h b/libraries/AP_HAL_ESP32/UARTDriver.h
index b38dfb2172..22b0174ada 100644
--- a/libraries/AP_HAL_ESP32/UARTDriver.h
+++ b/libraries/AP_HAL_ESP32/UARTDriver.h
@@ -79,6 +79,19 @@ public:
     //bool lock_port(uint32_t write_key, uint32_t read_key) override;
 
     //size_t write_locked(const uint8_t *buffer, size_t size, uint32_t key) override;
+    //
+    /*
+      return timestamp estimate in microseconds for when the start of
+      a nbytes packet arrived on the uart. This should be treated as a
+      time constraint, not an exact time. It is guaranteed that the
+      packet did not start being received after this time, but it
+      could have been in a system buffer before the returned time.
+      This takes account of the baudrate of the link. For transports
+      that have no baudrate (such as USB) the time estimate may be
+      less accurate.
+      A return value of zero means the HAL does not support this API */
+     
+    uint64_t receive_time_constraint_us(uint16_t nbytes) override; 
 private:
     bool _initialized;
     const size_t TX_BUF_SIZE = 1024;
@@ -91,6 +104,13 @@ private:
     void write_data();
 
     uint8_t uart_num;
+
+    // timestamp for receiving data on the UART, avoiding a lock
+    uint64_t _receive_timestamp[2];
+    uint8_t _receive_timestamp_idx;
+    uint32_t _baudrate;
+
+    void _receive_timestamp_update(void);
 };
 
 }