AP_HAL_ChibiOS: add support for inducing clock drift

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

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