From 0cd13bad0e3f81530fc8680527727d9d5a80498b Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Fri, 29 Dec 2023 01:06:22 +1100
Subject: [PATCH] AP_HAL_ChibiOS: add support for setting scale factor on time

---
 libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c | 27 +++++++++++++++++----
 libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h |  2 ++
 libraries/AP_HAL_ChibiOS/system.cpp         | 10 +++-----
 3 files changed, 27 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
index 6c481475dab5de..f94510c4b3c8ec 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
@@ -41,10 +41,29 @@
   for micros64()
 */
 
+inline systime_t get_system_ticks()
+{
+#if defined(AP_MICROS_SCALE_FACTOR_PCT) || defined(AP_BOARD_START_TIME)
+    uint64_t now = st_lld_get_counter();
+#endif
+#ifdef AP_MICROS_SCALE_FACTOR_PCT
+    now = (((uint64_t)now) * (100 + AP_MICROS_SCALE_FACTOR_PCT)) / 100;
+#endif
+#ifdef AP_BOARD_START_TIME
+    now += AP_BOARD_START_TIME;
+#endif
+
+#if defined(AP_MICROS_SCALE_FACTOR_PCT) || defined(AP_BOARD_START_TIME)
+    return now;
+#else
+    return st_lld_get_counter();
+#endif
+}
+
 #if CH_CFG_ST_RESOLUTION == 16
 static uint32_t system_time_u32_us(void)
 {
-    systime_t now = chVTGetSystemTimeX();
+    systime_t now = get_system_ticks();
 #if CH_CFG_ST_FREQUENCY != 1000000U
     #error "must use 32 bit timer if system clock not 1MHz"
 #endif
@@ -58,7 +77,7 @@ static uint32_t system_time_u32_us(void)
 #elif CH_CFG_ST_RESOLUTION == 32
 static uint32_t system_time_u32_us(void)
 {
-    systime_t now = chVTGetSystemTimeX();
+    systime_t now = get_system_ticks();
 #if CH_CFG_ST_FREQUENCY != 1000000U
     now *= 1000000U/CH_CFG_ST_FREQUENCY;
 #endif
@@ -75,9 +94,7 @@ static uint32_t get_systime_us32(void)
 {
     static uint32_t last_us32;
     uint32_t now = system_time_u32_us();
-#ifdef AP_BOARD_START_TIME
-    now += AP_BOARD_START_TIME;
-#endif
+
     if (now < last_us32) {
         const uint64_t dt_us = 0x100000000ULL;
         timer_base_us64 += dt_us;
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h
index 9ecfd9a373c812..e8f9ed5f703718 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include "ch.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -8,6 +9,7 @@ void hrt_init(void);
 uint64_t hrt_micros64(void);
 uint32_t hrt_micros32(void);
 uint32_t hrt_millis32(void);
+systime_t get_system_ticks(void);
 #ifdef __cplusplus
 }
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/system.cpp b/libraries/AP_HAL_ChibiOS/system.cpp
index d58b069f7b73f7..dc7d001294c3be 100644
--- a/libraries/AP_HAL_ChibiOS/system.cpp
+++ b/libraries/AP_HAL_ChibiOS/system.cpp
@@ -347,11 +347,7 @@ __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
-    return st_lld_get_counter() + AP_BOARD_START_TIME;
-#else
-    return st_lld_get_counter();
-#endif
+    return get_system_ticks();
 #else
     return hrt_micros32();
 #endif
@@ -360,9 +356,9 @@ __FASTRAMFUNC__ uint32_t micros()
 uint16_t micros16()
 {
 #if CH_CFG_ST_RESOLUTION == 32 && CH_CFG_ST_FREQUENCY==1000000U
-    return st_lld_get_counter() & 0xFFFF;
+    return get_system_ticks() & 0xFFFF;
 #elif CH_CFG_ST_RESOLUTION == 16 && CH_CFG_ST_FREQUENCY==1000000U
-    return st_lld_get_counter();
+    return get_system_ticks();
 #else
     return hrt_micros32() & 0xFFFF;
 #endif