From a31ea572ec216ed19286f791a0567f067afb089c Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Wed, 31 Jan 2024 07:53:51 +1100
Subject: [PATCH] AP_HAL_ChibiOS: add support for inducing clock drift

---
 libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c | 10 ++++++++++
 libraries/AP_HAL_ChibiOS/system.cpp         | 13 ++++++++++++-
 2 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
index f1e510ccebd07e..e2881f68727e17 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
@@ -73,6 +73,12 @@ static uint32_t system_time_u32_us(void)
 static uint32_t get_systime_us32(void)
 {
     uint32_t now = system_time_u32_us();
+#ifdef AP_INDUCE_PPM_OFFSET
+    // Please note that this will induce incorrect
+    // clock drift after 71 minutes, use hrt_micros64()
+    // as "now" here if you want to avoid that
+    now += (((uint64_t)now) * AP_INDUCE_PPM_OFFSET) / 1000000U;
+#endif
 #ifdef AP_BOARD_START_TIME
     now += AP_BOARD_START_TIME;
 #endif
@@ -90,6 +96,10 @@ static uint64_t hrt_micros64I(void)
 #if CH_CFG_ST_FREQUENCY != 1000000U
     ret *= 1000000U/CH_CFG_ST_FREQUENCY;
 #endif
+#ifdef AP_INDUCE_PPM_OFFSET
+    // induce clock drift
+    ret += (ret * AP_INDUCE_PPM_OFFSET) / 1000000U;
+#endif
 #ifdef AP_BOARD_START_TIME
     ret += AP_BOARD_START_TIME;
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/system.cpp b/libraries/AP_HAL_ChibiOS/system.cpp
index 09cb6668215000..b63d833eba5b8d 100644
--- a/libraries/AP_HAL_ChibiOS/system.cpp
+++ b/libraries/AP_HAL_ChibiOS/system.cpp
@@ -353,7 +353,18 @@ __FASTRAMFUNC__ uint32_t micros()
 {
 #if CH_CFG_ST_RESOLUTION == 32 && CH_CFG_ST_FREQUENCY==1000000U
     // special case optimisation for 32 bit timers
-#ifdef AP_BOARD_START_TIME
+#if defined(AP_INDUCE_PPM_OFFSET)
+    uint64_t now = st_lld_get_counter();
+#if defined(AP_BOARD_START_TIME)
+    now += AP_BOARD_START_TIME;
+#else
+    // Please note that this will induce incorrect
+    // clock drift after 71 minutes, use hrt_micros64()
+    // as "now" here if you want to avoid that
+    now += (now * AP_INDUCE_PPM_OFFSET) / 1000000U;
+    return now & 0xFFFFFFFF;
+#endif
+#elif defined(AP_BOARD_START_TIME)
     return st_lld_get_counter() + AP_BOARD_START_TIME;
 #else
     return st_lld_get_counter();