mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-21 16:18:29 -04:00
HAL_ChibiOS: support starting the clock at non-zero
for testing time wrap bugs
This commit is contained in:
parent
c55a2916f6
commit
7527c6e278
@ -243,6 +243,10 @@ ifeq ($(ENABLE_STATS),yes)
|
||||
ASXFLAGS += -DHAL_ENABLE_THREAD_STATISTICS
|
||||
endif
|
||||
|
||||
ifneq ($(AP_BOARD_START_TIME),)
|
||||
UDEFS += -DAP_BOARD_START_TIME=$(AP_BOARD_START_TIME)
|
||||
endif
|
||||
|
||||
# Define ASM defines here
|
||||
UADEFS =
|
||||
|
||||
|
@ -75,6 +75,9 @@ 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;
|
||||
|
@ -319,7 +319,11 @@ __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
|
||||
#else
|
||||
return hrt_micros32();
|
||||
#endif
|
||||
|
Loading…
Reference in New Issue
Block a user