diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 496e272d62..d063561eb7 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -49,7 +49,7 @@ __BEGIN_DECLS -/* +/** * Absolute time, in microsecond units. * * Absolute time is measured from some arbitrary epoch shortly after @@ -57,7 +57,7 @@ __BEGIN_DECLS */ typedef uint64_t hrt_abstime; -/* +/** * Callout function type. * * Note that callouts run in the timer interrupt context, so @@ -66,7 +66,7 @@ typedef uint64_t hrt_abstime; */ typedef void (* hrt_callout)(void *arg); -/* +/** * Callout record. */ typedef struct hrt_call { @@ -78,22 +78,22 @@ typedef struct hrt_call { void *arg; } *hrt_call_t; -/* +/** * Get absolute time. */ __EXPORT extern hrt_abstime hrt_absolute_time(void); -/* +/** * Convert a timespec to absolute time. */ __EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts); -/* +/** * Convert absolute time to a timespec. */ __EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); -/* +/** * Compute the delta between a timestamp taken in the past * and now. * @@ -102,14 +102,14 @@ __EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); */ __EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then); -/* +/** * Store the absolute time in an interrupt-safe fashion. * * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. */ __EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now); -/* +/** * Call callout(arg) after delay has elapsed. * * If callout is NULL, this can be used to implement a timeout by testing the call @@ -117,12 +117,12 @@ __EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now); */ __EXPORT extern void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg); -/* +/** * Call callout(arg) at absolute time calltime. */ __EXPORT extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg); -/* +/** * Call callout(arg) after delay, and then after every interval. * * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may @@ -131,7 +131,7 @@ __EXPORT extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, h __EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg); -/* +/** * If this returns true, the entry has been invoked and removed from the callout list, * or it has never been entered. * @@ -139,13 +139,13 @@ __EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, h */ __EXPORT extern bool hrt_called(struct hrt_call *entry); -/* +/** * Remove the entry from the callout list. */ __EXPORT extern void hrt_cancel(struct hrt_call *entry); -/* - * initialise a hrt_call structure +/** + * Initialise a hrt_call structure */ __EXPORT extern void hrt_call_init(struct hrt_call *entry); @@ -163,4 +163,21 @@ __EXPORT extern void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay); */ __EXPORT extern void hrt_init(void); +#ifdef __PX4_POSIX + +/** + * Start to delay the HRT return value. + * + * Until hrt_stop_delay() is called the HRT calls will return the timestamp + * at the instance then hrt_start_delay() was called. + */ +__EXPORT extern void hrt_start_delay(void); + +/** + * Stop to delay the HRT. + */ +__EXPORT extern void hrt_stop_delay(void); + +#endif + __END_DECLS diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 69dbb90609..ed9e2d772b 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -65,10 +65,16 @@ static void hrt_call_reschedule(void); static px4_sem_t _hrt_lock; static struct work_s _hrt_work; static hrt_abstime px4_timestart = 0; +static hrt_abstime _start_delay_time = 0; +static hrt_abstime _delay_interval = 0; +pthread_mutex_t _hrt_mutex = PTHREAD_MUTEX_INITIALIZER; static void hrt_call_invoke(void); +static hrt_abstime +_hrt_absolute_time_internal(void); + __EXPORT hrt_abstime hrt_reset(void); static void hrt_lock(void) @@ -116,7 +122,7 @@ int px4_clock_settime(clockid_t clk_id, struct timespec *tp) /* * Get absolute time. */ -hrt_abstime hrt_absolute_time(void) +hrt_abstime _hrt_absolute_time_internal(void) { struct timespec ts; @@ -129,10 +135,39 @@ hrt_abstime hrt_absolute_time(void) return ts_to_abstime(&ts) - px4_timestart; } +/* + * Get absolute time. + */ +hrt_abstime hrt_absolute_time(void) +{ + pthread_mutex_lock(&_hrt_mutex); + static hrt_abstime max_time = 0; + + hrt_abstime ret; + + if (_start_delay_time > 0) { + ret = _start_delay_time; + } else { + ret = _hrt_absolute_time_internal(); + } + + ret -= _delay_interval; + + if (ret < max_time) { + PX4_ERR("WARNING! TIME IS NEGATIVE! %d vs %d", (int)ret, (int)max_time); + ret = max_time; + } + + max_time = ret; + pthread_mutex_unlock(&_hrt_mutex); + + return ret; +} + __EXPORT hrt_abstime hrt_reset(void) { px4_timestart = 0; - return hrt_absolute_time(); + return _hrt_absolute_time_internal(); } /* @@ -157,7 +192,7 @@ hrt_abstime ts_to_abstime(struct timespec *ts) */ hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) { - hrt_abstime delta = hrt_absolute_time() - *then; + hrt_abstime delta = _hrt_absolute_time_internal() - *then; return delta; } @@ -168,7 +203,7 @@ hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) */ hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now) { - hrt_abstime ts = hrt_absolute_time(); + hrt_abstime ts = _hrt_absolute_time_internal(); return ts; } @@ -218,7 +253,7 @@ void hrt_call_init(struct hrt_call *entry) */ void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) { - entry->deadline = hrt_absolute_time() + delay; + entry->deadline = _hrt_absolute_time_internal() + delay; } /* @@ -237,6 +272,26 @@ void hrt_init(void) memset(&_hrt_work, 0, sizeof(_hrt_work)); } +void hrt_start_delay() +{ + pthread_mutex_lock(&_hrt_mutex); + _start_delay_time = _hrt_absolute_time_internal(); + pthread_mutex_unlock(&_hrt_mutex); +} + +void hrt_stop_delay() +{ + pthread_mutex_lock(&_hrt_mutex); + int64_t delta = _hrt_absolute_time_internal() - _start_delay_time; + _delay_interval += delta; + _start_delay_time = 0; + if (delta > 10000) { + PX4_INFO("simulator is slow. Delay added: %llu us", delta); + } + pthread_mutex_unlock(&_hrt_mutex); + +} + static void hrt_call_enter(struct hrt_call *entry) { @@ -295,7 +350,7 @@ hrt_tim_isr(void *p) static void hrt_call_reschedule() { - hrt_abstime now = hrt_absolute_time(); + hrt_abstime now = _hrt_absolute_time_internal(); hrt_abstime delay = HRT_INTERVAL_MAX; struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); hrt_abstime deadline = now + HRT_INTERVAL_MAX; @@ -383,7 +438,7 @@ void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callo { //printf("hrt_call_after\n"); hrt_call_internal(entry, - hrt_absolute_time() + delay, + _hrt_absolute_time_internal() + delay, 0, callout, arg); @@ -398,7 +453,7 @@ void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callo void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) { hrt_call_internal(entry, - hrt_absolute_time() + delay, + _hrt_absolute_time_internal() + delay, interval, callout, arg); @@ -429,7 +484,7 @@ hrt_call_invoke(void) while (true) { /* get the current time */ - hrt_abstime now = hrt_absolute_time(); + hrt_abstime now = _hrt_absolute_time_internal(); call = (struct hrt_call *)sq_peek(&callout_queue);