HAL_ChibiOS: fixed micros and millis on boards without 1MHz clock

ArduPilot · Dec 23, 2023 · d2d2067 · d2d2067
1 parent 5c1ed96
commit d2d2067
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 7 deletions.
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
@@ -84,12 +84,16 @@ static uint32_t get_systime_us32(void)
   wrap and directly gives a uint64_t (aka systimestamp_t)
 */
 
-uint64_t hrt_micros64I()
+static uint64_t hrt_micros64I(void)
 {
+    uint64_t ret = chVTGetTimeStampI();
+#if CH_CFG_ST_FREQUENCY != 1000000U
+    ret *= 1000000U/CH_CFG_ST_FREQUENCY;
+#endif
 #ifdef AP_BOARD_START_TIME
-    return chVTGetTimeStampI() + AP_BOARD_START_TIME;
+    ret += AP_BOARD_START_TIME;
 #endif
-    return chVTGetTimeStampI();
+    return ret;
 }
 
 static inline bool is_locked(void) {
@@ -99,17 +103,17 @@ static inline bool is_locked(void) {
 uint64_t hrt_micros64()
 {
     if (is_locked()) {
-        return chVTGetTimeStampI();
+        return hrt_micros64I();
     } else if (port_is_isr_context()) {
         uint64_t ret;
         chSysLockFromISR();
-        ret = chVTGetTimeStampI();
+        ret = hrt_micros64I();
         chSysUnlockFromISR();
         return ret;
     } else {
         uint64_t ret;
         chSysLock();
-        ret = chVTGetTimeStampI();
+        ret = hrt_micros64I();
         chSysUnlock();
         return ret;
     }

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h
@@ -7,7 +7,6 @@ extern "C" {
 
 void hrt_init(void);
 uint64_t hrt_micros64(void);
-uint64_t hrt_micros64I(void); // from locked context
 uint64_t hrt_micros64_from_ISR(void); // from an ISR
 uint32_t hrt_micros32(void);
 uint32_t hrt_millis32(void);