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.
This commit is contained in:
Julian Oes 2016-02-11 16:06:17 +01:00
parent dbd89fe584
commit 9e77f554d2
1 changed files with 2 additions and 4 deletions

View File

@ -43,6 +43,7 @@
#include "px4_workqueue.h"
#include "px4_time.h"
#include "hrt_work.h"
#include <drivers/drv_hrt.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
@ -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);