diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 0be34240c6..0a45644481 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -61,6 +61,7 @@ static void hrt_call_reschedule(void); static sem_t _hrt_lock; static struct work_s _hrt_work; +static hrt_abstime px4_timestart = 0; static void hrt_call_invoke(void); @@ -86,7 +87,6 @@ static void hrt_unlock(void) #define clockid_t int static double px4_timebase = 0.0; -static uint64_t px4_timestart = 0; int clock_gettime(clockid_t clk_id, struct timespec *t) { @@ -119,8 +119,13 @@ hrt_abstime hrt_absolute_time(void) { struct timespec ts; + if (!px4_timestart) { + clock_gettime(CLOCK_MONOTONIC, &ts); + px4_timestart = ts_to_abstime(&ts); + } + clock_gettime(CLOCK_MONOTONIC, &ts); - return ts_to_abstime(&ts); + return ts_to_abstime(&ts) - px4_timestart; } /*