Linux: Added work queue support and unit test

PX4 uses NuttX data structures throughout so those data structures
were preserved and used to implement high and low priority queues.

A unit test for the work queues was added.

The polling rate of the queues are set in px4_config.h in
CONFIG_SCHED_WORKPERIOD. The units are milliseconds.

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

linux-configs/linuxtest/init/rc.S

@@ -0,0 +1,4 @@
+uorb start
+mavlink start
+blink start
+blink systemstate

makefiles/linux/config_linux_default.mk

@@ -63,4 +63,5 @@ MODULES		+= platforms/linux/px4_layer
 #MODULES		+= platforms/linux/tests/hello
 #MODULES		+= platforms/linux/tests/vcdev_test
 #MODULES		+= platforms/linux/tests/hrt_test
+#MODULES		+= platforms/linux/tests/wqueue
 

src/platforms/linux/px4_layer/px4_linux_impl.cpp

@@ -42,6 +42,8 @@
 #include <px4_workqueue.h>
 #include <stdint.h>
 #include <stdio.h>
+#include <signal.h>
+#include <errno.h>
 #include <unistd.h>
 #include "systemlib/param/param.h"
 
@@ -58,11 +60,28 @@ __END_DECLS
 
 extern struct wqueue_s gwork[NWORKERS];
 
+void sighandler(int sig)
+{
+	printf("Received sig %d\n", sig);
+}
+
 namespace px4
 {
 
 void init(int argc, char *argv[], const char *app_name)
 {
+	int ret;
+	struct sigaction actions;
+
+	memset(&actions, 0, sizeof(actions));
+	sigemptyset(&actions.sa_mask);
+	actions.sa_flags = 0;
+	actions.sa_handler = sighandler;
+	ret = sigaction(SIGUSR2,&actions,NULL);
+
+	if (ret < 0) {
+		printf("sigaction failed: %d\n", errno);
+	}
 	printf("App name: %s\n", app_name);
 
 	// Create high priority worker thread

src/platforms/linux/px4_layer/px4_linux_tasks.c

@@ -153,16 +153,21 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 
         rv = pthread_create (&task, &attr, (void *)&entry_adapter, (void *) taskdata);
 	if (rv != 0) {
-		printf("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno);
 
 		if (rv == EPERM) {
-			printf("WARNING: INSUFFICIENT PRIVILEGE TO RUN REALTIME THREADS\n");
+			printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n");
         		rv = pthread_create (&task, NULL, (void *)&entry_adapter, (void *) taskdata);
+			if (rv != 0) {
+				printf("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno);
+				return (rv < 0) ? rv : -rv;
+			}
+		}
+		else {
+			return (rv < 0) ? rv : -rv;
 		}
-		return (rv < 0) ? rv : -rv;
 	}
 
-	//printf("pthread_create task=%d rv=%d\n",(int)task, rv);
+	printf("pthread_create task=%lu rv=%d\n",(unsigned long)task, rv);
 
 	for (i=0; i<PX4_MAX_TASKS; ++i) {
 		// FIXME - precludes pthread task to have an ID of 0
@@ -230,3 +235,19 @@ void px4_killall(void)
 	}
 }
 
+int px4_task_kill(px4_task_t id, int sig)
+{
+	int rv = 0;
+	pthread_t pid;
+	//printf("Called px4_task_delete\n");
+
+	if (id < PX4_MAX_TASKS && taskmap[id] != 0)
+		pid = taskmap[id];
+	else
+		return -EINVAL;
+
+	// If current thread then exit, otherwise cancel
+	rv = pthread_kill(pid, sig);
+
+	return rv;
+}

src/platforms/linux/px4_layer/work_queue.c

@@ -121,7 +121,7 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_
   work->qtime  = clock_systimer(); /* Time work queued */
 
   dq_addlast((dq_entry_t *)work, &wqueue->q);
-  pthread_kill(wqueue->pid, SIGUSR1);      /* Wake up the worker thread */
+  px4_task_kill(wqueue->pid, SIGCONT);      /* Wake up the worker thread */
 
   //irqrestore(flags);
   return PX4_OK;

src/platforms/linux/px4_layer/work_thread.c

@@ -40,9 +40,11 @@
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <stdint.h>
+#include <stdio.h>
 #include <unistd.h>
 #include <queue.h>
 #include <px4_workqueue.h>
+#include <drivers/drv_hrt.h>
 
 #ifdef CONFIG_SCHED_WORKQUEUE
 
@@ -97,8 +99,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
    * we process items in the work list.
    */
 
-  //next  = CONFIG_SCHED_WORKPERIOD / USEC_PER_TICK;
+  next  = CONFIG_SCHED_WORKPERIOD;
-  next  = 100;
   //flags = irqsave();
   work  = (FAR struct work_s *)wqueue->q.head;
   while (work)
@@ -110,6 +111,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
        */
 
       elapsed = clock_systimer() - work->qtime;
+      //printf("work_process: elapsed=%d delay=%d\n", elapsed, work->delay);
       if (elapsed >= work->delay)
         {
           /* Remove the ready-to-execute work from the list */
@@ -132,7 +134,11 @@ static void work_process(FAR struct wqueue_s *wqueue)
            */
 
           //irqrestore(flags);
-          worker(arg);
+	  if (!worker) {
+             printf("MESSED UP: worker = 0\n");
+          }
+          else
+            worker(arg);
 
           /* Now, unfortunately, since we re-enabled interrupts we don't
            * know the state of the work list and we will have to start
@@ -167,7 +173,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
    * the time elapses or until we are awakened by a signal.
    */
 
-  usleep(next * USEC_PER_TICK);
+  usleep(next);
   //irqrestore(flags);
 }
 
@@ -284,8 +290,8 @@ int work_usrthread(int argc, char *argv[])
 
 #endif /* CONFIG_SCHED_USRWORK */
 
-int clock_systimer()
+uint32_t clock_systimer()
 {
-	return 1;
+	return (0x00000000ffffffff & hrt_absolute_time());
 }
 #endif /* CONFIG_SCHED_WORKQUEUE */

src/platforms/linux/tests/wqueue/module.mk

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. 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.
+#
+############################################################################
+
+#
+# Publisher Example Application
+#
+
+MODULE_COMMAND	= wqueue_test
+
+SRCS		= wqueue_main.cpp \
+		  wqueue_start_linux.cpp \
+		  wqueue_test.cpp
+

src/platforms/linux/tests/wqueue/wqueue_main.cpp

@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   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 wqueue_main.cpp
+ * Example for Linux
+ *
+ * @author Mark Charlebois <charlebm@gmail.com>
+ */
+#include <px4_middleware.h>
+#include <px4_app.h>
+#include "wqueue_test.h"
+#include <stdio.h>
+
+int PX4_MAIN(int argc, char **argv)
+{
+	px4::init(argc, argv, "wqueue_test");
+
+	printf("wqueue hello\n");
+	WQueueTest wq;
+	wq.main();
+
+	printf("goodbye\n");
+	return 0;
+}

src/platforms/linux/tests/wqueue/wqueue_start_linux.cpp

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   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 wqueue_start_linux.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Mark Charlebois <mcharleb@gmail.com>
+ */
+#include "wqueue_test.h"
+#include <px4_app.h>
+#include <px4_tasks.h>
+#include <stdio.h>
+#include <string.h>
+#include <sched.h>
+
+static int daemon_task;             /* Handle of deamon task / thread */
+
+//using namespace px4;
+
+extern "C" __EXPORT int wqueue_test_main(int argc, char *argv[]);
+int wqueue_test_main(int argc, char *argv[])
+{
+	
+	if (argc < 2) {
+		printf("usage: wqueue_test {start|stop|status}\n");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (WQueueTest::appState.isRunning()) {
+			printf("already running\n");
+			/* this is not an error */
+			return 0;
+		}
+
+		daemon_task = px4_task_spawn_cmd("wqueue",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       PX4_MAIN,
+				       (argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		WQueueTest::appState.requestExit();
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (WQueueTest::appState.isRunning()) {
+			printf("is running\n");
+
+		} else {
+			printf("not started\n");
+		}
+
+		return 0;
+	}
+
+	printf("usage: wqueue_test {start|stop|status}\n");
+	return 1;
+}

src/platforms/linux/tests/wqueue/wqueue_test.cpp

@@ -0,0 +1,96 @@
+
+/****************************************************************************
+ *
+ *   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 wqueue_example.cpp
+ * Example for Linux
+ *
+ * @author Mark Charlebois <charlebm@gmail.com>
+ */
+
+#include <px4_workqueue.h>
+#include "wqueue_test.h"
+#include <unistd.h>
+#include <stdio.h>
+
+px4::AppState WQueueTest::appState;
+
+void WQueueTest::hp_worker_cb(void *p)
+{
+	WQueueTest *wqep = (WQueueTest *)p;
+
+	wqep->do_hp_work();
+}
+
+void WQueueTest::lp_worker_cb(void *p)
+{
+	WQueueTest *wqep = (WQueueTest *)p;
+
+	wqep->do_lp_work();
+}
+
+void WQueueTest::do_lp_work()
+{
+	printf("done lp work\n");
+	_lpwork_done = true;
+}
+
+void WQueueTest::do_hp_work()
+{
+	printf("done hp work\n");
+	_hpwork_done = true;
+}
+
+int WQueueTest::main()
+{
+	appState.setRunning(true);
+
+	//Put work on HP work queue
+	work_queue(HPWORK, &_hpwork, (worker_t)&hp_worker_cb, this, 1);
+
+
+	//Put work on LP work queue
+	work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1);
+
+
+	// Wait for work to finsh
+	while (!appState.exitRequested() && !(_hpwork_done && _lpwork_done)) {
+		printf("  Sleeping...\n");
+		sleep(2);
+
+		printf("  Waiting on work...\n");
+	}
+
+	return 0;
+}

src/platforms/linux/tests/wqueue/wqueue_test.h

@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   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 wqueue_test.h
+ * Example app for Linux
+ *
+ * @author Mark Charlebois <charlebm@gmail.com>
+ */
+#pragma once
+
+#include <px4_app.h>
+#include <px4_workqueue.h>
+#include <string.h>
+
+class WQueueTest {
+public:
+	WQueueTest() : 
+		_lpwork_done(false), 
+		_hpwork_done(false) 
+	{
+		memset(&_lpwork, 0, sizeof(_lpwork));
+		memset(&_hpwork, 0, sizeof(_hpwork));
+	};
+
+	~WQueueTest() {};
+
+	int main();
+
+	static px4::AppState appState; /* track requests to terminate app */
+private:
+	static void hp_worker_cb(void *p);
+	static void lp_worker_cb(void *p);
+
+	void do_lp_work(void);
+	void do_hp_work(void);
+
+	bool _lpwork_done;
+	bool _hpwork_done;
+	work_s _lpwork;
+	work_s _hpwork;
+};

src/platforms/px4_config.h

@@ -49,6 +49,9 @@
 #define CONFIG_SCHED_LPWORK 1
 #define CONFIG_ARCH_BOARD_LINUXTEST 1
 
+/** time in ms between checks for work in work queues **/
+#define CONFIG_SCHED_WORKPERIOD 10
+
 #define px4_errx(x, ...) errx(x, __VA_ARGS__)
 
 #endif

src/platforms/px4_tasks.h

@@ -86,7 +86,13 @@ __EXPORT px4_task_t px4_task_spawn_cmd(const char *name,
 			px4_main_t entry,
 			char * const argv[]);
 
+/** Deletes a task - does not do resource cleanup **/
 __EXPORT int px4_task_delete(px4_task_t pid);
+
+/** Send a signal to a task **/
+__EXPORT int px4_task_kill(px4_task_t pid, int sig);
+
+/** Exit current task with return value **/
 __EXPORT void px4_task_exit(int ret);
 
 __END_DECLS

src/platforms/px4_workqueue.h

@@ -123,7 +123,7 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_
 
 int work_cancel(int qid, struct work_s *work);
 
-int clock_systimer(void);
+uint32_t clock_systimer(void);
 
 int work_hpthread(int argc, char *argv[]);
 int work_lpthread(int argc, char *argv[]);