From dbd89fe584d478af71cbf6a2cf8c5731b5976158 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 11 Feb 2016 14:14:18 +0100 Subject: [PATCH] px4_qurt_tasks: fix hang because of absolute time The timeout was triggered using absolute time instead of a delay in usec. This lead to the system hanging. With the fix it continues after the timeout, however, the rates still don't seem right. --- src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 7589c2bbcd..f1cbf02ddd 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -41,6 +41,7 @@ #include "px4_log.h" #include "px4_posix.h" #include "px4_workqueue.h" +#include "px4_time.h" #include "hrt_work.h" #include #include @@ -369,9 +370,14 @@ int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts) { work_s _hpwork = {}; - // Create a timer to unblock - uint32_t timeout = ts->tv_sec * 1000000 + (ts->tv_nsec / 1000); - hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout); + // We get an absolute time but want to calculate a timeout in us. + struct timespec ts_now; + px4_clock_gettime(CLOCK_REALTIME, &ts_now); + uint32_t timeout_us = ((ts->tv_sec - ts_now.tv_sec) * 1000000) + + ((ts->tv_nsec - ts_now.tv_nsec) / 1000); + + // Create a timer to unblock. + hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us); sem_wait(sem); hrt_work_cancel(&_hpwork); return 0;