diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
index 4adcfb3ec6..69710b5eec 100644
--- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
+++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
@@ -970,12 +970,12 @@ void UARTDriver::receive_timestamp_update(void)
   
   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 UARTDriver::receive_time_constraint_us(uint16_t nbytes)
 {
     uint64_t last_receive_us = _receive_timestamp[_receive_timestamp_idx];
     if (_baudrate > 0 && !sdef.is_usb) {
         // assume 10 bits per byte. For USB we assume zero transport delay
-        uint32_t transport_time_us = (1000000UL * 10UL / _baudrate) * nbytes;
+        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_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h
index 94d054c719..93cf90ca46 100644
--- a/libraries/AP_HAL_ChibiOS/UARTDriver.h
+++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h
@@ -94,7 +94,7 @@ public:
 
       A return value of zero means the HAL does not support this API
      */
-    uint64_t receive_time_constraint_us(uint16_t nbytes) const override;
+    uint64_t receive_time_constraint_us(uint16_t nbytes) override;
     
 private:
     bool tx_bounce_buf_ready;