From 18304a2a0dff8f6b3499861ece27ae98615a367c Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Thu, 11 Jun 2015 11:36:58 -0700
Subject: [PATCH] POSIX: Fixed px4_getpid() calls from shell context

When px4_getpid() was called from the shell, there was no opaque
thread ID to return. Added a special thread ID for the shell
context. This ID only works for px4_getpid() and cannot be used
for other px4_task_*() calls.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 src/platforms/posix/px4_layer/px4_posix_impl.cpp  |  4 ++++
 src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 13 ++++++++++---
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp
index 0d926f0520..243716abe6 100644
--- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp
@@ -48,6 +48,8 @@
 #include "systemlib/param/param.h"
 #include "hrt_work.h"
 
+extern pthread_t _shell_task_id;
+
 __BEGIN_DECLS
 
 long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK);
@@ -63,6 +65,8 @@ void init_once(void);
 
 void init_once(void)
 {
+	_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/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
index eb442e708b..ad24d32bc6 100644
--- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
+++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp
@@ -58,6 +58,10 @@
 #define MAX_CMD_LEN 100
 
 #define PX4_MAX_TASKS 100
+#define SHELL_TASK_ID (PX4_MAX_TASKS+1)
+
+pthread_t _shell_task_id = 0;
+
 struct task_entry
 {
 	pthread_t pid;
@@ -243,7 +247,7 @@ int px4_task_kill(px4_task_t id, int sig)
 	pthread_t pid;
 	PX4_DEBUG("Called px4_task_kill %d", sig);
 
-	if (id < PX4_MAX_TASKS && taskmap[id].pid != 0)
+	if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0)
 		pid = taskmap[id].pid;
 	else
 		return -EINVAL;
@@ -278,13 +282,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;
 }