From 9e77f554d222308c37cecf15b17c738695825ba1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 11 Feb 2016 16:06:17 +0100 Subject: [PATCH] px4_qurt_tasks: fix timeout calculation It was wrong to subtract seconds and nanoseconds from each other. This will wrap, therefore use ts_to_abstime and do the subtraction afterwards. --- src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index f1cbf02ddd..b603a2ed5a 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -43,6 +43,7 @@ #include "px4_workqueue.h" #include "px4_time.h" #include "hrt_work.h" +#include #include #include #include @@ -371,10 +372,7 @@ int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts) work_s _hpwork = {}; // 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); + hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - hrt_absolute_time(); // Create a timer to unblock. hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us);