diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 33aaa1647f..f9a6ebc559 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -43,6 +43,7 @@ #include "device.h" #include "vfile.h" +#include #include #include #include @@ -61,7 +62,7 @@ struct timerData { ~timerData() {} }; -static void *timer_handler(void *data) +static void timer_cb(void *data) { struct timerData *td = (struct timerData *)data; @@ -72,7 +73,6 @@ static void *timer_handler(void *data) sem_post(&(td->sem)); PX4_DEBUG("timer_handler: Timer expired"); - return 0; } #define PX4_MAX_FD 200 @@ -239,25 +239,16 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) { if (timeout >= 0) { - pthread_t pt; - void *res; + // Use a work queue task + work_s _hpwork; - ts.tv_sec = timeout/1000; - ts.tv_nsec = (timeout % 1000)*1000000; - - // Create a timer to unblock struct timerData td(sem, ts); - int rv = pthread_create(&pt, NULL, timer_handler, (void *)&td); - if (rv != 0) { - count = -1; - goto cleanup; - } + hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&td, 1000*timeout); sem_wait(&sem); // Make sure timer thread is killed before sem goes // out of scope - (void)pthread_cancel(pt); - (void)pthread_join(pt, &res); + hrt_work_cancel(&_hpwork); } else { @@ -283,7 +274,6 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) } } -cleanup: sem_destroy(&sem); return count; diff --git a/src/platforms/posix/px4_layer/hrt_work.h b/src/platforms/posix/include/hrt_work.h similarity index 95% rename from src/platforms/posix/px4_layer/hrt_work.h rename to src/platforms/posix/include/hrt_work.h index d926a6d250..39e53f95d2 100644 --- a/src/platforms/posix/px4_layer/hrt_work.h +++ b/src/platforms/posix/include/hrt_work.h @@ -43,12 +43,9 @@ extern sem_t _hrt_work_lock; extern struct wqueue_s g_hrt_work; void hrt_work_queue_init(void); -int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay); void hrt_work_cancel(struct work_s *work); -//inline void hrt_work_lock(void); -//inline void hrt_work_unlock(void); - static inline void hrt_work_lock() { //PX4_INFO("hrt_work_lock"); diff --git a/src/platforms/qurt/px4_layer/hrt_work.h b/src/platforms/qurt/include/hrt_work.h similarity index 99% rename from src/platforms/qurt/px4_layer/hrt_work.h rename to src/platforms/qurt/include/hrt_work.h index 566684eb86..92b079ac6b 100644 --- a/src/platforms/qurt/px4_layer/hrt_work.h +++ b/src/platforms/qurt/include/hrt_work.h @@ -43,7 +43,7 @@ extern sem_t _hrt_work_lock; extern struct wqueue_s g_hrt_work; void hrt_work_queue_init(void); -int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay); void hrt_work_cancel(struct work_s *work); inline void hrt_work_lock(void);