diff --git a/platforms/posix/src/px4_layer/drv_hrt.c b/platforms/posix/src/px4_layer/drv_hrt.c index d8b7408cd2..e17e6b3645 100644 --- a/platforms/posix/src/px4_layer/drv_hrt.c +++ b/platforms/posix/src/px4_layer/drv_hrt.c @@ -50,6 +50,8 @@ #include #include "hrt_work.h" +#define SPEED_FACTOR 1 + static struct sq_queue_s callout_queue; /* latency histogram */ @@ -67,7 +69,7 @@ static void hrt_call_reschedule(void); static px4_sem_t _hrt_lock; static struct work_s _hrt_work; #ifndef __PX4_QURT -static hrt_abstime px4_timestart = 0; +static hrt_abstime px4_timestart_monotonic = 0; #else static int32_t dsp_offset = 0; #endif @@ -153,28 +155,8 @@ uint64_t hrt_system_time(void) hrt_abstime _hrt_absolute_time_internal(void) { struct timespec ts; - -#if defined(__PX4_QURT) - // Don't use the timestart on the DSP on Snapdragon because we manually - // set the px4_timestart using the hrt_set_absolute_time_offset(). - px4_clock_gettime(CLOCK_MONOTONIC, &ts); - return ts_to_abstime(&ts) + dsp_offset; - -#elif (defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR)) - // Don't do any offseting on the Linux side on the Snapdragon. px4_clock_gettime(CLOCK_MONOTONIC, &ts); return ts_to_abstime(&ts); - -#else - - if (!px4_timestart) { - px4_clock_gettime(CLOCK_MONOTONIC, &ts); - px4_timestart = ts_to_abstime(&ts); - } - - px4_clock_gettime(CLOCK_MONOTONIC, &ts); - return ts_to_abstime(&ts) - px4_timestart; -#endif } #ifdef __PX4_QURT @@ -217,7 +199,7 @@ hrt_abstime hrt_absolute_time(void) __EXPORT hrt_abstime hrt_reset(void) { #ifndef __PX4_QURT - px4_timestart = 0; + px4_timestart_monotonic = 0; #endif max_time = 0; return _hrt_absolute_time_internal(); @@ -535,12 +517,10 @@ void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callo hrt_call_internal(entry, calltime, 0, callout, arg); } -#if 0 /* * Convert absolute time to a timespec. */ void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); -#endif static void hrt_call_invoke(void) @@ -601,3 +581,79 @@ hrt_call_invoke(void) hrt_unlock(); } +void abstime_to_ts(struct timespec *ts, hrt_abstime abstime) +{ + ts->tv_sec = abstime / 1000000; + abstime -= ts->tv_sec * 1000000; + ts->tv_nsec = abstime * 1000; +} + +int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) +{ +#if defined(__PX4_QURT) + // Don't use the timestart on the DSP on Snapdragon because we manually + // set the px4_timestart using the hrt_set_absolute_time_offset(). + return clock_gettime(clk_id, &tp); + +#elif defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR) + // Don't do any offseting on the Linux side on the Snapdragon. + return clock_gettime(clk_id, &tp); +#else + struct timespec actual_tp; + const int ret = clock_gettime(clk_id, &actual_tp); + + if (ret != 0) { + return ret; + } + + const uint64_t actual_usec = ts_to_abstime(&actual_tp); + + + if (clk_id == CLOCK_MONOTONIC) { + if (px4_timestart_monotonic == 0) { + px4_timestart_monotonic = actual_usec; + } + + abstime_to_ts(tp, (actual_usec - px4_timestart_monotonic) * SPEED_FACTOR); + + } else { + abstime_to_ts(tp, actual_usec); + } + + //static unsigned counter = 0; + //if (counter++ % 1000000 == 0) { + // PX4_INFO("clk_id: %d, actual_tp.tv_sec: %llu, actual_tp.tv_nsec: %llu, px4_timestart: %llu: 0x%x", + // clk_id, actual_tp.tv_sec, actual_tp.tv_nsec, px4_timestart[clk_id], &px4_timestart[clk_id]); + //} + + return 0; +#endif +} + +int px4_clock_settime(clockid_t clk_id, const struct timespec *tp) +{ + // not implemented + return 0; +} + + +int px4_usleep(useconds_t usec) +{ + return system_usleep(usec / SPEED_FACTOR); +} + +unsigned int px4_sleep(unsigned int seconds) +{ + useconds_t usec = seconds * 1000000; + return system_usleep(usec / SPEED_FACTOR); +} + +int px4_pthread_cond_timedwait(pthread_cond_t *cond, + pthread_mutex_t *mutex, + const struct timespec *abstime) +{ + struct timespec new_abstime; + new_abstime.tv_sec = abstime->tv_sec / SPEED_FACTOR; + new_abstime.tv_nsec = abstime->tv_nsec / SPEED_FACTOR; + return pthread_cond_timedwait(cond, mutex, &new_abstime); +} diff --git a/platforms/posix/src/px4_layer/px4_sem.cpp b/platforms/posix/src/px4_layer/px4_sem.cpp index 6e28af7b59..938188a6fe 100644 --- a/platforms/posix/src/px4_layer/px4_sem.cpp +++ b/platforms/posix/src/px4_layer/px4_sem.cpp @@ -124,7 +124,7 @@ int px4_sem_timedwait(px4_sem_t *s, const struct timespec *abstime) errno = 0; if (s->value < 0) { - ret = pthread_cond_timedwait(&(s->wait), &(s->lock), abstime); + ret = px4_pthread_cond_timedwait(&(s->wait), &(s->lock), abstime); } else { ret = 0; diff --git a/platforms/qurt/src/px4_layer/drv_hrt.c b/platforms/qurt/src/px4_layer/drv_hrt.c index 3ca4cbfe9c..dc67e6703a 100644 --- a/platforms/qurt/src/px4_layer/drv_hrt.c +++ b/platforms/qurt/src/px4_layer/drv_hrt.c @@ -375,12 +375,6 @@ void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callo hrt_call_internal(entry, calltime, 0, callout, arg); } -#if 0 -/* - * Convert absolute time to a timespec. - */ -void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); -#endif static void hrt_call_invoke(void) diff --git a/src/include/visibility.h b/src/include/visibility.h index 3c74ce7832..91445679dd 100644 --- a/src/include/visibility.h +++ b/src/include/visibility.h @@ -73,6 +73,12 @@ #define system_sleep sleep #pragma GCC poison sleep +#include +#define system_pthread_cond_timedwait pthread_cond_timedwait +// We can't poison pthread_cond_timedwait because it seems to be used in the +// include. + + #ifdef __cplusplus #include #endif diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 9f456acb3c..98248e1cf4 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 9f456acb3c8d70df3cd8906123388935dd6303f6 +Subproject commit 98248e1cf4e9e78f942aec663bb58e675749498c diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h index 80bfe34a76..548f58abd8 100644 --- a/src/platforms/px4_time.h +++ b/src/platforms/px4_time.h @@ -2,6 +2,7 @@ #include #include +#include #if defined(__PX4_APPLE_LEGACY) @@ -31,10 +32,29 @@ __END_DECLS #else + +#if 0 #define px4_clock_gettime clock_gettime #define px4_clock_settime clock_settime #define px4_usleep system_usleep #define px4_sleep system_sleep +#define px4_pthread_cond_timedwait system_pthread_cond_timedwait +#else + + +__BEGIN_DECLS +__EXPORT int px4_clock_gettime(clockid_t clk_id, struct timespec *tp); +__EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp); + +__EXPORT int px4_usleep(useconds_t usec); +__EXPORT unsigned int px4_sleep(unsigned int seconds); +__EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond, + pthread_mutex_t *mutex, + const struct timespec *abstime); +__END_DECLS +#endif + + #endif