diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk
index 48fcefffe5..2e2323f4f0 100644
--- a/makefiles/qurt/config_qurt_default.mk
+++ b/makefiles/qurt/config_qurt_default.mk
@@ -72,7 +72,7 @@ MODULES		+= platforms/posix/drivers/barosim
 # Unit tests
 #
 MODULES		+= platforms/qurt/tests/hello
-#MODULES		+= platforms/posix/tests/vcdev_test
+MODULES		+= platforms/posix/tests/vcdev_test
 #MODULES		+= platforms/posix/tests/hrt_test
-#MODULES		+= platforms/posix/tests/wqueue
+MODULES		+= platforms/posix/tests/wqueue
 
diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp
index f02b021ffc..473307b1cc 100644
--- a/src/drivers/device/i2c_posix.cpp
+++ b/src/drivers/device/i2c_posix.cpp
@@ -108,6 +108,10 @@ I2C::init()
 		return PX4_ERROR;
 	}
 
+#ifdef __PX4_QURT
+	simulate = true;
+#endif
+
 	if (simulate) {
 		_fd = 10000;
 	}
diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp
index ca75c91923..d3b54c3db5 100644
--- a/src/drivers/device/vdev_posix.cpp
+++ b/src/drivers/device/vdev_posix.cpp
@@ -48,8 +48,6 @@
 #include <pthread.h>
 #include <unistd.h>
 
-#define PX4_DEBUG(...)  
-
 using namespace device;
 
 extern "C" {
diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp
index 57155d1350..e712234f1b 100644
--- a/src/modules/simulator/simulator.cpp
+++ b/src/modules/simulator/simulator.cpp
@@ -36,7 +36,7 @@
  * A device simulator
  */
 
-#include <px4_debug.h>
+#include <px4_log.h>
 #include <px4_tasks.h>
 #include <px4_time.h>
 #include <pthread.h>
diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
index c831ae04dd..c09845b79a 100644
--- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
@@ -37,7 +37,7 @@
  * Implementation of existing task API for Linux
  */
 
-#include <px4_debug.h>
+#include <px4_log.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <stdlib.h>
diff --git a/src/platforms/posix/tests/hello/hello_start_posix.cpp b/src/platforms/posix/tests/hello/hello_start_posix.cpp
index 962645fabe..8dde729a6e 100644
--- a/src/platforms/posix/tests/hello/hello_start_posix.cpp
+++ b/src/platforms/posix/tests/hello/hello_start_posix.cpp
@@ -57,14 +57,14 @@ int hello_main(int argc, char *argv[])
 {
 	
 	if (argc < 2) {
-		printf("usage: hello {start|stop|status}\n");
+		PX4_WARN("usage: hello {start|stop|status}\n");
 		return 1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
 		if (HelloExample::appState.isRunning()) {
-			printf("already running\n");
+			PX4_INFO("already running\n");
 			/* this is not an error */
 			return 0;
 		}
@@ -86,15 +86,15 @@ int hello_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (HelloExample::appState.isRunning()) {
-			printf("is running\n");
+			PX4_INFO("is running\n");
 
 		} else {
-			printf("not started\n");
+			PX4_INFO("not started\n");
 		}
 
 		return 0;
 	}
 
-	printf("usage: hello_main {start|stop|status}\n");
+	PX4_WARN("usage: hello_main {start|stop|status}\n");
 	return 1;
 }
diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp
index 7f1c452d33..0721ae035a 100644
--- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp
+++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp
@@ -59,7 +59,7 @@ static int writer_main(int argc, char *argv[])
 
 	int fd = px4_open(TESTDEV, PX4_F_RDONLY);
 	if (fd < 0) {
-		printf("--- Open failed %d %d", fd, px4_errno);
+		PX4_INFO("--- Open failed %d %d", fd, px4_errno);
 		return -px4_errno;
 	}
 
@@ -67,14 +67,14 @@ static int writer_main(int argc, char *argv[])
 	int i=0;
 	while (i<3) {
 		// Wait for 3 seconds
-		printf("--- Sleeping for 4 sec\n");
+		PX4_INFO("--- Sleeping for 4 sec\n");
 		ret = sleep(4);
 		if (ret < 0) {
-			printf("--- sleep failed %d %d\n", ret, errno);
+			PX4_INFO("--- sleep failed %d %d\n", ret, errno);
 			return ret;
 		}
 
-		printf("--- writing to fd\n");
+		PX4_INFO("--- writing to fd\n");
 		ret = px4_write(fd, buf, 1);
 	++i;
 	}
@@ -110,16 +110,16 @@ static int test_pub_block(int fd, unsigned long blocked)
 {
 	int ret = px4_ioctl(fd, DEVIOCSPUBBLOCK, blocked);
 	if (ret < 0) {
-		printf("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno);
+		PX4_INFO("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno);
 		return -px4_errno;
 	}
 
 	ret = px4_ioctl(fd, DEVIOCGPUBBLOCK, 0);
 	if (ret < 0) {
-		printf("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno);
+		PX4_INFO("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno);
 		return -px4_errno;
 	}
-	printf("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL");
+	PX4_INFO("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL");
 
 	return 0;
 }
@@ -131,29 +131,29 @@ int VCDevExample::main()
 	_node = new VCDevNode();
 
 	if (_node == 0) {
-		printf("Failed to allocate VCDevNode\n");
+		PX4_INFO("Failed to allocate VCDevNode\n");
 		return -ENOMEM;
 	}
 
 	if (_node->init() != PX4_OK) {
-		printf("Failed to init VCDevNode\n");
+		PX4_INFO("Failed to init VCDevNode\n");
 		return 1;
 	}
 
 	int fd = px4_open(TESTDEV, PX4_F_RDONLY);
 
 	if (fd < 0) {
-		printf("Open failed %d %d", fd, px4_errno);
+		PX4_INFO("Open failed %d %d", fd, px4_errno);
 		return -px4_errno;
 	}
 
 	void *p = 0;
 	int ret = px4_ioctl(fd, DIOC_GETPRIV, (unsigned long)&p);
 	if (ret < 0) {
-		printf("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno);
+		PX4_INFO("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno);
 		return -px4_errno;
 	}
-	printf("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL");
+	PX4_INFO("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL");
 
 	ret = test_pub_block(fd, 1);
 	if (ret < 0)
@@ -174,32 +174,32 @@ int VCDevExample::main()
 				       (char* const*)NULL);
 
 	while (!appState.exitRequested() && i<13) {
-		printf("=====================\n");
-		printf("====  sleeping 2 sec ====\n");
+		PX4_INFO("=====================\n");
+		PX4_INFO("====  sleeping 2 sec ====\n");
 		sleep(2);
 
 		fds[0].fd = fd;
 		fds[0].events = POLLIN;
 		fds[0].revents = 0;
 
-		printf("==== Calling Poll\n");
+		PX4_INFO("==== Calling Poll\n");
 		ret = px4_poll(fds, 1, 1000);
-		printf("==== Done poll\n");
+		PX4_INFO("==== Done poll\n");
 		if (ret < 0) {
-			printf("==== poll failed %d %d\n", ret, px4_errno);
+			PX4_INFO("==== poll failed %d %d\n", ret, px4_errno);
 			px4_close(fd);
 		} 
 		else if (i > 0) {
 			if (ret == 0)
-				printf("==== Nothing to read - PASS\n");
+				PX4_INFO("==== Nothing to read - PASS\n");
 			else
-				printf("==== poll returned %d\n", ret);
+				PX4_INFO("==== poll returned %d\n", ret);
 		}
 		else if (i == 0) {
 			if (ret == 1) 
-				printf("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL");
+				PX4_INFO("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL");
 			else
-				printf("==== %d to read - FAIL\n", ret);
+				PX4_INFO("==== %d to read - FAIL\n", ret);
 		
 		}
 		++i;
diff --git a/src/platforms/posix/tests/wqueue/wqueue_main.cpp b/src/platforms/posix/tests/wqueue/wqueue_main.cpp
index a7117cca09..8dd5cc9707 100644
--- a/src/platforms/posix/tests/wqueue/wqueue_main.cpp
+++ b/src/platforms/posix/tests/wqueue/wqueue_main.cpp
@@ -37,6 +37,7 @@
  *
  * @author Mark Charlebois <charlebm@gmail.com>
  */
+#include <px4_log.h>
 #include <px4_middleware.h>
 #include <px4_app.h>
 #include "wqueue_test.h"
@@ -46,10 +47,10 @@ int PX4_MAIN(int argc, char **argv)
 {
 	px4::init(argc, argv, "wqueue_test");
 
-	printf("wqueue hello\n");
+	PX4_INFO("wqueue hello\n");
 	WQueueTest wq;
 	wq.main();
 
-	printf("goodbye\n");
+	PX4_INFO("goodbye\n");
 	return 0;
 }
diff --git a/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp
index 20c9975578..2479020097 100644
--- a/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp
+++ b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp
@@ -39,6 +39,7 @@
  */
 #include "wqueue_test.h"
 #include <px4_app.h>
+#include <px4_log.h>
 #include <px4_tasks.h>
 #include <stdio.h>
 #include <string.h>
@@ -53,14 +54,14 @@ int wqueue_test_main(int argc, char *argv[])
 {
 	
 	if (argc < 2) {
-		printf("usage: wqueue_test {start|stop|status}\n");
+		PX4_INFO("usage: wqueue_test {start|stop|status}\n");
 		return 1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
 		if (WQueueTest::appState.isRunning()) {
-			printf("already running\n");
+			PX4_INFO("already running\n");
 			/* this is not an error */
 			return 0;
 		}
@@ -82,15 +83,15 @@ int wqueue_test_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (WQueueTest::appState.isRunning()) {
-			printf("is running\n");
+			PX4_INFO("is running\n");
 
 		} else {
-			printf("not started\n");
+			PX4_INFO("not started\n");
 		}
 
 		return 0;
 	}
 
-	printf("usage: wqueue_test {start|stop|status}\n");
+	PX4_INFO("usage: wqueue_test {start|stop|status}\n");
 	return 1;
 }
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 1b2cf1dea5..c17b39e4c5 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -39,7 +39,7 @@
 
 #pragma once
 
-#include <px4_debug.h>
+#include <px4_log.h>
 
 /* Get the name of the default value fiven the param name */
 #define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT
diff --git a/src/platforms/px4_debug.h b/src/platforms/px4_log.h
similarity index 84%
rename from src/platforms/px4_debug.h
rename to src/platforms/px4_log.h
index 6cac039b28..e0d3cff8d4 100644
--- a/src/platforms/px4_debug.h
+++ b/src/platforms/px4_log.h
@@ -32,13 +32,25 @@
  ****************************************************************************/
 
 /**
- * @file px4_debug.h
- * Platform dependant debug
+ * @file px4_log.h
+ * Platform dependant logging/debug
  */
 
 #pragma once
 
-#if defined(__PX4_LINUX) || defined(__PX4_QURT)
+#if defined(__PX4_QURT)
+
+__BEGIN_DECLS
+extern void qurt_log(const char *fmt, ...);
+__END_DECLS
+
+#define PX4_DBG(...)	qurt_log(__VA_ARGS__)
+#define PX4_DEBUG(...)	qurt_log(__VA_ARGS__)
+#define PX4_INFO(...) 	qurt_log(__VA_ARGS__)
+#define PX4_WARN(...) 	qurt_log(__VA_ARGS__)
+#define PX4_ERR(...)	{ qurt_log("ERROR file %s line %d:", __FILE__, __LINE__); qurt_log(__VA_ARGS__); }
+
+#elif defined(__PX4_LINUX)
 
 #if defined(__PX4_LINUX)
 #include <err.h>
diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h
index d72cdbb8b1..0c9b7f24c9 100644
--- a/src/platforms/px4_time.h
+++ b/src/platforms/px4_time.h
@@ -10,10 +10,13 @@
 
 #elif defined(__PX4_QURT)
 
+#include <sys/timespec.h>
+
 #define CLOCK_REALTIME 1
 
 __BEGIN_DECLS
 
+#if 0
 #if !defined(__cplusplus)
 struct timespec
 {
@@ -21,6 +24,7 @@ struct timespec
 	long tv_nsec;
 };
 #endif
+#endif
 int px4_clock_gettime(clockid_t clk_id, struct timespec *tp);
 int px4_clock_settime(clockid_t clk_id, struct timespec *tp);
 
diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/main.cpp
index 217fa68d0c..58d37da24b 100644
--- a/src/platforms/qurt/main.cpp
+++ b/src/platforms/qurt/main.cpp
@@ -52,6 +52,9 @@
 #include "px4_middleware.h"
 
 static const char *commands = 
+"x\n"
+"hello start"
+#if 0
 "uorb start\n"
 "simulator start -s\n"
 "barosim start\n"
@@ -68,7 +71,9 @@ static const char *commands =
 "hil mode_pwm\n"
 "commander start\n"
 "list_devices\n"
-"list_topics\n";
+"list_topics\n"
+#endif
+;
 
 
 static void run_cmd(const vector<string> &appargs) {
@@ -148,8 +153,13 @@ static void process_commands(const char *cmds)
 	}
 }
 
+namespace px4 {
+extern void init_once(void);
+};
+
 int main(int argc, char **argv)
 {
+	px4::init_once();
 	px4::init(argc, argv, "mainapp");
 	process_commands(commands);
 	for (;;) {}
diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c
index 39e0f73b37..3edf34f64c 100644
--- a/src/platforms/qurt/px4_layer/drv_hrt.c
+++ b/src/platforms/qurt/px4_layer/drv_hrt.c
@@ -37,9 +37,12 @@
  * High-resolution timer with callouts and timekeeping.
  */
 
+#include <px4_workqueue.h>
 #include <drivers/drv_hrt.h>
+#include <semaphore.h>
 #include <time.h>
 #include <string.h>
+#include <stdio.h>
 
 static struct sq_queue_s	callout_queue;
 
@@ -51,9 +54,28 @@ __EXPORT uint32_t	latency_counters[LATENCY_BUCKET_COUNT + 1];
 
 static void		hrt_call_reschedule(void);
 
+// Intervals in ms
 #define HRT_INTERVAL_MIN	50
 #define HRT_INTERVAL_MAX	50000
 
+static sem_t 	_hrt_lock;
+static struct work_s	_hrt_work;
+
+static void
+hrt_call_invoke(void);
+
+static void hrt_lock(void)
+{
+	//printf("hrt_lock\n");
+	sem_wait(&_hrt_lock);
+}
+
+static void hrt_unlock(void)
+{
+	//printf("hrt_unlock\n");
+	sem_post(&_hrt_lock);
+}
+
 /*
  * Get absolute time.
  */
@@ -61,7 +83,7 @@ hrt_abstime hrt_absolute_time(void)
 {
 	struct timespec ts;
 
-	// FIXME - not supported in QURT
+	// FIXME - clock_gettime unsupported in QuRT
 	//clock_gettime(CLOCK_MONOTONIC, &ts);
 	return ts_to_abstime(&ts);
 }
@@ -120,7 +142,7 @@ bool	hrt_called(struct hrt_call *entry)
  */
 void	hrt_cancel(struct hrt_call *entry)
 {
-	// FIXME - need a lock
+	hrt_lock();
 	sq_rem(&entry->link, &callout_queue);
 	entry->deadline = 0;
 
@@ -128,6 +150,7 @@ void	hrt_cancel(struct hrt_call *entry)
 	 * being re-entered when the callout returns.
 	 */
 	entry->period = 0;
+	hrt_unlock();
 	// endif
 }
 
@@ -156,7 +179,10 @@ void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
  */
 void	hrt_init(void)
 {
+	//printf("hrt_init\n");
 	sq_init(&callout_queue);
+	sem_init(&_hrt_lock, 0, 1);
+	memset(&_hrt_work, 0, sizeof(_hrt_work));
 }
 
 static void
@@ -164,6 +190,7 @@ hrt_call_enter(struct hrt_call *entry)
 {
 	struct hrt_call	*call, *next;
 
+	//printf("hrt_call_enter\n");
 	call = (struct hrt_call *)sq_peek(&callout_queue);
 
 	if ((call == NULL) || (entry->deadline < call->deadline)) {
@@ -187,6 +214,27 @@ hrt_call_enter(struct hrt_call *entry)
 	//lldbg("scheduled\n");
 }
 
+/**
+ * Timer interrupt handler
+ *
+ * This routine simulates a timer interrupt handler
+ */
+static void 
+hrt_tim_isr(void *p)
+{
+
+	//printf("hrt_tim_isr\n");
+	/* run any callouts that have met their deadline */
+	hrt_call_invoke();
+
+	hrt_lock();
+
+	/* and schedule the next interrupt */
+	hrt_call_reschedule();
+
+	hrt_unlock();
+}
+
 /**
  * Reschedule the next timer interrupt.
  *
@@ -198,7 +246,10 @@ hrt_call_reschedule()
 	hrt_abstime	now = hrt_absolute_time();
 	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
 	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+	uint32_t	ticks = USEC2TICK(HRT_INTERVAL_MAX*1000);
 
+	//printf("hrt_call_reschedule\n");
+	
 	/*
 	 * Determine what the next deadline will be.
 	 *
@@ -216,18 +267,26 @@ hrt_call_reschedule()
 		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
 			//lldbg("pre-expired\n");
 			/* set a minimal deadline so that we call ASAP */
-			deadline = now + HRT_INTERVAL_MIN;
+			ticks = USEC2TICK(HRT_INTERVAL_MIN*1000);
 
 		} else if (next->deadline < deadline) {
 			//lldbg("due soon\n");
-			deadline = next->deadline;
+			ticks = USEC2TICK((next->deadline - now)*1000);
 		}
 	}
+
+	// There is no timer ISR, so simulate one by putting an event on the 
+	// high priority work queue
+	//printf("ticks = %u\n", ticks);
+        work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks);
 }
 
 static void
 hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
 {
+	//printf("hrt_call_internal\n");
+	hrt_lock();
+	//printf("hrt_call_internal after lock\n");
 	/* if the entry is currently queued, remove it */
 	/* note that we are using a potentially uninitialised
 	   entry->link here, but it is safe as sq_rem() doesn't
@@ -245,6 +304,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	entry->arg = arg;
 
 	hrt_call_enter(entry);
+	hrt_unlock();
 }
 
 /*
@@ -255,6 +315,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
  */
 void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
 {
+	//printf("hrt_call_after\n");
 	hrt_call_internal(entry,
 			  hrt_absolute_time() + delay,
 			  0,
@@ -292,3 +353,57 @@ void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callo
 void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
 #endif
 
+static void
+hrt_call_invoke(void)
+{
+	struct hrt_call	*call;
+	hrt_abstime deadline;
+
+	hrt_lock();
+	while (true) {
+		/* get the current time */
+		hrt_abstime now = hrt_absolute_time();
+
+		call = (struct hrt_call *)sq_peek(&callout_queue);
+
+		if (call == NULL)
+			break;
+
+		if (call->deadline > now)
+			break;
+
+		sq_rem(&call->link, &callout_queue);
+		//lldbg("call pop\n");
+
+		/* save the intended deadline for periodic calls */
+		deadline = call->deadline;
+
+		/* zero the deadline, as the call has occurred */
+		call->deadline = 0;
+
+		/* invoke the callout (if there is one) */
+		if (call->callout) {
+			// Unlock so we don't deadlock in callback
+			hrt_unlock();
+
+			//lldbg("call %p: %p(%p)\n", call, call->callout, call->arg);
+			call->callout(call->arg);
+
+			hrt_lock();
+		}
+
+		/* if the callout has a non-zero period, it has to be re-entered */
+		if (call->period != 0) {
+			// re-check call->deadline to allow for
+			// callouts to re-schedule themselves
+			// using hrt_call_delay()
+			if (call->deadline <= now) {
+				call->deadline = deadline + call->period;
+			}
+
+			hrt_call_enter(call);
+		}
+	}
+	hrt_unlock();
+}
+
diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
index 5b7df7d6df..f878a8d140 100644
--- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
+++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
@@ -56,6 +56,8 @@ long PX4_TICKS_PER_SEC = 1000;
 void usleep(useconds_t usec) {}
 unsigned int sleep(unsigned int sec) { return 0; }
 
+extern void hrt_init(void);
+
 __END_DECLS
 
 extern struct wqueue_s gwork[NWORKERS];
@@ -63,6 +65,14 @@ extern struct wqueue_s gwork[NWORKERS];
 namespace px4
 {
 
+void init_once(void);
+
+void init_once(void)
+{
+	work_queues_init();
+	hrt_init();
+}
+
 void init(int argc, char *argv[], const char *app_name)
 {
 	printf("App name: %s\n", app_name);
diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
index 456a4a442b..e5d2297e9a 100644
--- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
+++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
@@ -33,11 +33,10 @@
  ****************************************************************************/
 
 /**
- * @file px4_qurt_tasks.c
+ * @file px4_linux_tasks.c
  * Implementation of existing task API for Linux
  */
 
-#include <px4_debug.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -54,17 +53,20 @@
 #include <string>
 
 #include <px4_tasks.h>
+#include <px4_log.h>
 #include <hexagon_standalone.h>
 
 #define MAX_CMD_LEN 100
 
-#define PX4_MAX_TASKS 100
+#define PX4_MAX_TASKS 5
+
 struct task_entry
 {
-	pthread_t pid;
+	int pid;
 	std::string name;
 	bool isused;
 	task_entry() : isused(false) {}
+	void *sp;
 };
 
 static task_entry taskmap[PX4_MAX_TASKS];
@@ -73,32 +75,31 @@ typedef struct
 {
 	px4_main_t entry;
 	int argc;
-	char *argv[];
-	// strings are allocated after the 
+	char * argv[];
+	// strings are allocated after  
 } pthdata_t;
 
-static void *entry_adapter ( void *ptr )
+static void entry_adapter ( void *ptr )
 {
-	PX4_DBG("entry_adapter\n");
-	pthdata_t *data;
-	data = (pthdata_t *) ptr;
+	pthdata_t *data = (pthdata_t *) ptr;
+	PX4_DEBUG("entry_adapter %p %p entry %p %d %p\n", ptr, data, data->entry, data->argc, data->argv[0]);
 
+	PX4_DEBUG("data->entry = %p\n", data->entry);
 	data->entry(data->argc, data->argv);
 	free(ptr);
-	PX4_DBG("Before px4_task_exit");
+	PX4_DEBUG("after entry\n");
+	PX4_DEBUG("Before px4_task_exit\n");
 	px4_task_exit(0); 
-	PX4_DBG("After px4_task_exit");
-
-	return NULL;
+	PX4_DEBUG("After px4_task_exit\n");
 } 
 
 void
 px4_systemreset(bool to_bootloader)
 {
-	PX4_WARN("Called px4_system_reset");
+	PX4_DEBUG("Called px4_system_reset\n");
 }
 
-px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[])
+px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const *argv)
 {
 	int rv;
 	int argc = 0;
@@ -108,10 +109,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 	unsigned long structsize;
 	char * p = (char *)argv;
 
-        pthread_t task;
-	pthread_attr_t attr;
-	struct sched_param param;
-
+	PX4_DEBUG("px4_task_spawn_cmd entry = %p %p %s\n", entry, argv, argv[0]);
 	// Calculate argc
 	while (p != (char *)0) {
 		p = argv[argc];
@@ -120,18 +118,24 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 		++argc;
 		len += strlen(p)+1;
 	}
+	PX4_DEBUG("arg %d %p\n", argc, argv);
         structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *);
 	pthdata_t *taskdata;
     
+	PX4_DEBUG("arg %d %p\n", argc, argv);
 	// not safe to pass stack data to the thread creation
 	taskdata = (pthdata_t *)malloc(structsize+len);
+	PX4_DEBUG("arg %d %p\n", argc, argv);
 	offset = ((unsigned long)taskdata)+structsize;
+	PX4_DEBUG("arg %d %p\n", argc, argv);
 
     	taskdata->entry = entry;
 	taskdata->argc = argc;
 
+	PX4_DEBUG("arg %d %p\n", argc, argv);
 	for (i=0; i<argc; i++) {
-		printf("arg %d %s\n", i, argv[i]);
+		PX4_DEBUG("TEST\n");
+		PX4_DEBUG("arg %d %s\n", i, argv[i]);
 		taskdata->argv[i] = (char *)offset;
 		strcpy((char *)offset, argv[i]);
 		offset+=strlen(argv[i])+1;
@@ -139,124 +143,38 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 	// Must add NULL at end of argv
 	taskdata->argv[argc] = (char *)0;
 
-	rv = pthread_attr_init(&attr);
-	if (rv != 0) {
-		PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs");
-		return (rv < 0) ? rv : -rv;
-	}
-#if 0
-	rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
-	if (rv != 0) {
-		PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched");
-		return (rv < 0) ? rv : -rv;
-	}
-	rv = pthread_attr_setschedpolicy(&attr, scheduler);
-	if (rv != 0) {
-		PX4_WARN("px4_task_spawn_cmd: failed to set sched policy");
-		return (rv < 0) ? rv : -rv;
-	}
-
-	param.sched_priority = priority;
-
-	rv = pthread_attr_setschedparam(&attr, &param);
-	if (rv != 0) {
-		PX4_WARN("px4_task_spawn_cmd: failed to set sched param");
-		return (rv < 0) ? rv : -rv;
-	}
-#endif
-
-        rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata);
-	if (rv != 0) {
-
-		if (rv == EPERM) {
-			//printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n");
-        		rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata);
-			if (rv != 0) {
-				PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno);
-				return (rv < 0) ? rv : -rv;
-			}
-		}
-		else {
-			return (rv < 0) ? rv : -rv;
-		}
-	}
-
 	for (i=0; i<PX4_MAX_TASKS; ++i) {
 		if (taskmap[i].isused == false) {
-			taskmap[i].pid = task;
+			taskmap[i].pid = i+1;
 			taskmap[i].name = name;
 			taskmap[i].isused = true;
-			//taskmap[i].sp = malloc(stack_size);
+			taskmap[i].sp = malloc(2048);
 			break;
 		}
 	}
-	if (i>=PX4_MAX_TASKS) {
-		return -ENOSPC;
-	}
-        return i;
+	PX4_DEBUG("taskdata %p entry %p %d %p\n", taskdata, taskdata->entry, taskdata->argc, taskdata->argv[0]);
+	thread_create(entry_adapter, taskmap[i].sp, i+1, (void *) taskdata);
+
+        return i+1;
 }
 
 int px4_task_delete(px4_task_t id)
 {
-	int rv = 0;
-	pthread_t pid;
-	PX4_WARN("Called px4_task_delete");
-
-	if (id < PX4_MAX_TASKS && taskmap[id].isused)
-		pid = taskmap[id].pid;
-	else
-		return -EINVAL;
-
-	// If current thread then exit, otherwise cancel
-        if (pthread_self() == pid) {
-		taskmap[id].isused = false;
-		pthread_exit(0);
-	} else {
-		rv = pthread_cancel(pid);
-	}
-
-	taskmap[id].isused = false;
-
-	return rv;
+	PX4_DEBUG("Called px4_task_delete\n");
+	return -EINVAL;
 }
 
 void px4_task_exit(int ret)
 {
-	int i; 
-	pthread_t pid = pthread_self();
+	thread_stop();
 
-	// Get pthread ID from the opaque ID
-	for (i=0; i<PX4_MAX_TASKS; ++i) {
-		if (taskmap[i].pid == pid) {
-			taskmap[i].isused = false;
-			break;
-		}
-	}
-	if (i>=PX4_MAX_TASKS)  {
-		PX4_ERR("px4_task_exit: self task not found!");
-	}
-	else {
-		PX4_DBG("px4_task_exit: %s", taskmap[i].name.c_str());
-	}
-
-	pthread_exit((void *)(unsigned long)ret);
+	// Free stack
 }
 
 int px4_task_kill(px4_task_t id, int sig)
 {
-	int rv = 0;
-	pthread_t pid;
-	PX4_DBG("Called px4_task_kill %d", sig);
-
-	if (id < PX4_MAX_TASKS && taskmap[id].pid != 0)
-		pid = taskmap[id].pid;
-	else
-		return -EINVAL;
-
-	// If current thread then exit, otherwise cancel
-	rv = pthread_kill(pid, sig);
-
-	return rv;
+	PX4_DEBUG("Called px4_task_kill\n");
+	return -EINVAL;
 }
 
 void px4_show_tasks()
@@ -264,16 +182,17 @@ void px4_show_tasks()
 	int idx;
 	int count = 0;
 
-	PX4_INFO("Active Tasks:");
+	PX4_DEBUG("Active Tasks:\n");
 	for (idx=0; idx < PX4_MAX_TASKS; idx++)
 	{
 		if (taskmap[idx].isused) {
-			PX4_INFO("   %-10s", taskmap[idx].name.c_str());
+			PX4_DEBUG("   %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid);
 			count++;
 		}
 	}
 	if (count == 0)
-		PX4_INFO("   No running tasks");
+		PX4_DEBUG("   No running tasks\n");
+
 }
 
 // STUBS
@@ -282,4 +201,7 @@ extern "C" {
 void hrt_sleep(unsigned long)
 {
 }
+
 }
+int ioctl(int d, int request, unsigned long foo) { return 0; }
+int write(int a, char const*b, int c) { return c; }
diff --git a/src/platforms/qurt/px4_layer/work_lock.h b/src/platforms/qurt/px4_layer/work_lock.h
new file mode 100644
index 0000000000..f77f228b36
--- /dev/null
+++ b/src/platforms/qurt/px4_layer/work_lock.h
@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <semaphore.h>
+#include <stdio.h>
+
+#pragma once
+extern sem_t _work_lock[];
+
+inline void work_lock(int id);
+inline void work_lock(int id)
+{
+	//printf("work_lock %d\n", id);
+	sem_wait(&_work_lock[id]);
+}
+
+inline void work_unlock(int id);
+inline void work_unlock(int id)
+{
+	//printf("work_unlock %d\n", id);
+	sem_post(&_work_lock[id]);
+}
+
diff --git a/src/platforms/qurt/px4_layer/work_queue.c b/src/platforms/qurt/px4_layer/work_queue.c
index 644b25e54b..1746359b63 100644
--- a/src/platforms/qurt/px4_layer/work_queue.c
+++ b/src/platforms/qurt/px4_layer/work_queue.c
@@ -43,7 +43,10 @@
 #include <signal.h>
 #include <stdint.h>
 #include <queue.h>
+#include <stdio.h>
+#include <semaphore.h>
 #include <px4_workqueue.h>
+#include "work_lock.h"
 
 #ifdef CONFIG_SCHED_WORKQUEUE
 
@@ -117,13 +120,13 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_
    * from with task logic or interrupt handlers.
    */
 
-  //flags        = irqsave();
+  work_lock(qid);
   work->qtime  = clock_systimer(); /* Time work queued */
 
   dq_addlast((dq_entry_t *)work, &wqueue->q);
   px4_task_kill(wqueue->pid, SIGALRM);      /* Wake up the worker thread */
 
-  //irqrestore(flags);
+  work_unlock(qid);
   return PX4_OK;
 }
 
diff --git a/src/platforms/qurt/px4_layer/work_thread.c b/src/platforms/qurt/px4_layer/work_thread.c
index cb8cbe9766..3873034189 100644
--- a/src/platforms/qurt/px4_layer/work_thread.c
+++ b/src/platforms/qurt/px4_layer/work_thread.c
@@ -39,12 +39,14 @@
 
 #include <px4_config.h>
 #include <px4_defines.h>
+#include <px4_time.h>
 #include <stdint.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <queue.h>
 #include <px4_workqueue.h>
 #include <drivers/drv_hrt.h>
+#include "work_lock.h"
 
 #ifdef CONFIG_SCHED_WORKQUEUE
 
@@ -66,6 +68,7 @@ struct wqueue_s g_work[NWORKERS];
 /****************************************************************************
  * Private Variables
  ****************************************************************************/
+sem_t _work_lock[NWORKERS];
 
 /****************************************************************************
  * Private Functions
@@ -85,13 +88,12 @@ struct wqueue_s g_work[NWORKERS];
  *
  ****************************************************************************/
 
-static void work_process(FAR struct wqueue_s *wqueue)
+static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
 {
   volatile FAR struct work_s *work;
   worker_t  worker;
-  //irqstate_t flags;
   FAR void *arg;
-  uint32_t elapsed;
+  uint64_t elapsed;
   uint32_t remaining;
   uint32_t next;
 
@@ -100,7 +102,9 @@ static void work_process(FAR struct wqueue_s *wqueue)
    */
 
   next  = CONFIG_SCHED_WORKPERIOD;
-  //flags = irqsave();
+
+  work_lock(lock_id);
+
   work  = (FAR struct work_s *)wqueue->q.head;
   while (work)
     {
@@ -110,8 +114,8 @@ static void work_process(FAR struct wqueue_s *wqueue)
        * zero.  Therefore a delay of zero will always execute immediately.
        */
 
-      elapsed = clock_systimer() - work->qtime;
-      //printf("work_process: elapsed=%d delay=%d\n", elapsed, work->delay);
+      elapsed = USEC2TICK(clock_systimer() - work->qtime);
+      //printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay);
       if (elapsed >= work->delay)
         {
           /* Remove the ready-to-execute work from the list */
@@ -133,9 +137,9 @@ static void work_process(FAR struct wqueue_s *wqueue)
            * performed... we don't have any idea how long that will take!
            */
 
-          //irqrestore(flags);
+          work_unlock(lock_id);
 	  if (!worker) {
-             printf("MESSED UP: worker = 0\n");
+             PX4_WARN("MESSED UP: worker = 0\n");
           }
           else
             worker(arg);
@@ -145,7 +149,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
            * back at the head of the list.
            */
 
-          //flags = irqsave();
+          work_lock(lock_id);
           work  = (FAR struct work_s *)wqueue->q.head;
         }
       else
@@ -155,7 +159,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
            */
 
           /* Here: elapsed < work->delay */
-          remaining = work->delay - elapsed;
+          remaining = USEC_PER_TICK*(work->delay - elapsed);
           if (remaining < next)
             {
               /* Yes.. Then schedule to wake up when the work is ready */
@@ -172,15 +176,40 @@ static void work_process(FAR struct wqueue_s *wqueue)
   /* Wait awhile to check the work list.  We will wait here until either
    * the time elapses or until we are awakened by a signal.
    */
+  work_unlock(lock_id);
 
-  //FIXME - DSPAL doesn't support usleep
-  //usleep(next);
-  //irqrestore(flags);
+  usleep(next);
 }
 
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
+void work_queues_init(void)
+{
+	sem_init(&_work_lock[HPWORK], 0, 1);
+	sem_init(&_work_lock[LPWORK], 0, 1);
+#ifdef CONFIG_SCHED_USRWORK
+	sem_init(&_work_lock[USRWORK], 0, 1);
+#endif
+
+	// Create high priority worker thread
+	g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high",
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_MAX-1,
+			       2000,
+			       work_hpthread,
+			       (char* const*)NULL);
+
+	// Create low priority worker thread
+	g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low",
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_MIN,
+			       2000,
+			       work_lpthread,
+			       (char* const*)NULL);
+
+}
+
 /****************************************************************************
  * Name: work_hpthread, work_lpthread, and work_usrthread
  *
@@ -235,7 +264,7 @@ int work_hpthread(int argc, char *argv[])
        * we process items in the work list.
        */
 
-      work_process(&g_work[HPWORK]);
+      work_process(&g_work[HPWORK], HPWORK);
     }
 
   return PX4_OK; /* To keep some compilers happy */
@@ -262,7 +291,7 @@ int work_lpthread(int argc, char *argv[])
        * we process items in the work list.
        */
 
-      work_process(&g_work[LPWORK]);
+      work_process(&g_work[LPWORK], LPWORK);
     }
 
   return PX4_OK; /* To keep some compilers happy */
@@ -283,7 +312,7 @@ int work_usrthread(int argc, char *argv[])
        * we process items in the work list.
        */
 
-      work_process(&g_work[USRWORK]);
+      work_process(&g_work[USRWORK], USRWORK);
     }
 
   return PX4_OK; /* To keep some compilers happy */
@@ -293,6 +322,7 @@ int work_usrthread(int argc, char *argv[])
 
 uint32_t clock_systimer()
 {
+	//printf("clock_systimer: %0lx\n", hrt_absolute_time());
 	return (0x00000000ffffffff & hrt_absolute_time());
 }
 #endif /* CONFIG_SCHED_WORKQUEUE */
diff --git a/src/platforms/qurt/tests/hello/hello_example.cpp b/src/platforms/qurt/tests/hello/hello_example.cpp
index 6827220888..d0a4de0dc6 100644
--- a/src/platforms/qurt/tests/hello/hello_example.cpp
+++ b/src/platforms/qurt/tests/hello/hello_example.cpp
@@ -1,4 +1,3 @@
-
 /****************************************************************************
  *
  *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
@@ -40,6 +39,7 @@
  */
 
 #include "hello_example.h"
+#include <px4_log.h>
 #include <unistd.h>
 #include <stdio.h>
 
@@ -52,7 +52,7 @@ int HelloExample::main()
 	int i=0;
 	while (!appState.exitRequested() && i<5) {
 
-		printf("  Doing work...\n");
+		PX4_DEBUG("  Doing work...\n");
 		++i;
 	}
 
diff --git a/src/platforms/qurt/tests/hello/hello_main.cpp b/src/platforms/qurt/tests/hello/hello_main.cpp
index 69e8c21ec0..d18c016753 100644
--- a/src/platforms/qurt/tests/hello/hello_main.cpp
+++ b/src/platforms/qurt/tests/hello/hello_main.cpp
@@ -38,6 +38,7 @@
  * @author Mark Charlebois <charlebm@gmail.com>
  */
 #include <px4_middleware.h>
+#include <px4_log.h>
 #include <px4_app.h>
 #include "hello_example.h"
 #include <stdio.h>
@@ -46,10 +47,10 @@ int PX4_MAIN(int argc, char **argv)
 {
 	px4::init(argc, argv, "hello");
 
-	printf("hello\n");
+	PX4_DEBUG("hello\n");
 	HelloExample hello;
 	hello.main();
 
-	printf("goodbye\n");
+	PX4_DEBUG("goodbye\n");
 	return 0;
 }
diff --git a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp
index 71f30cd8dd..7c935e1ab4 100644
--- a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp
+++ b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp
@@ -38,6 +38,7 @@
  * @author Mark Charlebois <mcharleb@gmail.com>
  */
 #include "hello_example.h"
+#include <px4_log.h>
 #include <px4_app.h>
 #include <px4_tasks.h>
 #include <stdio.h>
@@ -52,11 +53,11 @@ extern "C" __EXPORT int hello_main(int argc, char *argv[]);
 
 static void usage()
 {
-	printf("usage: hello {start|stop|status}\n");
+	PX4_DEBUG("usage: hello {start|stop|status}\n");
 }
 int hello_main(int argc, char *argv[])
 {
-	printf("argc = %d %s %s %p\n", argc, argv[0], argv[1], argv[2]);
+	PX4_DEBUG("argc = %d %s %s %p\n", argc, argv[0], argv[1], argv[2]);
 	if (argc < 2) {
 		usage();
 		return 1;
@@ -64,8 +65,9 @@ int hello_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "start")) {
 
+		PX4_DEBUG("Starting\n");
 		if (HelloExample::appState.isRunning()) {
-			printf("already running\n");
+			PX4_DEBUG("already running\n");
 			/* this is not an error */
 			return 0;
 		}
@@ -73,7 +75,7 @@ int hello_main(int argc, char *argv[])
 		daemon_task = px4_task_spawn_cmd("hello",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2000,
+				       16000,
 				       PX4_MAIN,
 				       (char* const*)argv);
 
@@ -87,10 +89,10 @@ int hello_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (HelloExample::appState.isRunning()) {
-			printf("is running\n");
+			PX4_DEBUG("is running\n");
 
 		} else {
-			printf("not started\n");
+			PX4_DEBUG("not started\n");
 		}
 
 		return 0;