QuRT: pthread API now working

The use of std::map and static initialization was an issue.

The code was refactored to not use static initialization.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/platforms/qurt/px4_layer/main.cpp

@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+/**
+ * @file main.cpp
+ * Basic shell to execute builtin "apps" 
+ *
+ * @author Mark Charlebois <charlebm@gmail.com>
+ */
+
+#include <px4_middleware.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <vector>
+#include <string>
+#include <map>
+#include <stdio.h>
+
+using namespace std;
+
+extern void init_app_map(map<string,px4_main_t> &apps);
+extern void list_builtins(map<string,px4_main_t> &apps);
+
+static const char *commands = 
+"hello start"
+#if 0
+"uorb start\n"
+"simulator start -s\n"
+"barosim start\n"
+"adcsim start\n"
+"accelsim start\n"
+"gyrosim start\n"
+"param set CAL_GYRO0_ID 2293760\n"
+"param set CAL_ACC0_ID 1310720\n"
+"param set CAL_ACC1_ID 1376256\n"
+"param set CAL_MAG0_ID 196608\n"
+"rgbled start\n"
+"mavlink start -d /tmp/ttyS0\n"
+"sensors start\n"
+"hil mode_pwm\n"
+"commander start\n"
+"list_devices\n"
+"list_topics\n"
+#endif
+;
+
+static void run_cmd(map<string,px4_main_t> &apps, const vector<string> &appargs) {
+	// command is appargs[0]
+	string command = appargs[0];
+	if (apps.find(command) != apps.end()) {
+		const char *arg[2+1];
+
+		unsigned int i = 0;
+		while (i < appargs.size() && appargs[i].c_str()[0] != '\0') {
+			arg[i] = (char *)appargs[i].c_str();
+			printf("  arg = '%s'\n", arg[i]);
+			++i;
+		}
+		arg[i] = (char *)0;
+		apps[command](i,(char **)arg);
+	}
+	else
+	{
+		list_builtins(apps);
+	}
+}
+
+void eat_whitespace(const char *&b, int &i)
+{
+	// Eat whitespace
+	while (b[i] == ' ' || b[i] == '\t') { ++i; }
+	b = &b[i];
+	i=0;
+}
+
+static void process_commands(map<string,px4_main_t> &apps, const char *cmds)
+{
+	vector<string> appargs;
+	int i=0;
+	const char *b = cmds;
+	bool found_first_char = false;
+	char arg[20];
+
+	// Eat leading whitespace
+	eat_whitespace(b, i);
+
+	for(;;) {
+		// End of command line
+		if (b[i] == '\n' || b[i] == '\0') {
+			strncpy(arg, b, i);
+			arg[i] = '\0';
+			appargs.push_back(arg);
+
+			// If we have a command to run
+			if (appargs.size() > 0) {
+				run_cmd(apps, appargs);
+			}
+			appargs.clear();
+			if (b[i] == '\n') {
+				eat_whitespace(b, ++i);
+				continue;
+			}
+			else {
+				break;
+			}
+		}
+		// End of arg
+		else if (b[i] == ' ') {
+			strncpy(arg, b, i);
+			arg[i] = '\0';
+			appargs.push_back(arg);
+			eat_whitespace(b, ++i);
+			continue;
+		}
+		printf("ch %c\n", b[i]);
+		++i;
+	}
+}
+
+namespace px4 {
+extern void init_once(void);
+};
+
+int main(int argc, char **argv)
+{
+	printf("In main\n");
+	map<string,px4_main_t> apps;
+	init_app_map(apps);
+	px4::init_once();
+	px4::init(argc, argv, "mainapp");
+	process_commands(apps, commands);
+	for (;;) {}
+}
+

src/platforms/qurt/px4_layer/module.mk

@@ -37,13 +37,6 @@
 
 SRCDIR=$(dir $(MODULE_MK))
 
-apps.h: $(PX4_BASE)/Tools/qurt_apps.py
-	$(PX4_BASE)/Tools/qurt_apps.py > $@
-
-# Force creation of apps.h
-main_.cpp: $(SRCDIR)/../main.cpp apps.h
-	cp $(SRCDIR)/../main.cpp $@
-
 SRCS		 = 	\
 			px4_qurt_impl.cpp \
 			px4_qurt_tasks.cpp  \
@@ -59,6 +52,6 @@ SRCS		 = 	\
 			sq_remfirst.c \
 			sq_addafter.c \
 			dq_rem.c \
-			main_.cpp 
+			main.cpp 
 
 MAXOPTIMIZATION	 = -Os

src/platforms/qurt/px4_layer/px4_qurt_impl.cpp

@@ -38,6 +38,7 @@
  */
 
 #include <px4_defines.h>
+#include <px4_tasks.h>
 #include <px4_middleware.h>
 #include <px4_workqueue.h>
 #include <dataman/dataman.h>
@@ -47,19 +48,28 @@
 #include <errno.h>
 #include <unistd.h>
 #include <semaphore.h>
+#include <qurt.h>
 #include "systemlib/param/param.h"
-#include <hexagon_standalone.h>
 
 __BEGIN_DECLS
 
 // FIXME - sysconf(_SC_CLK_TCK) not supported
 long PX4_TICKS_PER_SEC = 1000;
 
-void usleep(useconds_t usec) {}
-unsigned int sleep(unsigned int sec) { return 0; }
+void usleep(useconds_t usec) {
+	qurt_timer_sleep(usec);
+}
+
+unsigned int sleep(unsigned int sec) 
+{ 
+	for (unsigned int i=0; i< sec; i++)
+		qurt_timer_sleep(1000000);
+	return 0; 
+}
 
 extern void hrt_init(void);
 
+#if 0
 void qurt_log(const char *fmt, ...)
 {
 	va_list	args;
@@ -67,11 +77,15 @@ void qurt_log(const char *fmt, ...)
 	printf(fmt, args);
 	printf("n");
 }
+#endif
+
+extern int _posix_init(void);
 
 __END_DECLS
 
 extern struct wqueue_s gwork[NWORKERS];
 
+
 namespace px4
 {
 
@@ -79,25 +93,11 @@ void init_once(void);
 
 void init_once(void)
 {
+	// Required for QuRT
+	_posix_init();
+
 	work_queues_init();
 	hrt_init();
-	
-	// Create high priority worker thread
-	g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high",
-			       SCHED_DEFAULT,
-			       SCHED_PRIORITY_MAX,
-			       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);
-
 }
 
 void init(int argc, char *argv[], const char *app_name)
@@ -141,3 +141,12 @@ size_t strnlen(const char *s, size_t maxlen)
 	return i;
 }
 
+int ioctl(int a, int b, unsigned long c)
+{
+	return -1;
+}
+
+int write(int a, char const* b, int c)
+{
+	return -1;
+}

src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp

@@ -33,10 +33,11 @@
  ****************************************************************************/
 
 /**
- * @file px4_linux_tasks.c
+ * @file px4_posix_tasks.c
  * Implementation of existing task API for Linux
  */
 
+#include <px4_log.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -53,20 +54,16 @@
 #include <string>
 
 #include <px4_tasks.h>
-#include <px4_log.h>
-#include <hexagon_standalone.h>
 
 #define MAX_CMD_LEN 100
 
-#define PX4_MAX_TASKS 5
-
+#define PX4_MAX_TASKS 100
 struct task_entry
 {
-	int pid;
+	pthread_t pid;
 	std::string name;
 	bool isused;
 	task_entry() : isused(false) {}
-	void *sp;
 };
 
 static task_entry taskmap[PX4_MAX_TASKS];
@@ -75,31 +72,32 @@ typedef struct
 {
 	px4_main_t entry;
 	int argc;
-	char * argv[];
-	// strings are allocated after  
+	char *argv[];
+	// strings are allocated after the 
 } pthdata_t;
 
-static void entry_adapter ( void *ptr )
+static void *entry_adapter ( void *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]);
+	pthdata_t *data;            
+	data = (pthdata_t *) ptr;  
 
-	PX4_DEBUG("data->entry = %p\n", data->entry);
+	PX4_DBG("entry_adapter");
 	data->entry(data->argc, data->argv);
 	free(ptr);
-	PX4_DEBUG("after entry\n");
-	PX4_DEBUG("Before px4_task_exit\n");
+	PX4_DBG("Before px4_task_exit");
 	px4_task_exit(0); 
-	PX4_DEBUG("After px4_task_exit\n");
+	PX4_DBG("After px4_task_exit");
+
+	return NULL;
 } 
 
 void
 px4_systemreset(bool to_bootloader)
 {
-	PX4_DEBUG("Called px4_system_reset\n");
+	PX4_WARN("Called px4_system_reset");
 }
 
-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;
@@ -109,7 +107,11 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 	unsigned long structsize;
 	char * p = (char *)argv;
 
-	PX4_DEBUG("px4_task_spawn_cmd entry = %p %p %s\n", entry, argv, argv[0]);
+	printf("Creating %s\n", name);
+        pthread_t task;
+	pthread_attr_t attr;
+	struct sched_param param;
+
 	// Calculate argc
 	while (p != (char *)0) {
 		p = argv[argc];
@@ -118,24 +120,18 @@ 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++) {
-		PX4_DEBUG("TEST\n");
-		PX4_DEBUG("arg %d %s\n", i, argv[i]);
+		printf("arg %d %s\n", i, argv[i]);
 		taskdata->argv[i] = (char *)offset;
 		strcpy((char *)offset, argv[i]);
 		offset+=strlen(argv[i])+1;
@@ -143,38 +139,123 @@ 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;
+	}
+#endif
+
+	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;
+	}
+
+        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 = i+1;
+			taskmap[i].pid = task;
 			taskmap[i].name = name;
 			taskmap[i].isused = true;
-			taskmap[i].sp = malloc(2048);
 			break;
 		}
 	}
-	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;
+	if (i>=PX4_MAX_TASKS) {
+		return -ENOSPC;
+	}
+        return i;
 }
 
 int px4_task_delete(px4_task_t id)
 {
-	PX4_DEBUG("Called px4_task_delete\n");
-	return -EINVAL;
+	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;
 }
 
 void px4_task_exit(int ret)
 {
-	thread_stop();
+	int i; 
+	pthread_t pid = pthread_self();
 
-	// Free stack
+	// 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);
 }
 
 int px4_task_kill(px4_task_t id, int sig)
 {
-	PX4_DEBUG("Called px4_task_kill\n");
-	return -EINVAL;
+	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;
 }
 
 void px4_show_tasks()
@@ -182,26 +263,15 @@ void px4_show_tasks()
 	int idx;
 	int count = 0;
 
-	PX4_DEBUG("Active Tasks:\n");
+	PX4_INFO("Active Tasks:");
 	for (idx=0; idx < PX4_MAX_TASKS; idx++)
 	{
 		if (taskmap[idx].isused) {
-			PX4_DEBUG("   %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid);
+			PX4_INFO("   %-10s %u", taskmap[idx].name.c_str(), taskmap[idx].pid);
 			count++;
 		}
 	}
 	if (count == 0)
-		PX4_DEBUG("   No running tasks\n");
+		PX4_INFO("   No running tasks");
 
 }
-
-// STUBS
-
-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; }