From e4a8f32f1ba9cec74a71e06a1dd00000f8d2e310 Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Thu, 11 Jun 2015 18:13:36 -0700
Subject: [PATCH] QuRT: Added HRT workqueues as per POSIX

A high rate workqueue is required that acts like an interrupt handler
for a HW timer.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 makefiles/posix/config_posix_default.mk           |  1 -
 src/platforms/posix/px4_layer/px4_posix_tasks.cpp |  2 +-
 src/platforms/qurt/px4_layer/module.mk            |  3 +++
 src/platforms/qurt/px4_layer/px4_qurt_impl.cpp    |  6 ++++++
 src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp   | 13 ++++++++++---
 5 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/makefiles/posix/config_posix_default.mk b/makefiles/posix/config_posix_default.mk
index 5f1d80bdea..30dada7bbd 100644
--- a/makefiles/posix/config_posix_default.mk
+++ b/makefiles/posix/config_posix_default.mk
@@ -7,7 +7,6 @@
 #
 MODULES		+= drivers/device
 MODULES		+= drivers/blinkm
-MODULES		+= drivers/buzzer
 MODULES		+= drivers/hil
 MODULES		+= drivers/rgbled
 MODULES		+= drivers/led
diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
index ad24d32bc6..ac4c6b62bf 100644
--- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
@@ -57,7 +57,7 @@
 
 #define MAX_CMD_LEN 100
 
-#define PX4_MAX_TASKS 100
+#define PX4_MAX_TASKS 50
 #define SHELL_TASK_ID (PX4_MAX_TASKS+1)
 
 pthread_t _shell_task_id = 0;
diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk
index aa1a3b7d61..13b41db3ad 100644
--- a/src/platforms/qurt/px4_layer/module.mk
+++ b/src/platforms/qurt/px4_layer/module.mk
@@ -40,6 +40,9 @@ SRCDIR=$(dir $(MODULE_MK))
 SRCS		 = 	\
 			px4_qurt_impl.cpp \
 			px4_qurt_tasks.cpp  \
+			hrt_thread.c \
+                        hrt_queue.c \
+                        hrt_work_cancel.c \
 			work_thread.c \
 			work_queue.c \
 			work_lock.c \
diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
index 9087fd873e..a5782a6f25 100644
--- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
+++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
@@ -49,8 +49,11 @@
 #include <unistd.h>
 #include <semaphore.h>
 #include "systemlib/param/param.h"
+#include "hrt_work.h"
 
 
+extern pthread_t _shell_task_id;
+
 __BEGIN_DECLS
 
 // FIXME - sysconf(_SC_CLK_TCK) not supported
@@ -92,7 +95,10 @@ void init_once(void)
 	// Required for QuRT
 	//_posix_init();
 
+	_shell_task_id = pthread_self();
+	PX4_INFO("Shell id is %lu", _shell_task_id);
 	work_queues_init();
+	hrt_work_queue_init();
 	hrt_init();
 }
 
diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
index bea12adfbc..cf6d665724 100644
--- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
+++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
@@ -57,7 +57,11 @@
 
 #define MAX_CMD_LEN 100
 
-#define PX4_MAX_TASKS 100
+#define PX4_MAX_TASKS 50
+#define SHELL_TASK_ID (PX4_MAX_TASKS+1)
+
+pthread_t _shell_task_id = 0;
+
 struct task_entry
 {
 	pthread_t pid;
@@ -269,13 +273,16 @@ int px4_getpid()
 {
 	pthread_t pid = pthread_self();
 
+	if (pid == _shell_task_id)
+		return SHELL_TASK_ID;
+
 	// Get pthread ID from the opaque ID
 	for (int i=0; i<PX4_MAX_TASKS; ++i) {
-		if (taskmap[i].pid == pid) {
+		if (taskmap[i].isused && taskmap[i].pid == pid) {
 			return i;
 		}
 	}
-	PX4_ERR("px4_getpid() called from non-thread context!");
+	PX4_ERR("px4_getpid() called from unknown thread context!");
 	return -EINVAL;
 }