From a2d01995163eac503b1d1121c177b44a1c3cb35b Mon Sep 17 00:00:00 2001
From: Zachary Lowell <zachary.lowell@ascendengineer.com>
Date: Tue, 1 Nov 2022 17:35:20 -0500
Subject: [PATCH] Uncommenting Qurt platform task code (#20533)

---
 platforms/qurt/src/px4/tasks.cpp | 27 +++++++++++++++------------
 1 file changed, 15 insertions(+), 12 deletions(-)

diff --git a/platforms/qurt/src/px4/tasks.cpp b/platforms/qurt/src/px4/tasks.cpp
index 2dd044da44..de6c685233 100644
--- a/platforms/qurt/src/px4/tasks.cpp
+++ b/platforms/qurt/src/px4/tasks.cpp
@@ -35,8 +35,11 @@
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/tasks.h>
+#include <px4_platform_common/workqueue.h>
 
+#include <drivers/drv_hrt.h>
 #include <pthread.h>
+#include "hrt_work.h"
 
 #define PX4_TASK_STACK_SIZE 8192
 #define PX4_TASK_MAX_NAME_LENGTH 32
@@ -338,25 +341,25 @@ const char *px4_get_taskname()
 
 }
 
-// static void timer_cb(void *data)
-// {
-// 	px4_sem_t *sem = reinterpret_cast<px4_sem_t *>(data);
+static void timer_cb(void *data)
+{
+	px4_sem_t *sem = reinterpret_cast<px4_sem_t *>(data);
 
-// 	sem_post(sem);
-// }
+	sem_post(sem);
+}
 
 int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
 {
-	// work_s _hpwork = {};
+	work_s _hpwork = {};
 
-	// struct timespec ts_now;
-	// px4_clock_gettime(CLOCK_MONOTONIC, &ts_now);
+	struct timespec ts_now;
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts_now);
 
-	// hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now);
+	hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now);
 
-	// hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us);
-	// sem_wait(sem);
-	// hrt_work_cancel(&_hpwork);
+	hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us);
+	sem_wait(sem);
+	hrt_work_cancel(&_hpwork);
 	return 0;
 }