src/platforms/common: move to platforms/common

Script to update include paths:
for i in $(grep -rl 'include <px4_work_queue' src platforms); do sed -i 's/#include <px4_work_queue/#include <px4_platform_common\/px4_work_queue/' $i; done

platforms/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2019 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.
+#
+############################################################################
+
+add_subdirectory(common)
+
+

platforms/common/CMakeLists.txt

@@ -0,0 +1,56 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+
+set(SRCS)
+
+if (NOT "${PX4_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_BOARD}" MATCHES "io-v2")
+	list(APPEND SRCS
+		px4_log.c
+		)
+endif()
+
+add_library(px4_platform
+	module.cpp
+	px4_getopt.c
+	px4_cli.cpp
+	shutdown.cpp
+	${SRCS}
+	)
+add_dependencies(px4_platform prebuild_targets)
+
+if (NOT "${PX4_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_BOARD}" MATCHES "io-v2")
+	target_link_libraries(px4_platform PRIVATE modules__uORB) # px4_log awkward dependency with uORB, TODO: orb should part of the platform layer
+endif()
+
+add_subdirectory(px4_work_queue)
+add_subdirectory(work_queue)

platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp

@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "WorkItem.hpp"
+
+#include <drivers/drv_hrt.h>
+
+namespace px4
+{
+
+class ScheduledWorkItem : public WorkItem
+{
+public:
+
+	ScheduledWorkItem(const wq_config_t &config) : WorkItem(config) {}
+	virtual ~ScheduledWorkItem() override;
+
+	/**
+	 * Schedule next run with a delay in microseconds.
+	 *
+	 * @param delay_us		The delay in microseconds.
+	 */
+	void ScheduleDelayed(uint32_t delay_us);
+
+	/**
+	 * Schedule repeating run with optional delay.
+	 *
+	 * @param interval_us		The interval in microseconds.
+	 * @param delay_us			The delay (optional) in microseconds.
+	 */
+	void ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us = 0);
+
+	/**
+	 * Clear any scheduled work.
+	 */
+	void ScheduleClear();
+
+	virtual void Run() override = 0;
+
+private:
+
+	static void	schedule_trampoline(void *arg);
+
+	hrt_call	_call{};
+
+};
+
+} // namespace px4

platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+
+#include "WorkQueueManager.hpp"
+#include "WorkQueue.hpp"
+
+#include <containers/IntrusiveQueue.hpp>
+#include <px4_defines.h>
+#include <drivers/drv_hrt.h>
+
+namespace px4
+{
+
+class WorkItem : public IntrusiveQueueNode<WorkItem *>
+{
+public:
+
+	explicit WorkItem(const wq_config_t &config);
+	WorkItem() = delete;
+
+	virtual ~WorkItem();
+
+	inline void ScheduleNow() { if (_wq != nullptr) _wq->Add(this); }
+
+	virtual void Run() = 0;
+
+protected:
+
+	/**
+	 * Initialize WorkItem given a WorkQueue config. This call
+	 * can also be used to switch to a different WorkQueue.
+	 * NOTE: Caller is responsible for synchronization.
+	 *
+	 * @param config The WorkQueue configuration (see WorkQueueManager.hpp).
+	 * @return true if initialization was successful
+	 */
+	bool Init(const wq_config_t &config);
+	void Deinit();
+
+private:
+
+	WorkQueue *_wq{nullptr};
+
+};
+
+} // namespace px4

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp

@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "WorkQueueManager.hpp"
+
+#include <containers/List.hpp>
+#include <containers/IntrusiveQueue.hpp>
+#include <px4_atomic.h>
+#include <px4_defines.h>
+#include <px4_sem.h>
+#include <px4_tasks.h>
+
+namespace px4
+{
+
+class WorkItem;
+
+class WorkQueue : public ListNode<WorkQueue *>
+{
+public:
+	explicit WorkQueue(const wq_config_t &wq_config);
+	WorkQueue() = delete;
+
+	~WorkQueue();
+
+	const char *get_name() { return _config.name; }
+
+	void Add(WorkItem *item);
+	void Remove(WorkItem *item);
+
+	void Clear();
+
+	void Run();
+
+	void request_stop() { _should_exit.store(true); }
+
+	void print_status();
+
+private:
+
+	bool should_exit() const { return _should_exit.load(); }
+
+#ifdef __PX4_NUTTX
+	// In NuttX work can be enqueued from an ISR
+	void work_lock() { _flags = enter_critical_section(); }
+	void work_unlock() { leave_critical_section(_flags); }
+	irqstate_t _flags;
+#else
+	void work_lock() { px4_sem_wait(&_qlock); }
+	void work_unlock() { px4_sem_post(&_qlock); }
+	px4_sem_t _qlock;
+#endif
+
+	IntrusiveQueue<WorkItem *>	_q;
+	px4_sem_t		_process_lock;
+
+	px4::atomic_bool	_should_exit{false};
+	const wq_config_t	&_config;
+
+};
+
+} // namespace px4

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdint.h>
+
+namespace px4
+{
+
+class WorkQueue; // forward declaration
+
+struct wq_config_t {
+	const char *name;
+	uint16_t stacksize;
+	int8_t relative_priority; // relative to max
+};
+
+namespace wq_configurations
+{
+static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1600, 0}; // PX4 inner loop highest priority
+
+static constexpr wq_config_t SPI1{"wq:SPI1", 1400, -1};
+static constexpr wq_config_t SPI2{"wq:SPI2", 1400, -2};
+static constexpr wq_config_t SPI3{"wq:SPI3", 1400, -3};
+static constexpr wq_config_t SPI4{"wq:SPI4", 1400, -4};
+static constexpr wq_config_t SPI5{"wq:SPI5", 1400, -5};
+static constexpr wq_config_t SPI6{"wq:SPI6", 1400, -6};
+
+static constexpr wq_config_t I2C1{"wq:I2C1", 1250, -7};
+static constexpr wq_config_t I2C2{"wq:I2C2", 1250, -8};
+static constexpr wq_config_t I2C3{"wq:I2C3", 1250, -9};
+static constexpr wq_config_t I2C4{"wq:I2C4", 1250, -10};
+
+static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 2000, -11}; // PX4 att/pos controllers, highest priority after sensors
+
+static constexpr wq_config_t hp_default{"wq:hp_default", 1500, -12};
+static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50};
+
+static constexpr wq_config_t test1{"wq:test1", 800, 0};
+static constexpr wq_config_t test2{"wq:test2", 800, 0};
+
+} // namespace wq_configurations
+
+/**
+ * Start the work queue manager task.
+ */
+int WorkQueueManagerStart();
+
+/**
+ * Stop the work queue manager task.
+ */
+int WorkQueueManagerStop();
+
+/**
+ * Create (or find) a work queue with a particular configuration.
+ *
+ * @param new_wq		The work queue configuration (see WorkQueueManager.hpp).
+ * @return		A pointer to the WorkQueue, or nullptr on failure.
+ */
+WorkQueue *WorkQueueFindOrCreate(const wq_config_t &new_wq);
+
+/**
+ * Map a PX4 driver device id to a work queue (by sensor bus).
+ *
+ * @param device_id		The PX4 driver's device id.
+ * @return		A work queue configuration.
+ */
+const wq_config_t &device_bus_to_wq(uint32_t device_id);
+
+
+} // namespace px4

platforms/common/module.cpp

@@ -0,0 +1,167 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2017 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file module.cpp
+ * Implementation of the API declared in px4_module.h.
+ */
+
+#ifndef MODULE_NAME
+#define MODULE_NAME "module"
+#endif
+
+#include <px4_module.h>
+#include <px4_defines.h>
+#include <px4_log.h>
+
+pthread_mutex_t px4_modules_mutex = PTHREAD_MUTEX_INITIALIZER;
+
+#ifndef __PX4_NUTTX
+
+void PRINT_MODULE_DESCRIPTION(const char *description)
+{
+	// TODO: the output could be improved by:
+	// - mark titles in bold (lines starting with ##)
+	// - highlight commands (lines starting with $, or `cmd`)
+	PX4_INFO_RAW("%s\n\n", description);
+}
+
+#endif /* __PX4_NUTTX */
+
+void PRINT_MODULE_USAGE_NAME(const char *executable_name, const char *category)
+{
+	PX4_INFO_RAW("Usage: %s <command> [arguments...]\n", executable_name);
+	PX4_INFO_RAW(" Commands:\n");
+}
+
+void PRINT_MODULE_USAGE_SUBCATEGORY(const char *subcategory)
+{
+	(void)subcategory;
+}
+
+void PRINT_MODULE_USAGE_NAME_SIMPLE(const char *executable_name, const char *category)
+{
+	PX4_INFO_RAW("Usage: %s [arguments...]\n", executable_name);
+}
+
+void PRINT_MODULE_USAGE_COMMAND_DESCR(const char *name, const char *description)
+{
+	if (description) {
+		PX4_INFO_RAW("\n   %-13s %s\n", name, description);
+
+	} else {
+		PX4_INFO_RAW("\n   %s\n", name);
+	}
+}
+
+void PRINT_MODULE_USAGE_PARAM_COMMENT(const char *comment)
+{
+	PX4_INFO_RAW("\n %s\n", comment);
+}
+
+void PRINT_MODULE_USAGE_PARAM_INT(char option_char, int default_val, int min_val, int max_val,
+				  const char *description, bool is_optional)
+{
+	if (is_optional) {
+		PX4_INFO_RAW("     [-%c <val>]  %s\n", option_char, description);
+
+		if (default_val != -1) {
+			PX4_INFO_RAW("                 default: %i\n", default_val);
+		}
+
+	} else {
+		PX4_INFO_RAW("     -%c <val>    %s\n", option_char, description);
+	}
+}
+
+void PRINT_MODULE_USAGE_PARAM_FLOAT(char option_char, float default_val, float min_val, float max_val,
+				    const char *description, bool is_optional)
+{
+	if (is_optional) {
+		PX4_INFO_RAW("     [-%c <val>]  %s\n", option_char, description);
+
+		if (PX4_ISFINITE(default_val)) {
+			PX4_INFO_RAW("                 default: %.1f\n", (double)default_val);
+		}
+
+	} else {
+		PX4_INFO_RAW("     -%c <val>    %s\n", option_char, description);
+	}
+}
+
+void PRINT_MODULE_USAGE_PARAM_FLAG(char option_char, const char *description, bool is_optional)
+{
+	if (is_optional) {
+		PX4_INFO_RAW("     [-%c]        %s\n", option_char, description);
+
+	} else {
+		PX4_INFO_RAW("     -%c          %s\n", option_char, description);
+	}
+}
+
+void PRINT_MODULE_USAGE_PARAM_STRING(char option_char, const char *default_val, const char *values,
+				     const char *description, bool is_optional)
+{
+	if (is_optional) {
+		PX4_INFO_RAW("     [-%c <val>]  %s\n", option_char, description);
+
+	} else {
+		PX4_INFO_RAW("     -%c <val>    %s\n", option_char, description);
+	}
+
+	if (values) {
+		if (default_val) {
+			PX4_INFO_RAW("                 values: %s, default: %s\n", values, default_val);
+
+		} else {
+			PX4_INFO_RAW("                 values: %s\n", values);
+		}
+
+	} else {
+		if (default_val) {
+			PX4_INFO_RAW("                 default: %s\n", default_val);
+		}
+	}
+}
+
+
+void PRINT_MODULE_USAGE_ARG(const char *values, const char *description, bool is_optional)
+{
+	if (is_optional) {
+		PX4_INFO_RAW("     [%-9s] %s\n", values, description);
+
+	} else {
+		PX4_INFO_RAW("     %-11s %s\n", values, description);
+	}
+}
+

platforms/common/px4_cli.cpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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.
+ *
+ ****************************************************************************/
+
+#include <parameters/param.h>
+#include <px4_cli.h>
+
+#ifndef MODULE_NAME
+#define MODULE_NAME "cli"
+#endif
+
+#include <px4_log.h>
+
+#include <cstring>
+#include <errno.h>
+#include <cstdlib>
+
+int px4_get_parameter_value(const char *option, int &value)
+{
+	value = 0;
+
+	/* check if this is a param name */
+	if (strncmp("p:", option, 2) == 0) {
+
+		const char *param_name = option + 2;
+
+		/* user wants to use a param name */
+		param_t param_handle = param_find(param_name);
+
+		if (param_handle != PARAM_INVALID) {
+
+			if (param_type(param_handle) != PARAM_TYPE_INT32) {
+				return -EINVAL;
+			}
+
+			int32_t pwm_parm;
+			int ret = param_get(param_handle, &pwm_parm);
+
+			if (ret != 0) {
+				return ret;
+			}
+
+			value = pwm_parm;
+
+		} else {
+			PX4_ERR("param name '%s' not found", param_name);
+			return -ENXIO;
+		}
+
+	} else {
+		char *ep;
+		value = strtol(option, &ep, 0);
+
+		if (*ep != '\0') {
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}

platforms/common/px4_getopt.c

@@ -0,0 +1,187 @@
+/****************************************************************************
+ *
+ * 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 px4_getopt.cpp
+ * Minimal, thread safe version of getopt
+ */
+
+#include <px4_getopt.h>
+#include <stdio.h>
+
+// check if p is a valid option and if the option takes an arg
+static char isvalidopt(char p, const char *options, int *takesarg)
+{
+	int idx = 0;
+	*takesarg = 0;
+
+	while (options[idx] != 0 && p != options[idx]) {
+		++idx;
+	}
+
+	if (options[idx] == 0) {
+		return '?';
+	}
+
+	if (options[idx + 1] == ':') {
+		*takesarg = 1;
+	}
+
+	return options[idx];
+}
+
+// reorder argv and put non-options at the end
+static int reorder(int argc, char **argv, const char *options)
+{
+	char *tmp_argv[argc];
+	char c;
+	int idx = 1;
+	int tmpidx = 1;
+	int takesarg;
+
+	// move the options to the front
+	while (idx < argc && argv[idx] != 0) {
+		if (argv[idx][0] == '-') {
+			c = isvalidopt(argv[idx][1], options, &takesarg);
+
+			if (c == '?') {
+				return 1;
+			}
+
+			tmp_argv[tmpidx] = argv[idx];
+			tmpidx++;
+
+			if (takesarg) {
+				if (idx + 1 >= argc) { //Error: option takes an argument, but there is no more argument
+					return 1;
+				}
+
+				tmp_argv[tmpidx] = argv[idx + 1];
+				// printf("tmp_argv[%d] = %s\n", tmpidx, tmp_argv[tmpidx]);
+				tmpidx++;
+				idx++;
+			}
+		}
+
+		idx++;
+	}
+
+	// Add non-options to the end
+	idx = 1;
+
+	while (idx < argc && argv[idx] != 0) {
+		if (argv[idx][0] == '-') {
+			c = isvalidopt(argv[idx][1], options, &takesarg);
+
+			if (c == '?') {
+				return c;
+			}
+
+			if (takesarg) {
+				idx++;
+			}
+
+		} else {
+			tmp_argv[tmpidx] = argv[idx];
+			tmpidx++;
+		}
+
+		idx++;
+	}
+
+	// Reorder argv
+	for (idx = 1; idx < argc; idx++) {
+		argv[idx] = tmp_argv[idx];
+	}
+
+	return 0;
+}
+
+//
+// px4_getopt
+//
+// returns:
+//            the valid option character
+//            '?' if any option is unknown
+//            -1 if no remaining options
+//
+// If the option takes an arg, myoptarg will be updated accordingly.
+// After each call to px4_getopt, myoptind in incremented to the next
+// unparsed arg index.
+// Argv is changed to put all options and option args at the beginning,
+// followed by non-options.
+//
+__EXPORT int px4_getopt(int argc, char *argv[], const char *options, int *myoptind, const char **myoptarg)
+{
+	char *p;
+	char c;
+	int takesarg;
+
+	if (*myoptind == 1) {
+		if (reorder(argc, argv, options) != 0) {
+			*myoptind += 1;
+			return (int)'?';
+		}
+	}
+
+	p = argv[*myoptind];
+
+	if (*myoptarg == 0) {
+		*myoptarg = argv[*myoptind];
+	}
+
+	if (p && options && myoptind && p[0] == '-') {
+		c = isvalidopt(p[1], options, &takesarg);
+
+		if (c == '?') {
+			*myoptind += 1;
+			return (int)c;
+		}
+
+		*myoptind += 1;
+
+		if (takesarg) {
+			*myoptarg = argv[*myoptind];
+
+			if (!*myoptarg) { //Error: option takes an argument, but there is no more argument
+				return -1;
+			}
+
+			*myoptind += 1;
+		}
+
+		return (int)c;
+	}
+
+	return -1;
+}

platforms/common/px4_log.c

@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2017-2018 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.
+ *
+ ****************************************************************************/
+
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#ifndef MODULE_NAME
+#define MODULE_NAME "log"
+#endif
+
+#include <px4_log.h>
+#if defined(__PX4_POSIX)
+#include <px4_daemon/server_io.h>
+#endif
+
+#include <uORB/uORB.h>
+#include <uORB/topics/log_message.h>
+#include <drivers/drv_hrt.h>
+
+static orb_advert_t orb_log_message_pub = NULL;
+
+__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "DEBUG", "INFO", "WARN", "ERROR", "PANIC" };
+__EXPORT const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1] =
+{ PX4_ANSI_COLOR_GREEN, PX4_ANSI_COLOR_RESET, PX4_ANSI_COLOR_YELLOW, PX4_ANSI_COLOR_RED, PX4_ANSI_COLOR_RED };
+
+
+void px4_log_initialize(void)
+{
+	assert(orb_log_message_pub == NULL);
+
+	/* we need to advertise with a valid message */
+	struct log_message_s log_message;
+	log_message.timestamp = hrt_absolute_time();
+	log_message.severity = 6; //info
+	strcpy((char *)log_message.text, "initialized uORB logging");
+
+#if !defined(PARAM_NO_ORB)
+	orb_log_message_pub = orb_advertise_queue(ORB_ID(log_message), &log_message, 2);
+#endif /* !PARAM_NO_ORB */
+
+	if (!orb_log_message_pub) {
+		PX4_ERR("failed to advertise log_message");
+	}
+}
+
+
+__EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...)
+{
+	FILE *out = stdout;
+	bool use_color = true;
+
+#ifdef __PX4_POSIX
+	out = get_stdout(&use_color);
+#endif
+
+#ifndef PX4_LOG_COLORIZED_OUTPUT
+	use_color = false;
+#endif
+
+	if (level >= _PX4_LOG_LEVEL_INFO) {
+		if (use_color) { fputs(__px4_log_level_color[level], out); }
+
+		fprintf(out, __px4__log_level_fmt __px4__log_level_arg(level));
+
+		if (use_color) { fputs(PX4_ANSI_COLOR_GRAY, out); }
+
+		fprintf(out, __px4__log_modulename_pfmt, moduleName);
+
+		if (use_color) { fputs(__px4_log_level_color[level], out); }
+
+		va_list argptr;
+		va_start(argptr, fmt);
+		vfprintf(out, fmt, argptr);
+		va_end(argptr);
+
+		if (use_color) { fputs(PX4_ANSI_COLOR_RESET, out); }
+
+		fputc('\n', out);
+	}
+
+	/* publish an orb log message */
+	if (level >= _PX4_LOG_LEVEL_WARN && orb_log_message_pub) { //only publish important messages
+
+		struct log_message_s log_message;
+		const unsigned max_length_pub = sizeof(log_message.text);
+		log_message.timestamp = hrt_absolute_time();
+
+		const uint8_t log_level_table[] = {
+			7, /* _PX4_LOG_LEVEL_DEBUG */
+			6, /* _PX4_LOG_LEVEL_INFO */
+			4, /* _PX4_LOG_LEVEL_WARN */
+			3, /* _PX4_LOG_LEVEL_ERROR */
+			0  /* _PX4_LOG_LEVEL_PANIC */
+		};
+		log_message.severity = log_level_table[level];
+
+		unsigned pos = 0;
+
+		va_list argptr;
+
+		pos += snprintf((char *)log_message.text + pos, max_length_pub - pos, __px4__log_modulename_pfmt, moduleName);
+		va_start(argptr, fmt);
+		pos += vsnprintf((char *)log_message.text + pos, max_length_pub - pos, fmt, argptr);
+		va_end(argptr);
+		log_message.text[max_length_pub - 1] = 0; //ensure 0-termination
+
+#if !defined(PARAM_NO_ORB)
+		orb_publish(ORB_ID(log_message), orb_log_message_pub, &log_message);
+#endif /* !PARAM_NO_ORB */
+	}
+}
+
+__EXPORT void px4_log_raw(int level, const char *fmt, ...)
+{
+	FILE *out = stdout;
+	bool use_color = true;
+
+#ifdef __PX4_POSIX
+	out = get_stdout(&use_color);
+#endif
+
+#ifndef PX4_LOG_COLORIZED_OUTPUT
+	use_color = false;
+#endif
+
+	if (level >= _PX4_LOG_LEVEL_INFO) {
+		if (use_color) { fputs(__px4_log_level_color[level], out); }
+
+		va_list argptr;
+		va_start(argptr, fmt);
+		vfprintf(out, fmt, argptr);
+		va_end(argptr);
+
+		if (use_color) { fputs(PX4_ANSI_COLOR_RESET, out); }
+	}
+}

platforms/common/px4_work_queue/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2019 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.
+#
+############################################################################
+
+px4_add_library(px4_work_queue
+	ScheduledWorkItem.cpp
+	WorkItem.cpp
+	WorkQueue.cpp
+	WorkQueueManager.cpp
+)
+
+if(PX4_TESTING)
+	add_subdirectory(test)
+endif()
+
+target_link_libraries(px4_work_queue PRIVATE px4_platform)

platforms/common/px4_work_queue/ScheduledWorkItem.cpp

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+namespace px4
+{
+
+ScheduledWorkItem::~ScheduledWorkItem()
+{
+	ScheduleClear();
+}
+
+void ScheduledWorkItem::schedule_trampoline(void *arg)
+{
+	ScheduledWorkItem *dev = reinterpret_cast<ScheduledWorkItem *>(arg);
+	dev->ScheduleNow();
+}
+
+void ScheduledWorkItem::ScheduleDelayed(uint32_t delay_us)
+{
+	hrt_call_after(&_call, delay_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this);
+}
+
+void ScheduledWorkItem::ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us)
+{
+	hrt_call_every(&_call, delay_us, interval_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this);
+}
+
+void ScheduledWorkItem::ScheduleClear()
+{
+	hrt_cancel(&_call);
+}
+
+} // namespace px4

platforms/common/px4_work_queue/WorkItem.cpp

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+
+#include <px4_platform_common/px4_work_queue/WorkQueue.hpp>
+#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
+
+#include <px4_log.h>
+#include <drivers/drv_hrt.h>
+
+namespace px4
+{
+
+WorkItem::WorkItem(const wq_config_t &config)
+{
+	if (!Init(config)) {
+		PX4_ERR("init failed");
+	}
+}
+
+WorkItem::~WorkItem()
+{
+	Deinit();
+}
+
+bool WorkItem::Init(const wq_config_t &config)
+{
+	// clear any existing first
+	Deinit();
+
+	px4::WorkQueue *wq = WorkQueueFindOrCreate(config);
+
+	if (wq == nullptr) {
+		PX4_ERR("%s not available", config.name);
+
+	} else {
+		_wq = wq;
+		return true;
+	}
+
+	return false;
+}
+
+void WorkItem::Deinit()
+{
+	// remove any currently queued work
+	if (_wq != nullptr) {
+		// prevent additional insertions
+		px4::WorkQueue *wq_temp = _wq;
+		_wq = nullptr;
+
+		wq_temp->Remove(this);
+	}
+}
+
+} // namespace px4

platforms/common/px4_work_queue/WorkQueue.cpp

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/px4_work_queue/WorkQueue.hpp>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+
+#include <string.h>
+
+#include <px4_tasks.h>
+#include <px4_time.h>
+#include <drivers/drv_hrt.h>
+
+namespace px4
+{
+
+WorkQueue::WorkQueue(const wq_config_t &config) :
+	_config(config)
+{
+	// set the threads name
+#ifdef __PX4_DARWIN
+	pthread_setname_np(_config.name);
+#elif !defined(__PX4_QURT)
+	pthread_setname_np(pthread_self(), _config.name);
+#endif
+
+#ifndef __PX4_NUTTX
+	px4_sem_init(&_qlock, 0, 1);
+#endif /* __PX4_NUTTX */
+
+	px4_sem_init(&_process_lock, 0, 0);
+	px4_sem_setprotocol(&_process_lock, SEM_PRIO_NONE);
+}
+
+WorkQueue::~WorkQueue()
+{
+	work_lock();
+	px4_sem_destroy(&_process_lock);
+	work_unlock();
+
+#ifndef __PX4_NUTTX
+	px4_sem_destroy(&_qlock);
+#endif /* __PX4_NUTTX */
+}
+
+void WorkQueue::Add(WorkItem *item)
+{
+	// TODO: prevent additions when shutting down
+
+	work_lock();
+	_q.push(item);
+	work_unlock();
+
+	// Wake up the worker thread
+	px4_sem_post(&_process_lock);
+}
+
+void WorkQueue::Remove(WorkItem *item)
+{
+	work_lock();
+	_q.remove(item);
+	work_unlock();
+}
+
+void WorkQueue::Clear()
+{
+	work_lock();
+
+	while (!_q.empty()) {
+		_q.pop();
+	}
+
+	work_unlock();
+}
+
+void WorkQueue::Run()
+{
+	while (!should_exit()) {
+		px4_sem_wait(&_process_lock);
+
+		work_lock();
+
+		// process queued work
+		while (!_q.empty()) {
+			WorkItem *work = _q.pop();
+
+			work_unlock(); // unlock work queue to run (item may requeue itself)
+			work->Run();
+			work_lock(); // re-lock
+		}
+
+		work_unlock();
+	}
+}
+
+void WorkQueue::print_status()
+{
+	PX4_INFO("WorkQueue: %s running", get_name());
+}
+
+} // namespace px4

platforms/common/px4_work_queue/WorkQueueManager.cpp

@@ -0,0 +1,296 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
+
+#include <px4_platform_common/px4_work_queue/WorkQueue.hpp>
+
+#include <drivers/drv_hrt.h>
+#include <px4_posix.h>
+#include <px4_tasks.h>
+#include <px4_time.h>
+#include <px4_atomic.h>
+#include <containers/BlockingList.hpp>
+#include <containers/BlockingQueue.hpp>
+#include <lib/drivers/device/Device.hpp>
+#include <lib/mathlib/mathlib.h>
+
+#include <limits.h>
+#include <string.h>
+
+using namespace time_literals;
+
+namespace px4
+{
+
+// list of current work queues
+static BlockingList<WorkQueue *> *_wq_manager_wqs_list{nullptr};
+
+// queue of WorkQueues to be created (as threads in the wq manager task)
+static BlockingQueue<const wq_config_t *, 1> *_wq_manager_create_queue{nullptr};
+
+static px4::atomic_bool _wq_manager_should_exit{false};
+
+
+static WorkQueue *FindWorkQueueByName(const char *name)
+{
+	if (_wq_manager_wqs_list == nullptr) {
+		PX4_ERR("not running");
+		return nullptr;
+	}
+
+	auto lg = _wq_manager_wqs_list->getLockGuard();
+
+	// search list
+	for (WorkQueue *wq : *_wq_manager_wqs_list) {
+		if (strcmp(wq->get_name(), name) == 0) {
+			return wq;
+		}
+	}
+
+	return nullptr;
+}
+
+WorkQueue *WorkQueueFindOrCreate(const wq_config_t &new_wq)
+{
+	if (_wq_manager_create_queue == nullptr) {
+		PX4_ERR("not running");
+		return nullptr;
+	}
+
+	// search list for existing work queue
+	WorkQueue *wq = FindWorkQueueByName(new_wq.name);
+
+	// create work queue if it doesn't already exist
+	if (wq == nullptr) {
+		// add WQ config to list
+		//  main thread wakes up, creates the thread
+		_wq_manager_create_queue->push(&new_wq);
+
+		// we wait until new wq is created, then return
+		uint64_t t = 0;
+
+		while (wq == nullptr && t < 10_s) {
+			// Wait up to 10 seconds, checking every 1 ms
+			t += 1_ms;
+			px4_usleep(1_ms);
+
+			wq = FindWorkQueueByName(new_wq.name);
+		}
+
+		if (wq == nullptr) {
+			PX4_ERR("failed to create %s", new_wq.name);
+		}
+	}
+
+	return wq;
+}
+
+const wq_config_t &device_bus_to_wq(uint32_t device_id_int)
+{
+	union device::Device::DeviceId device_id;
+	device_id.devid = device_id_int;
+
+	const device::Device::DeviceBusType bus_type = device_id.devid_s.bus_type;
+	const uint8_t bus = device_id.devid_s.bus;
+
+	if (bus_type == device::Device::DeviceBusType_I2C) {
+		switch (bus) {
+		case 1: return wq_configurations::I2C1;
+
+		case 2: return wq_configurations::I2C2;
+
+		case 3: return wq_configurations::I2C3;
+
+		case 4: return wq_configurations::I2C4;
+		}
+
+	} else if (bus_type == device::Device::DeviceBusType_SPI) {
+		switch (bus) {
+		case 1: return wq_configurations::SPI1;
+
+		case 2: return wq_configurations::SPI2;
+
+		case 3: return wq_configurations::SPI3;
+
+		case 4: return wq_configurations::SPI4;
+
+		case 5: return wq_configurations::SPI5;
+
+		case 6: return wq_configurations::SPI6;
+		}
+	}
+
+	// otherwise use high priority
+	return wq_configurations::hp_default;
+};
+
+static void *WorkQueueRunner(void *context)
+{
+	wq_config_t *config = static_cast<wq_config_t *>(context);
+	WorkQueue wq(*config);
+
+	// add to work queue list
+	_wq_manager_wqs_list->add(&wq);
+
+	wq.Run();
+
+	// remove from work queue list
+	_wq_manager_wqs_list->remove(&wq);
+
+	return nullptr;
+}
+
+static void WorkQueueManagerRun()
+{
+	_wq_manager_wqs_list = new BlockingList<WorkQueue *>();
+	_wq_manager_create_queue = new BlockingQueue<const wq_config_t *, 1>();
+
+	while (!_wq_manager_should_exit.load()) {
+		// create new work queues as needed
+		const wq_config_t *wq = _wq_manager_create_queue->pop();
+
+		if (wq != nullptr) {
+			// create new work queue
+
+			pthread_attr_t attr;
+			int ret_attr_init = pthread_attr_init(&attr);
+
+			if (ret_attr_init != 0) {
+				PX4_ERR("attr init for %s failed (%i)", wq->name, ret_attr_init);
+			}
+
+			sched_param param;
+			int ret_getschedparam = pthread_attr_getschedparam(&attr, &param);
+
+			if (ret_getschedparam != 0) {
+				PX4_ERR("getting sched param for %s failed (%i)", wq->name, ret_getschedparam);
+			}
+
+			// stack size
+#ifndef __PX4_QURT
+			const size_t stacksize = math::max(PTHREAD_STACK_MIN, PX4_STACK_ADJUSTED(wq->stacksize));
+#else
+			const size_t stacksize = math::max(8 * 1024, PX4_STACK_ADJUSTED(wq->stacksize));
+#endif
+			int ret_setstacksize = pthread_attr_setstacksize(&attr, stacksize);
+
+			if (ret_setstacksize != 0) {
+				PX4_ERR("setting stack size for %s failed (%i)", wq->name, ret_setstacksize);
+			}
+
+#ifndef __PX4_QURT
+			// schedule policy FIFO
+			int ret_setschedpolicy = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
+
+			if (ret_setschedpolicy != 0) {
+				PX4_ERR("failed to set sched policy SCHED_FIFO (%i)", ret_setschedpolicy);
+			}
+
+#endif // ! QuRT
+
+			// priority
+			param.sched_priority = sched_get_priority_max(SCHED_FIFO) + wq->relative_priority;
+			int ret_setschedparam = pthread_attr_setschedparam(&attr, &param);
+
+			if (ret_setschedparam != 0) {
+				PX4_ERR("setting sched params for %s failed (%i)", wq->name, ret_setschedparam);
+			}
+
+			// create thread
+			pthread_t thread;
+			int ret_create = pthread_create(&thread, &attr, WorkQueueRunner, (void *)wq);
+
+			if (ret_create == 0) {
+				PX4_INFO("creating: %s, priority: %d, stack: %zu bytes", wq->name, param.sched_priority, stacksize);
+
+			} else {
+				PX4_ERR("failed to create thread for %s (%i): %s", wq->name, ret_create, strerror(ret_create));
+			}
+
+			// destroy thread attributes
+			int ret_destroy = pthread_attr_destroy(&attr);
+
+			if (ret_destroy != 0) {
+				PX4_ERR("failed to destroy thread attributes for %s (%i)", wq->name, ret_create);
+			}
+		}
+	}
+}
+
+int WorkQueueManagerStart()
+{
+	int task_id = px4_task_spawn_cmd("wq:manager",
+					 SCHED_DEFAULT,
+					 PX4_WQ_HP_BASE,
+					 1200,
+					 (px4_main_t)&WorkQueueManagerRun,
+					 nullptr);
+
+	if (task_id < 0) {
+		PX4_ERR("task start failed (%i)", task_id);
+		return -errno;
+	}
+
+	return 0;
+}
+
+int WorkQueueManagerStop()
+{
+	if (_wq_manager_wqs_list != nullptr) {
+		auto lg = _wq_manager_wqs_list->getLockGuard();
+
+		// ask all work queues (threads) to stop
+		// NOTE: not currently safe without all WorkItems stopping first
+		for (WorkQueue *wq : *_wq_manager_wqs_list) {
+			wq->request_stop();
+		}
+
+		delete _wq_manager_wqs_list;
+	}
+
+	_wq_manager_should_exit.store(true);
+
+	if (_wq_manager_create_queue != nullptr) {
+		// push nullptr to wake the wq manager task
+		_wq_manager_create_queue->push(nullptr);
+
+		px4_usleep(1000);
+
+		delete _wq_manager_create_queue;
+	}
+
+	return PX4_OK;
+}
+
+} // namespace px4

platforms/common/px4_work_queue/test/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2019 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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE lib__work_queue__test__wqueue_test
+	MAIN wqueue_test
+	SRCS
+		wqueue_main.cpp
+		wqueue_scheduled_test.cpp
+		wqueue_start.cpp
+		wqueue_test.cpp
+	DEPENDS
+		px4_work_queue
+	)

platforms/common/px4_work_queue/test/wqueue_main.cpp

@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#include "wqueue_test.h"
+#include "wqueue_scheduled_test.h"
+
+#include <px4_log.h>
+#include <px4_middleware.h>
+#include <px4_app.h>
+#include <stdio.h>
+
+int PX4_MAIN(int argc, char **argv)
+{
+	px4::init(argc, argv, "wqueue_test");
+
+	PX4_INFO("wqueue test 1");
+	WQueueTest wq1;
+	wq1.main();
+
+	PX4_INFO("wqueue test 2 (scheduled)");
+	WQueueScheduledTest wq2;
+	wq2.main();
+
+	PX4_INFO("wqueue test complete, exiting");
+
+	return 0;
+}

platforms/common/px4_work_queue/test/wqueue_scheduled_test.cpp

@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#include "wqueue_scheduled_test.h"
+
+#include <drivers/drv_hrt.h>
+#include <px4_log.h>
+#include <px4_time.h>
+
+#include <unistd.h>
+#include <stdio.h>
+#include <inttypes.h>
+
+using namespace px4;
+
+AppState WQueueScheduledTest::appState;
+
+void WQueueScheduledTest::Run()
+{
+	//PX4_INFO("iter: %d elapsed: %" PRId64 " us", _iter, hrt_elapsed_time(&_qtime));
+
+	if (_iter > 1000) {
+		appState.requestExit();
+	}
+
+	_iter++;
+}
+
+int WQueueScheduledTest::main()
+{
+	appState.setRunning(true);
+
+	_iter = 0;
+
+	// Put work in the work queue
+	ScheduleOnInterval(4000);
+
+	// Wait for work to finsh
+	while (!appState.exitRequested()) {
+		px4_usleep(10000);
+	}
+
+	PX4_INFO("WQueueScheduledTest finished");
+
+	//print_status();
+
+	px4_sleep(2);
+
+	return 0;
+}

platforms/common/px4_work_queue/test/wqueue_scheduled_test.h

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_app.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <string.h>
+
+using namespace px4;
+
+class WQueueScheduledTest : public px4::ScheduledWorkItem
+{
+public:
+	WQueueScheduledTest() : px4::ScheduledWorkItem(px4::wq_configurations::test2) {}
+	~WQueueScheduledTest() = default;
+
+	int main();
+
+	void Run() override;
+
+	static px4::AppState appState; /* track requests to terminate app */
+
+private:
+	int _iter{0};
+};

platforms/common/px4_work_queue/test/wqueue_start.cpp

@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#include "wqueue_test.h"
+
+#include <px4_app.h>
+#include <px4_log.h>
+#include <px4_tasks.h>
+#include <stdio.h>
+#include <string.h>
+#include <sched.h>
+
+static int daemon_task = -1;             /* Handle of daemon task / thread */
+
+extern "C" __EXPORT int wqueue_test_main(int argc, char *argv[]);
+int wqueue_test_main(int argc, char *argv[])
+{
+
+	if (argc < 2) {
+		PX4_INFO("usage: wqueue_test {start|stop|status}\n");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (WQueueTest::appState.isRunning()) {
+			PX4_INFO("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[2]) ? (char *const *)&argv[2] : (char *const *)nullptr);
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		WQueueTest::appState.requestExit();
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (WQueueTest::appState.isRunning()) {
+			PX4_INFO("is running\n");
+
+		} else {
+			PX4_INFO("not started\n");
+		}
+
+		return 0;
+	}
+
+	PX4_INFO("usage: wqueue_test {start|stop|status}\n");
+	return 1;
+}

platforms/common/px4_work_queue/test/wqueue_test.cpp

@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#include "wqueue_test.h"
+
+#include <drivers/drv_hrt.h>
+#include <px4_log.h>
+#include <px4_time.h>
+
+#include <unistd.h>
+#include <stdio.h>
+#include <inttypes.h>
+
+using namespace px4;
+
+AppState WQueueTest::appState;
+
+void WQueueTest::Run()
+{
+	//PX4_INFO("iter: %d elapsed: %" PRId64 " us", _iter, hrt_elapsed_time(&_qtime));
+
+	if (_iter > 10000) {
+		appState.requestExit();
+
+	} else {
+		ScheduleNow();
+	}
+
+	_iter++;
+}
+
+int WQueueTest::main()
+{
+	appState.setRunning(true);
+
+	_iter = 0;
+
+	// Put work in the work queue
+	ScheduleNow();
+
+	// Wait for work to finsh
+	while (!appState.exitRequested()) {
+		px4_usleep(5000);
+	}
+
+	PX4_INFO("WQueueTest finished");
+
+	//print_status();
+
+	px4_sleep(2);
+
+	return 0;
+}

platforms/common/px4_work_queue/test/wqueue_test.h

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_app.h>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <string.h>
+
+using namespace px4;
+
+class WQueueTest : public px4::WorkItem
+{
+public:
+	WQueueTest() : px4::WorkItem(px4::wq_configurations::test1) {}
+	~WQueueTest() = default;
+
+	int main();
+
+	void Run() override;
+
+	static px4::AppState appState; /* track requests to terminate app */
+
+private:
+	int _iter{0};
+};

platforms/common/shutdown.cpp

@@ -0,0 +1,248 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2017 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file shutdown.cpp
+ * Implementation of the API declared in px4_shutdown.h.
+ */
+
+#include <board_config.h>
+
+#include <px4_workqueue.h>
+#include <px4_shutdown.h>
+#include <px4_tasks.h>
+
+#ifndef MODULE_NAME
+#define MODULE_NAME "shutdown"
+#endif
+
+#include <px4_log.h>
+
+#include <stdint.h>
+#include <errno.h>
+#include <pthread.h>
+
+static pthread_mutex_t shutdown_mutex =
+	PTHREAD_MUTEX_INITIALIZER; // protects access to shutdown_hooks & shutdown_lock_counter
+static uint8_t shutdown_lock_counter = 0;
+
+int px4_shutdown_lock()
+{
+	int ret = pthread_mutex_lock(&shutdown_mutex);
+
+	if (ret == 0) {
+		++shutdown_lock_counter;
+		return pthread_mutex_unlock(&shutdown_mutex);
+	}
+
+	return ret;
+}
+
+int px4_shutdown_unlock()
+{
+	int ret = pthread_mutex_lock(&shutdown_mutex);
+
+	if (ret == 0) {
+		if (shutdown_lock_counter > 0) {
+			--shutdown_lock_counter;
+
+		} else {
+			PX4_ERR("unmatched number of px4_shutdown_unlock() calls");
+		}
+
+		return pthread_mutex_unlock(&shutdown_mutex);
+	}
+
+	return ret;
+}
+
+
+#if (defined(__PX4_NUTTX) && !defined(CONFIG_SCHED_WORKQUEUE)) || __PX4_QURT
+// minimal NuttX/QuRT build without work queue support
+
+int px4_register_shutdown_hook(shutdown_hook_t hook)
+{
+	return -EINVAL;
+}
+int px4_unregister_shutdown_hook(shutdown_hook_t hook)
+{
+	return -EINVAL;
+}
+
+int px4_shutdown_request(bool reboot, bool to_bootloader)
+{
+	int ret = 0;
+	pthread_mutex_lock(&shutdown_mutex);
+
+	// FIXME: if shutdown_lock_counter > 0, we should wait, but unfortunately we don't have work queues
+	if (reboot) {
+		px4_systemreset(to_bootloader);
+
+	} else {
+		ret = board_shutdown();
+	}
+
+	pthread_mutex_unlock(&shutdown_mutex);
+
+	return ret;
+}
+
+#else
+
+static struct work_s shutdown_work = {};
+static uint16_t shutdown_counter = 0; ///< count how many times the shutdown worker was executed
+
+#define SHUTDOWN_ARG_IN_PROGRESS (1<<0)
+#define SHUTDOWN_ARG_REBOOT (1<<1)
+#define SHUTDOWN_ARG_TO_BOOTLOADER (1<<2)
+static uint8_t shutdown_args = 0;
+
+
+static const int max_shutdown_hooks = 1;
+static shutdown_hook_t shutdown_hooks[max_shutdown_hooks] = {};
+
+
+static const int shutdown_timeout_ms = 5000; ///< force shutdown after this time if modules do not respond in time
+
+
+/**
+ * work queue callback method to shutdown.
+ * @param arg unused
+ */
+static void shutdown_worker(void *arg);
+
+
+int px4_register_shutdown_hook(shutdown_hook_t hook)
+{
+	pthread_mutex_lock(&shutdown_mutex);
+
+	for (int i = 0; i < max_shutdown_hooks; ++i) {
+		if (!shutdown_hooks[i]) {
+			shutdown_hooks[i] = hook;
+			pthread_mutex_unlock(&shutdown_mutex);
+			return 0;
+		}
+	}
+
+	pthread_mutex_unlock(&shutdown_mutex);
+	return -ENOMEM;
+}
+
+int px4_unregister_shutdown_hook(shutdown_hook_t hook)
+{
+	pthread_mutex_lock(&shutdown_mutex);
+
+	for (int i = 0; i < max_shutdown_hooks; ++i) {
+		if (shutdown_hooks[i] == hook) {
+			shutdown_hooks[i] = nullptr;
+			pthread_mutex_unlock(&shutdown_mutex);
+			return 0;
+		}
+	}
+
+	pthread_mutex_unlock(&shutdown_mutex);
+	return -EINVAL;
+}
+
+
+
+void shutdown_worker(void *arg)
+{
+	PX4_DEBUG("shutdown worker (%i)", shutdown_counter);
+	bool done = true;
+
+	pthread_mutex_lock(&shutdown_mutex);
+
+	for (int i = 0; i < max_shutdown_hooks; ++i) {
+		if (shutdown_hooks[i]) {
+			if (!shutdown_hooks[i]()) {
+				done = false;
+			}
+		}
+	}
+
+	if ((done && shutdown_lock_counter == 0) || ++shutdown_counter > shutdown_timeout_ms / 10) {
+		if (shutdown_args & SHUTDOWN_ARG_REBOOT) {
+			PX4_WARN("Reboot NOW.");
+			px4_systemreset(shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER);
+
+		} else {
+			PX4_WARN("Shutdown NOW. Good Bye.");
+			board_shutdown();
+		}
+
+		pthread_mutex_unlock(&shutdown_mutex); // must NEVER come here
+
+	} else {
+		pthread_mutex_unlock(&shutdown_mutex);
+		work_queue(HPWORK, &shutdown_work, (worker_t)&shutdown_worker, nullptr, USEC2TICK(10000));
+	}
+}
+
+int px4_shutdown_request(bool reboot, bool to_bootloader)
+{
+	// fail immediately if the board does not support the requested method
+#if defined BOARD_HAS_NO_RESET
+	if (reboot) {
+		return -EINVAL;
+	}
+
+#endif
+#if !defined(BOARD_HAS_POWER_CONTROL)
+
+	if (!reboot) {
+		return -EINVAL;
+	}
+
+#endif
+
+	if (shutdown_args & SHUTDOWN_ARG_IN_PROGRESS) {
+		return 0;
+	}
+
+	shutdown_args |= SHUTDOWN_ARG_IN_PROGRESS;
+
+	if (reboot) {
+		shutdown_args |= SHUTDOWN_ARG_REBOOT;
+	}
+
+	if (to_bootloader) {
+		shutdown_args |= SHUTDOWN_ARG_TO_BOOTLOADER;
+	}
+
+	shutdown_worker(nullptr);
+	return 0;
+}
+
+
+#endif /* (defined(__PX4_NUTTX) && !defined(CONFIG_SCHED_WORKQUEUE)) || __PX4_QURT */

platforms/common/work_queue/CMakeLists.txt

@@ -0,0 +1,57 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+
+# nuttx uses internal work queue currently
+if (NOT "${PX4_PLATFORM}" MATCHES "nuttx")
+
+	add_library(work_queue
+		dq_addlast.c
+		dq_rem.c
+		dq_remfirst.c
+		hrt_queue.c
+		hrt_thread.c
+		hrt_work_cancel.c
+		queue.c
+		sq_addafter.c
+		sq_addlast.c
+		sq_remfirst.c
+		work_cancel.c
+		work_lock.c
+		work_queue.c
+		work_thread.c
+	)
+	target_compile_definitions(work_queue PRIVATE MODULE_NAME="work_queue")
+	target_compile_options(work_queue PRIVATE -Wno-cast-align) # TODO: fix and enable
+	add_dependencies(work_queue prebuild_targets)
+
+endif()

platforms/common/work_queue/dq_addlast.c

@@ -0,0 +1,73 @@
+/************************************************************
+ * libc/queue/dq_addlast.c
+ *
+ *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************/
+
+/************************************************************
+ * Compilation Switches
+ ************************************************************/
+
+/************************************************************
+ * Included Files
+ ************************************************************/
+
+#include <stddef.h>
+#include <queue.h>
+
+/************************************************************
+ * Public Functions
+ ************************************************************/
+
+/************************************************************
+ * Name: dq_addlast
+ *
+ * Description
+ *   dq_addlast adds 'node' to the end of 'queue'
+ *
+ ************************************************************/
+
+void dq_addlast(dq_entry_t *node, dq_queue_t *queue)
+{
+	node->flink = NULL;
+	node->blink = queue->tail;
+
+	if (!queue->head) {
+		queue->head = node;
+		queue->tail = node;
+
+	} else {
+		queue->tail->flink = node;
+		queue->tail        = node;
+	}
+}
+

platforms/common/work_queue/dq_rem.c

@@ -0,0 +1,81 @@
+/************************************************************
+ * libc/queue/dq_rem.c
+ *
+ *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************/
+
+/************************************************************
+ * Compilation Switches
+ ************************************************************/
+
+/************************************************************
+ * Included Files
+ ************************************************************/
+
+#include <stddef.h>
+#include <queue.h>
+
+/************************************************************
+ * Public Functions
+ ************************************************************/
+
+/************************************************************
+ * Name: dq_rem
+ *
+ * Description:
+ *   dq_rem removes 'node' from 'queue'
+ *
+ ************************************************************/
+
+void dq_rem(dq_entry_t *node, dq_queue_t *queue)
+{
+	dq_entry_t *prev = node->blink;
+	dq_entry_t *next = node->flink;
+
+	if (!prev) {
+		queue->head = next;
+
+	} else {
+		prev->flink = next;
+	}
+
+	if (!next) {
+		queue->tail = prev;
+
+	} else {
+		next->blink = prev;
+	}
+
+	node->flink = NULL;
+	node->blink = NULL;
+}
+

platforms/common/work_queue/dq_remfirst.c

@@ -0,0 +1,81 @@
+/************************************************************
+ * libc/queue/dq_remfirst.c
+ *
+ *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************/
+
+/************************************************************
+ * Compilation Switches
+ ************************************************************/
+
+/************************************************************
+ * Included Files
+ ************************************************************/
+
+#include <stddef.h>
+#include <queue.h>
+
+/************************************************************
+ * Public Functions
+ ************************************************************/
+
+/************************************************************
+ * Name: dq_remfirst
+ *
+ * Description:
+ *   dq_remfirst removes 'node' from the head of 'queue'
+ *
+ ************************************************************/
+
+dq_entry_t *dq_remfirst(dq_queue_t *queue)
+{
+	dq_entry_t *ret = queue->head;
+
+	if (ret) {
+		dq_entry_t *next = ret->flink;
+
+		if (!next) {
+			queue->head = NULL;
+			queue->tail = NULL;
+
+		} else {
+			queue->head = next;
+			next->blink = NULL;
+		}
+
+		ret->flink = NULL;
+		ret->blink = NULL;
+	}
+
+	return ret;
+}
+

platforms/common/work_queue/hrt_queue.c

@@ -0,0 +1,140 @@
+/****************************************************************************
+ * libc/wqueue/work_queue.c
+ *
+ *   Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_workqueue.h>
+#include <px4_tasks.h>
+
+#include <signal.h>
+#include <stdint.h>
+#include <queue.h>
+#include <stdio.h>
+#include <semaphore.h>
+#include <drivers/drv_hrt.h>
+#include <px4_workqueue.h>
+#include <px4_posix.h>
+#include "hrt_work.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Type Declarations
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: hrt_work_queue
+ *
+ * Description:
+ *   Queue work to be performed at a later time.  All queued work will be
+ *   performed on the worker thread of of execution (not the caller's).
+ *
+ *   The work structure is allocated by caller, but completely managed by
+ *   the work queue logic.  The caller should never modify the contents of
+ *   the work queue structure; the caller should not call work_queue()
+ *   again until either (1) the previous work has been performed and removed
+ *   from the queue, or (2) work_cancel() has been called to cancel the work
+ *   and remove it from the work queue.
+ *
+ * Input parameters:
+ *   work   - The work structure to queue
+ *   worker - The worker callback to be invoked.  The callback will invoked
+ *            on the worker thread of execution.
+ *   arg    - The argument that will be passed to the workder callback when
+ *            int is invoked.
+ *   delay  - Delay (in microseconds) from the time queue until the worker
+ *            is invoked. Zero means to perform the work immediately.
+ *
+ * Returned Value:
+ *   Zero on success, a negated errno on failure
+ *
+ ****************************************************************************/
+
+int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay)
+{
+	struct wqueue_s *wqueue = &g_hrt_work;
+
+	/* First, initialize the work structure */
+
+	work->worker = worker;           /* Work callback */
+	work->arg    = arg;              /* Callback argument */
+	work->delay  = delay;            /* Delay until work performed */
+
+	/* Now, time-tag that entry and put it in the work queue.  This must be
+	 * done with interrupts disabled.  This permits this function to be called
+	 * from with task logic or interrupt handlers.
+	 */
+
+	hrt_work_lock();
+	work->qtime  = hrt_absolute_time(); /* Time work queued */
+	//PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime);
+
+	dq_addlast((dq_entry_t *)work, &wqueue->q);
+
+	if (px4_getpid() != wqueue->pid) { /* only need to wake up if called from a different thread */
+#ifdef __PX4_QURT
+		px4_task_kill(wqueue->pid, SIGALRM);      /* Wake up the worker thread */
+#else
+		//wqueue->pid == own task? -> don't signal
+		px4_task_kill(wqueue->pid, SIGCONT);      /* Wake up the worker thread */
+#endif
+	}
+
+	hrt_work_unlock();
+	return PX4_OK;
+}
+

platforms/common/work_queue/hrt_thread.c

@@ -0,0 +1,296 @@
+/****************************************************************************
+ * libc/wqueue/work_thread.c
+ *
+ *   Copyright (C) 2009-2013 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *   Modified by: Mark Charlebois to use hrt compatible times
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_posix.h>
+#include <px4_tasks.h>
+#include <px4_time.h>
+#include <stdint.h>
+#include <signal.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <queue.h>
+#include <px4_workqueue.h>
+#include <drivers/drv_hrt.h>
+#include "hrt_work.h"
+
+#include <string.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Type Declarations
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/* The state of each work queue. */
+struct wqueue_s g_hrt_work;
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+px4_sem_t _hrt_work_lock;
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+static void hrt_work_process(void);
+
+static void _sighandler(int sig_num);
+
+/****************************************************************************
+ * Name: _sighandler
+ *
+ * Description:
+ *   This is the handler for the signal to wake the queue processing thread
+ *
+ * Input parameters:
+ *   sig_num - the received signal
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+static void _sighandler(int sig_num)
+{
+	PX4_DEBUG("RECEIVED SIGNAL %d", sig_num);
+}
+
+/****************************************************************************
+ * Name: work_process
+ *
+ * Description:
+ *   This is the logic that performs actions placed on any work list.
+ *
+ * Input parameters:
+ *   wqueue - Describes the work queue to be processed
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+static void hrt_work_process()
+{
+	struct wqueue_s *wqueue = &g_hrt_work;
+	volatile struct work_s *work;
+	worker_t  worker;
+	void *arg;
+	uint64_t elapsed;
+	uint32_t remaining;
+	uint32_t next;
+
+	// set the threads name
+#ifdef __PX4_DARWIN
+	pthread_setname_np("HRT");
+#else
+	// The Linux headers do not actually contain this
+	//rv = pthread_setname_np(pthread_self(), "HRT");
+#endif
+
+	/* Then process queued work.  We need to keep interrupts disabled while
+	 * we process items in the work list.
+	 */
+
+	/* Default to sleeping for 1 sec */
+	next  = 1000000;
+
+	hrt_work_lock();
+
+	work  = (struct work_s *)wqueue->q.head;
+
+	while (work) {
+		/* Is this work ready?  It is ready if there is no delay or if
+		 * the delay has elapsed. qtime is the time that the work was added
+		 * to the work queue.  It will always be greater than or equal to
+		 * zero.  Therefore a delay of zero will always execute immediately.
+		 */
+
+		elapsed = hrt_absolute_time() - work->qtime;
+
+		//PX4_INFO("hrt work_process: in usec elapsed=%lu delay=%u work=%p", elapsed, work->delay, work);
+		if (elapsed >= work->delay) {
+			/* Remove the ready-to-execute work from the list */
+
+			(void)dq_rem((struct dq_entry_s *) & (work->dq), &(wqueue->q));
+			//PX4_INFO("Dequeued work=%p", work);
+
+			/* Extract the work description from the entry (in case the work
+			 * instance by the re-used after it has been de-queued).
+			 */
+
+			worker = work->worker;
+			arg    = work->arg;
+
+			/* Mark the work as no longer being queued */
+
+			work->worker = NULL;
+
+			/* Do the work.  Re-enable interrupts while the work is being
+			 * performed... we don't have any idea how long that will take!
+			 */
+
+			hrt_work_unlock();
+
+			if (!worker) {
+				PX4_ERR("MESSED UP: worker = 0");
+
+			} 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
+			 * back at the head of the list.
+			 */
+
+			hrt_work_lock();
+			work  = (struct work_s *)wqueue->q.head;
+
+		} else {
+			/* This one is not ready.. will it be ready before the next
+			 * scheduled wakeup interval?
+			 */
+
+			/* Here: elapsed < work->delay */
+			remaining = work->delay - elapsed;
+
+			//PX4_INFO("remaining=%u delay=%u elapsed=%lu", remaining, work->delay, elapsed);
+			if (remaining < next) {
+				/* Yes.. Then schedule to wake up when the work is ready */
+
+				next = remaining;
+			}
+
+			/* Then try the next in the list. */
+
+			work = (struct work_s *)work->dq.flink;
+			//PX4_INFO("next %u work %p", next, work);
+		}
+	}
+
+	/* Wait awhile to check the work list.  We will wait here until either
+	 * the time elapses or until we are awakened by a signal.
+	 */
+	hrt_work_unlock();
+
+	/* might sleep less if a signal received and new item was queued */
+	//PX4_INFO("Sleeping for %u usec", next);
+	px4_usleep(next);
+}
+
+/****************************************************************************
+ * Name: work_hrtthread
+ *
+ * Description:
+ *   This is the worker threads that performs actions placed on the ISR work
+ *   list.
+ *
+ *   work_hpthread and work_lpthread:  These are the kernel mode work queues
+ *     (also build in the flat build).  One of these threads also performs
+ *     periodic garbage collection (that is otherwise performed by the idle
+ *     thread if CONFIG_SCHED_WORKQUEUE is not defined).
+ *
+ *     These worker threads are started by the OS during normal bringup.
+ *
+ *   All of these entrypoints are referenced by OS internally and should not
+ *   not be accessed by application logic.
+ *
+ * Input parameters:
+ *   argc, argv (not used)
+ *
+ * Returned Value:
+ *   Does not return
+ *
+ ****************************************************************************/
+
+static int work_hrtthread(int argc, char *argv[])
+{
+	/* Loop forever */
+
+	for (;;) {
+		/* First, perform garbage collection.  This cleans-up memory de-allocations
+		 * that were queued because they could not be freed in that execution
+		 * context (for example, if the memory was freed from an interrupt handler).
+		 * NOTE: If the work thread is disabled, this clean-up is performed by
+		 * the IDLE thread (at a very, very low priority).
+		 */
+
+		/* Then process queued work.  We need to keep interrupts disabled while
+		 * we process items in the work list.
+		 */
+
+		hrt_work_process();
+	}
+
+	return PX4_OK; /* To keep some compilers happy */
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+void hrt_work_queue_init(void)
+{
+	px4_sem_init(&_hrt_work_lock, 0, 1);
+	memset(&g_hrt_work, 0, sizeof(g_hrt_work));
+
+	// Create high priority worker thread
+	g_hrt_work.pid = px4_task_spawn_cmd("wkr_hrt",
+					    SCHED_DEFAULT,
+					    SCHED_PRIORITY_MAX,
+					    2000,
+					    work_hrtthread,
+					    (char *const *)NULL);
+
+
+#ifdef __PX4_QURT
+	signal(SIGALRM, _sighandler);
+#else
+	signal(SIGCONT, _sighandler);
+#endif
+}
+

platforms/common/work_queue/hrt_work_cancel.c

@@ -0,0 +1,111 @@
+/****************************************************************************
+ * libc/wqueue/work_cancel.c
+ *
+ *   Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_workqueue.h>
+
+#include "hrt_work.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Type Declarations
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: hrt_work_cancel
+ *
+ * Description:
+ *   Cancel previously queued work.  This removes work from the work queue.
+ *   After work has been canceled, it may be re-queue by calling
+ *   hrt_work_queue() again.
+ *
+ * Input parameters:
+ *   work   - The previously queue work structure to cancel
+ *
+ ****************************************************************************/
+
+void hrt_work_cancel(struct work_s *work)
+{
+	struct wqueue_s *wqueue = &g_hrt_work;
+
+	//DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
+
+	/* Cancelling the work is simply a matter of removing the work structure
+	 * from the work queue.  This must be done with interrupts disabled because
+	 * new work is typically added to the work queue from interrupt handlers.
+	 */
+
+	hrt_work_lock();
+
+	if (work->worker != NULL) {
+		/* A little test of the integrity of the work queue */
+
+		//DEBUGASSERT(work->dq.flink ||(dq_entry_t *)work == wqueue->q.tail);
+		//DEBUGASSERT(work->dq.blink ||(dq_entry_t *)work == wqueue->q.head);
+
+		/* Remove the entry from the work queue and make sure that it is
+		 * mark as availalbe (i.e., the worker field is nullified).
+		 */
+
+		dq_rem((dq_entry_t *)work, &wqueue->q);
+		work->worker = NULL;
+	}
+
+	hrt_work_unlock();
+}

platforms/common/work_queue/queue.c

@@ -0,0 +1,98 @@
+/************************************************************************
+ *
+ *   Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ * Modified for use in Linux by Mark Charlebois
+ *
+ ************************************************************************/
+
+// FIXME - need px4_queue
+#include <queue.h>
+#include <stddef.h>
+
+__EXPORT void sq_rem(sq_entry_t *node, sq_queue_t *queue);
+sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue);
+void sq_rem(sq_entry_t *node, sq_queue_t *queue)
+{
+	if (queue->head && node) {
+		if (node == queue->head) {
+			queue->head = node->flink;
+
+			if (node == queue->tail) {
+				queue->tail = NULL;
+			}
+
+		} else {
+			sq_entry_t *prev;
+
+			for (prev = (sq_entry_t *)queue->head;
+			     prev && prev->flink != node;
+			     prev = prev->flink);
+
+			if (prev) {
+				sq_remafter(prev, queue);
+			}
+		}
+	}
+}
+
+sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue)
+{
+	sq_entry_t *ret = node->flink;
+
+	if (queue->head && ret) {
+		if (queue->tail == ret) {
+			queue->tail = node;
+			node->flink = NULL;
+
+		} else {
+			node->flink = ret->flink;
+		}
+
+		ret->flink = NULL;
+	}
+
+	return ret;
+}
+
+__EXPORT void sq_addfirst(sq_entry_t *node, sq_queue_t *queue);
+void sq_addfirst(sq_entry_t *node, sq_queue_t *queue)
+{
+	node->flink = queue->head;
+
+	if (!queue->head) {
+		queue->tail = node;
+	}
+
+	queue->head = node;
+}
+
+

platforms/common/work_queue/sq_addafter.c

@@ -0,0 +1,69 @@
+/************************************************************
+ * libc/queue/sq_addafter.c
+ *
+ *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************/
+
+/************************************************************
+ * Compilation Switches
+ ************************************************************/
+
+/************************************************************
+ * Included Files
+ ************************************************************/
+
+#include <queue.h>
+
+/************************************************************
+ * Public Functions
+ ************************************************************/
+
+/************************************************************
+ * Name: sq_addafter.c
+ *
+ * Description:
+ *  The sq_addafter function adds 'node' after 'prev' in the
+ *  'queue.'
+ *
+ ************************************************************/
+
+void sq_addafter(sq_entry_t *prev, sq_entry_t *node,
+		 sq_queue_t *queue)
+{
+	if (!queue->head || prev == queue->tail) {
+		sq_addlast(node, queue);
+
+	} else {
+		node->flink = prev->flink;
+		prev->flink = node;
+	}
+}

platforms/common/work_queue/sq_addlast.c

@@ -0,0 +1,72 @@
+/************************************************************
+ * libc/queue/sq_addlast.c
+ *
+ *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************/
+
+/************************************************************
+ * Compilation Switches
+ ************************************************************/
+
+/************************************************************
+ * Included Files
+ ************************************************************/
+
+#include <stddef.h>
+#include <queue.h>
+
+/************************************************************
+ * Public Functions
+ ************************************************************/
+
+/************************************************************
+ * Name: sq_addlast
+ *
+ * Description:
+ *   The sq_addlast function places the 'node' at the tail of
+ *   the 'queue'
+ ************************************************************/
+
+void sq_addlast(sq_entry_t *node, sq_queue_t *queue)
+{
+	node->flink = NULL;
+
+	if (!queue->head) {
+		queue->head = node;
+		queue->tail = node;
+
+	} else {
+		queue->tail->flink = node;
+		queue->tail        = node;
+	}
+}
+

platforms/common/work_queue/sq_remfirst.c

@@ -0,0 +1,76 @@
+/************************************************************
+ * libc/queue/sq_remfirst.c
+ *
+ *   Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************/
+
+/************************************************************
+ * Compilation Switches
+ ************************************************************/
+
+/************************************************************
+ * Included Files
+ ************************************************************/
+
+#include <stddef.h>
+#include <queue.h>
+
+/************************************************************
+ * Public Functions
+ ************************************************************/
+
+/************************************************************
+ * Name: sq_remfirst
+ *
+ * Description:
+ *   sq_remfirst function removes the first entry from
+ *   'queue'
+ *
+ ************************************************************/
+
+sq_entry_t *sq_remfirst(sq_queue_t *queue)
+{
+	sq_entry_t *ret = queue->head;
+
+	if (ret) {
+		queue->head = ret->flink;
+
+		if (!queue->head) {
+			queue->tail = NULL;
+		}
+
+		ret->flink = NULL;
+	}
+
+	return ret;
+}
+

platforms/common/work_queue/work_cancel.c

@@ -0,0 +1,120 @@
+/****************************************************************************
+ * libc/wqueue/work_cancel.c
+ *
+ *   Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <queue.h>
+#include <px4_workqueue.h>
+#include "work_lock.h"
+
+#ifdef CONFIG_SCHED_WORKQUEUE
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Type Declarations
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: work_cancel
+ *
+ * Description:
+ *   Cancel previously queued work.  This removes work from the work queue.
+ *   After work has been canceled, it may be re-queue by calling work_queue()
+ *   again.
+ *
+ * Input parameters:
+ *   qid    - The work queue ID
+ *   work   - The previously queue work structure to cancel
+ *
+ * Returned Value:
+ *   Zero on success, a negated errno on failure
+ *
+ ****************************************************************************/
+
+int work_cancel(int qid, struct work_s *work)
+{
+	struct wqueue_s *wqueue = &g_work[qid];
+
+	//DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
+
+	/* Cancelling the work is simply a matter of removing the work structure
+	 * from the work queue.  This must be done with interrupts disabled because
+	 * new work is typically added to the work queue from interrupt handlers.
+	 */
+
+	work_lock(qid);
+
+	if (work->worker != NULL) {
+		/* A little test of the integrity of the work queue */
+
+		//DEBUGASSERT(work->dq.flink ||(dq_entry_t *)work == wqueue->q.tail);
+		//DEBUGASSERT(work->dq.blink ||(dq_entry_t *)work == wqueue->q.head);
+
+		/* Remove the entry from the work queue and make sure that it is
+		 * mark as availalbe (i.e., the worker field is nullified).
+		 */
+
+		dq_rem((dq_entry_t *)work, &wqueue->q);
+		work->worker = NULL;
+	}
+
+	work_unlock(qid);
+	return PX4_OK;
+}
+
+#endif /* CONFIG_SCHED_WORKQUEUE */

platforms/common/work_queue/work_lock.c

@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ *   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 <px4_log.h>
+#include <px4_posix.h>
+#include <stdio.h>
+#include "work_lock.h"
+
+
+extern px4_sem_t _work_lock[];
+
+void work_lock(int id)
+{
+	px4_sem_wait(&_work_lock[id]);
+}
+
+void work_unlock(int id)
+{
+	px4_sem_post(&_work_lock[id]);
+}

platforms/common/work_queue/work_lock.h

@@ -0,0 +1,43 @@
+/****************************************************************************
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#ifndef _work_lock_h_
+#define _work_lock_h_
+
+
+//#pragma once
+
+void work_lock(int id);
+void work_unlock(int id);
+
+#endif // _work_lock_h_

platforms/common/work_queue/work_queue.c

@@ -0,0 +1,138 @@
+/****************************************************************************
+ * libc/wqueue/work_queue.c
+ *
+ *   Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_workqueue.h>
+#include <px4_tasks.h>
+
+#include <signal.h>
+#include <stdint.h>
+#include <queue.h>
+#include <stdio.h>
+#include <semaphore.h>
+#include "work_lock.h"
+
+#ifdef CONFIG_SCHED_WORKQUEUE
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Type Declarations
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: work_queue
+ *
+ * Description:
+ *   Queue work to be performed at a later time.  All queued work will be
+ *   performed on the worker thread of of execution (not the caller's).
+ *
+ *   The work structure is allocated by caller, but completely managed by
+ *   the work queue logic.  The caller should never modify the contents of
+ *   the work queue structure; the caller should not call work_queue()
+ *   again until either (1) the previous work has been performed and removed
+ *   from the queue, or (2) work_cancel() has been called to cancel the work
+ *   and remove it from the work queue.
+ *
+ * Input parameters:
+ *   qid    - The work queue ID (index)
+ *   work   - The work structure to queue
+ *   worker - The worker callback to be invoked.  The callback will invoked
+ *            on the worker thread of execution.
+ *   arg    - The argument that will be passed to the workder callback when
+ *            int is invoked.
+ *   delay  - Delay (in clock ticks) from the time queue until the worker
+ *            is invoked. Zero means to perform the work immediately.
+ *
+ * Returned Value:
+ *   Zero on success, a negated errno on failure
+ *
+ ****************************************************************************/
+
+int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay)
+{
+	struct wqueue_s *wqueue = &g_work[qid];
+
+	//DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
+
+	/* First, initialize the work structure */
+
+	work->worker = worker;           /* Work callback */
+	work->arg    = arg;              /* Callback argument */
+	work->delay  = delay;            /* Delay until work performed */
+
+	/* Now, time-tag that entry and put it in the work queue.  This must be
+	 * done with interrupts disabled.  This permits this function to be called
+	 * from with task logic or interrupt handlers.
+	 */
+
+	work_lock(qid);
+	work->qtime  = clock_systimer(); /* Time work queued */
+
+	dq_addlast((dq_entry_t *)work, &wqueue->q);
+#ifdef __PX4_QURT
+	px4_task_kill(wqueue->pid, SIGALRM);      /* Wake up the worker thread */
+#else
+	px4_task_kill(wqueue->pid, SIGCONT);      /* Wake up the worker thread */
+#endif
+
+	work_unlock(qid);
+	return PX4_OK;
+}
+
+#endif /* CONFIG_SCHED_WORKQUEUE */

platforms/common/work_queue/work_thread.c

@@ -0,0 +1,337 @@
+/****************************************************************************
+ * libc/wqueue/work_thread.c
+ *
+ *   Copyright (C) 2009-2013 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_posix.h>
+#include <px4_time.h>
+#include <px4_tasks.h>
+#include <px4_workqueue.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <queue.h>
+#include <pthread.h>
+#include <drivers/drv_hrt.h>
+#include "work_lock.h"
+
+#ifdef CONFIG_SCHED_WORKQUEUE
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Type Declarations
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/* The state of each work queue. */
+struct wqueue_s g_work[NWORKERS];
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+px4_sem_t _work_lock[NWORKERS];
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: work_process
+ *
+ * Description:
+ *   This is the logic that performs actions placed on any work list.
+ *
+ * Input parameters:
+ *   wqueue - Describes the work queue to be processed
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+static void work_process(struct wqueue_s *wqueue, int lock_id)
+{
+	volatile struct work_s *work;
+	worker_t  worker;
+	void *arg;
+	uint64_t elapsed;
+	uint32_t remaining;
+	uint32_t next;
+
+	/* Then process queued work.  We need to keep interrupts disabled while
+	 * we process items in the work list.
+	 */
+
+	next  = CONFIG_SCHED_WORKPERIOD;
+
+	work_lock(lock_id);
+
+	work  = (struct work_s *)wqueue->q.head;
+
+	while (work) {
+		/* Is this work ready?  It is ready if there is no delay or if
+		 * the delay has elapsed. qtime is the time that the work was added
+		 * to the work queue.  It will always be greater than or equal to
+		 * zero.  Therefore a delay of zero will always execute immediately.
+		 */
+
+		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 */
+
+			(void)dq_rem((struct dq_entry_s *)work, &wqueue->q);
+
+			/* Extract the work description from the entry (in case the work
+			 * instance by the re-used after it has been de-queued).
+			 */
+
+			worker = work->worker;
+			arg    = work->arg;
+
+			/* Mark the work as no longer being queued */
+
+			work->worker = NULL;
+
+			/* Do the work.  Re-enable interrupts while the work is being
+			 * performed... we don't have any idea how long that will take!
+			 */
+
+			work_unlock(lock_id);
+
+			if (!worker) {
+				PX4_WARN("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
+			 * back at the head of the list.
+			 */
+
+			work_lock(lock_id);
+			work  = (struct work_s *)wqueue->q.head;
+
+		} else {
+			/* This one is not ready.. will it be ready before the next
+			 * scheduled wakeup interval?
+			 */
+
+			/* Here: elapsed < work->delay */
+			remaining = USEC_PER_TICK * (work->delay - elapsed);
+
+			if (remaining < next) {
+				/* Yes.. Then schedule to wake up when the work is ready */
+
+				next = remaining;
+			}
+
+			/* Then try the next in the list. */
+
+			work = (struct work_s *)work->dq.flink;
+		}
+	}
+
+	/* 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);
+
+	px4_usleep(next);
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+void work_queues_init(void)
+{
+	px4_sem_init(&_work_lock[HPWORK], 0, 1);
+	px4_sem_init(&_work_lock[LPWORK], 0, 1);
+#ifdef CONFIG_SCHED_USRWORK
+	px4_sem_init(&_work_lock[USRWORK], 0, 1);
+#endif
+
+	// Create high priority worker thread
+	g_work[HPWORK].pid = px4_task_spawn_cmd("hpwork",
+						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("lpwork",
+						SCHED_DEFAULT,
+						SCHED_PRIORITY_MIN,
+						2000,
+						work_lpthread,
+						(char *const *)NULL);
+
+}
+
+/****************************************************************************
+ * Name: work_hpthread, work_lpthread, and work_usrthread
+ *
+ * Description:
+ *   These are the worker threads that performs actions placed on the work
+ *   lists.
+ *
+ *   work_hpthread and work_lpthread:  These are the kernel mode work queues
+ *     (also build in the flat build).  One of these threads also performs
+ *     periodic garbage collection (that is otherwise performed by the idle
+ *     thread if CONFIG_SCHED_WORKQUEUE is not defined).
+ *
+ *     These worker threads are started by the OS during normal bringup.
+ *
+ *   work_usrthread:  This is a user mode work queue.  It must be built into
+ *     the applicatino blob during the user phase of a kernel build.  The
+ *     user work thread will then automatically be started when the system
+ *     boots by calling through the pointer found in the header on the user
+ *     space blob.
+ *
+ *   All of these entrypoints are referenced by OS internally and should not
+ *   not be accessed by application logic.
+ *
+ * Input parameters:
+ *   argc, argv (not used)
+ *
+ * Returned Value:
+ *   Does not return
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_SCHED_HPWORK
+
+int work_hpthread(int argc, char *argv[])
+{
+	/* Loop forever */
+
+	for (;;) {
+		/* First, perform garbage collection.  This cleans-up memory de-allocations
+		 * that were queued because they could not be freed in that execution
+		 * context (for example, if the memory was freed from an interrupt handler).
+		 * NOTE: If the work thread is disabled, this clean-up is performed by
+		 * the IDLE thread (at a very, very low priority).
+		 */
+
+#ifndef CONFIG_SCHED_LPWORK
+		sched_garbagecollection();
+#endif
+
+		/* Then process queued work.  We need to keep interrupts disabled while
+		 * we process items in the work list.
+		 */
+
+		work_process(&g_work[HPWORK], HPWORK);
+	}
+
+	return PX4_OK; /* To keep some compilers happy */
+}
+
+#ifdef CONFIG_SCHED_LPWORK
+
+int work_lpthread(int argc, char *argv[])
+{
+	/* Loop forever */
+
+	for (;;) {
+		/* First, perform garbage collection.  This cleans-up memory de-allocations
+		 * that were queued because they could not be freed in that execution
+		 * context (for example, if the memory was freed from an interrupt handler).
+		 * NOTE: If the work thread is disabled, this clean-up is performed by
+		 * the IDLE thread (at a very, very low priority).
+		 */
+
+		//sched_garbagecollection();
+
+		/* Then process queued work.  We need to keep interrupts disabled while
+		 * we process items in the work list.
+		 */
+
+		work_process(&g_work[LPWORK], LPWORK);
+	}
+
+	return PX4_OK; /* To keep some compilers happy */
+}
+
+#endif /* CONFIG_SCHED_LPWORK */
+#endif /* CONFIG_SCHED_HPWORK */
+
+#ifdef CONFIG_SCHED_USRWORK
+
+int work_usrthread(int argc, char *argv[])
+{
+	/* Loop forever */
+
+	int rv;
+	// set the threads name
+#ifdef __PX4_DARWIN
+	rv = pthread_setname_np("USR");
+#else
+	rv = pthread_setname_np(pthread_self(), "USR");
+#endif
+
+	for (;;) {
+		/* Then process queued work.  We need to keep interrupts disabled while
+		 * we process items in the work list.
+		 */
+
+		work_process(&g_work[USRWORK], USRWORK);
+	}
+
+	return PX4_OK; /* To keep some compilers happy */
+}
+
+#endif /* CONFIG_SCHED_USRWORK */
+
+uint32_t clock_systimer()
+{
+	//printf("clock_systimer: %0lx\n", hrt_absolute_time());
+	return (0x00000000ffffffff & hrt_absolute_time());
+}
+#endif /* CONFIG_SCHED_WORKQUEUE */

platforms/common/work_queue/wqueue_test/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE platforms__posix__tests__wqueue
+	MAIN wqueue_test
+	SRCS
+		wqueue_main.cpp
+		wqueue_start_posix.cpp
+		wqueue_test.cpp
+	DEPENDS
+	)

platforms/common/work_queue/wqueue_test/wqueue_main.cpp

@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   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 "wqueue_test.h"
+
+#include <px4_log.h>
+#include <px4_middleware.h>
+#include <px4_app.h>
+#include <stdio.h>
+
+int PX4_MAIN(int argc, char **argv)
+{
+	px4::init(argc, argv, "wqueue_test");
+
+	PX4_INFO("wqueue hello\n");
+	WQueueTest wq;
+	wq.main();
+
+	PX4_INFO("goodbye\n");
+	return 0;
+}

platforms/common/work_queue/wqueue_test/wqueue_start_posix.cpp

@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   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_posix.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Mark Charlebois <mcharleb@gmail.com>
+ */
+#include "wqueue_test.h"
+
+#include <px4_app.h>
+#include <px4_log.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) {
+		PX4_INFO("usage: wqueue_test {start|stop|status}\n");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (WQueueTest::appState.isRunning()) {
+			PX4_INFO("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 *)nullptr);
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		WQueueTest::appState.requestExit();
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (WQueueTest::appState.isRunning()) {
+			PX4_INFO("is running\n");
+
+		} else {
+			PX4_INFO("not started\n");
+		}
+
+		return 0;
+	}
+
+	PX4_INFO("usage: wqueue_test {start|stop|status}\n");
+	return 1;
+}

platforms/common/work_queue/wqueue_test/wqueue_test.cpp

@@ -0,0 +1,113 @@
+
+/****************************************************************************
+ *
+ *   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 "wqueue_test.h"
+
+#include <px4_time.h>
+#include <px4_workqueue.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()
+{
+	static int iter = 0;
+	printf("done lp work\n");
+
+	if (iter > 5) {
+		_lpwork_done = true;
+	}
+
+	++iter;
+
+	work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1000);
+}
+
+void WQueueTest::do_hp_work()
+{
+	static int iter = 0;
+	printf("done hp work\n");
+
+	if (iter > 5) {
+		_hpwork_done = true;
+	}
+
+	++iter;
+
+	// requeue
+	work_queue(HPWORK, &_hpwork, (worker_t)&hp_worker_cb, this, 1000);
+}
+
+int WQueueTest::main()
+{
+	appState.setRunning(true);
+
+	//Put work on HP work queue
+	work_queue(HPWORK, &_hpwork, (worker_t)&hp_worker_cb, this, 1000);
+
+
+	//Put work on LP work queue
+	work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1000);
+
+
+	// Wait for work to finsh
+	while (!appState.exitRequested() && !(_hpwork_done && _lpwork_done)) {
+		printf("  Sleeping for 2 sec...\n");
+		sleep(2);
+	}
+
+	return 0;
+}

platforms/common/work_queue/wqueue_test/wqueue_test.h

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   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;
+};

platforms/nuttx/src/px4/common/px4_init.cpp

@@ -38,7 +38,7 @@
 #include <px4_defines.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
-#include <px4_work_queue/WorkQueueManager.hpp>
+#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
 #include <systemlib/cpuload.h>
 
 #include <fcntl.h>

platforms/posix/src/px4/common/px4_init.cpp

@@ -37,7 +37,7 @@
 #include <px4_defines.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
-#include <px4_work_queue/WorkQueueManager.hpp>
+#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
 
 int px4_platform_init(void)
 {

platforms/qurt/src/px4/common/px4_init.cpp

@@ -37,7 +37,7 @@
 #include <px4_defines.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
-#include <px4_work_queue/WorkQueueManager.hpp>
+#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
 
 int px4_platform_init(void)
 {