From 5714ee113f88546aa7d952f8d0579e7d43dd1d32 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 2 Oct 2021 15:39:30 +1000
Subject: [PATCH] HAL_ChibiOS: added micros16() method

---
 libraries/AP_HAL_ChibiOS/system.cpp | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/libraries/AP_HAL_ChibiOS/system.cpp b/libraries/AP_HAL_ChibiOS/system.cpp
index cd23f8bb20..422bc4c3db 100644
--- a/libraries/AP_HAL_ChibiOS/system.cpp
+++ b/libraries/AP_HAL_ChibiOS/system.cpp
@@ -267,6 +267,17 @@ uint32_t micros()
 #endif
 }
 
+uint16_t micros16()
+{
+#if CH_CFG_ST_RESOLUTION == 32 && CH_CFG_ST_FREQUENCY==1000000U
+    return st_lld_get_counter() & 0xFFFF;
+#elif CH_CFG_ST_RESOLUTION == 16 && CH_CFG_ST_FREQUENCY==1000000U
+    return st_lld_get_counter();
+#else
+    return hrt_micros32() & 0xFFFF;
+#endif
+}
+    
 uint32_t millis()
 {
     return hrt_millis32();