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