diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp index c807cb497b..26fb08a5c3 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.cpp +++ b/libraries/AP_HAL_Linux/UARTDriver.cpp @@ -87,6 +87,8 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) _device->set_speed(b); + _baudrate = b; + _allocate_buffers(rxS, txS); } @@ -471,6 +473,10 @@ void UARTDriver::_timer_tick(void) } _readbuf.commit((unsigned)ret); + // update receive timestamp + _receive_timestamp[_receive_timestamp_idx^1] = AP_HAL::micros64(); + _receive_timestamp_idx ^= 1; + /* stop reading as we read less than we asked for */ if ((unsigned)ret < vec[i].len) { break; @@ -479,3 +485,27 @@ void UARTDriver::_timer_tick(void) _in_timer = false; } + +/* + 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) const +{ + uint64_t last_receive_us = _receive_timestamp[_receive_timestamp_idx]; + if (_baudrate > 0) { + // assume 10 bits per byte. + uint32_t transport_time_us = (1000000UL * 10UL / _baudrate) * nbytes; + last_receive_us -= transport_time_us; + } + return last_receive_us; +} diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h index 66c0072183..709ef3162a 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.h +++ b/libraries/AP_HAL_Linux/UARTDriver.h @@ -49,12 +49,28 @@ public: _device->set_flow_control(flow_control_setting); } + /* + 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) const override; + private: AP_HAL::OwnPtr _device; bool _nonblocking_writes; bool _console; volatile bool _in_timer; uint16_t _base_port; + uint32_t _baudrate; char *_ip; char *_flag; bool _connected; // true if a client has connected @@ -66,6 +82,10 @@ private: AP_HAL::OwnPtr _parseDevicePath(const char *arg); uint64_t _last_write_time; + // timestamp for receiving data on the UART, avoiding a lock + uint64_t _receive_timestamp[2]; + uint8_t _receive_timestamp_idx; + protected: const char *device_path; volatile bool _initialised;