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();