AP_HAL_ChibiOS: add support for setting scale factor on time

CubePilot · Dec 28, 2023 · 0cd13ba · 0cd13ba
1 parent 4cd4399
commit 0cd13ba
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 12 deletions.
diff --git 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
@@ -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
@@ -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