PX4 general work queue

.ci/Jenkinsfile-compile

@@ -43,7 +43,7 @@ pipeline {
           ]
 
           def nuttx_builds_other = [
-            target: ["px4_cannode-v1_default", "px4_esc-v1_default", "thiemar_s2740vc-v1_default"],
+            target: ["px4_esc-v1_default", "thiemar_s2740vc-v1_default"],
             image: docker_images.nuttx,
             archive: false
           ]

platforms/nuttx/src/px4_layer/CMakeLists.txt

@@ -44,6 +44,7 @@ if (NOT ${PX4_BOARD} MATCHES "px4_io")
 			nuttx_apps # up_cxxinitialize
 			nuttx_sched
 			drivers_boards_common_arch
+			px4_work_queue
 		)
 else()
 	add_library(px4_layer ${PX4_SOURCE_DIR}/src/platforms/empty.c)

platforms/nuttx/src/px4_layer/px4_init.cpp

@@ -37,6 +37,7 @@
 #include <px4_defines.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
+#include <px4_work_queue/WorkQueueManager.hpp>
 #include <systemlib/cpuload.h>
 
 #include <fcntl.h>
@@ -104,5 +105,7 @@ int px4_platform_init(void)
 	cpuload_initialize_once();
 #endif
 
+	px4::WorkQueueManagerStart();
+
 	return PX4_OK;
 }

platforms/posix/CMakeLists.txt

@@ -62,10 +62,11 @@ else()
 			${df_driver_libs}
 			df_driver_framework
 			pthread m
-			
+
 			# horrible circular dependencies that need to be teased apart
-			px4_layer
+			px4_layer px4_platform
-			px4_platform
+			work_queue
+			parameters
 	)
 
 	if (NOT APPLE)

platforms/posix/src/px4_layer/CMakeLists.txt

@@ -55,7 +55,7 @@ add_library(px4_layer
 )
 target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4")
 target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable
-target_link_libraries(px4_layer PRIVATE work_queue)
+target_link_libraries(px4_layer PRIVATE work_queue px4_work_queue)
 target_link_libraries(px4_layer PRIVATE px4_daemon)
 
 if(ENABLE_LOCKSTEP_SCHEDULER)

platforms/posix/src/px4_layer/px4_init.cpp

@@ -37,6 +37,7 @@
 #include <px4_defines.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
+#include <px4_work_queue/WorkQueueManager.hpp>
 
 int px4_platform_init(void)
 {
@@ -44,5 +45,7 @@ int px4_platform_init(void)
 
 	param_init();
 
+	px4::WorkQueueManagerStart();
+
 	return PX4_OK;
 }

platforms/qurt/src/px4_layer/px4_init.cpp

@@ -37,6 +37,7 @@
 #include <px4_defines.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
+#include <px4_work_queue/WorkQueueManager.hpp>
 
 int px4_platform_init(void)
 {
@@ -44,5 +45,7 @@ int px4_platform_init(void)
 
 	param_init();
 
+	px4::WorkQueueManagerStart();
+
 	return PX4_OK;
 }

src/include/containers/BlockingList.hpp

@@ -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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file BlockingList.hpp
+ *
+ * A blocking intrusive linked list.
+ */
+
+#pragma once
+
+#include "List.hpp"
+#include "LockGuard.hpp"
+
+#include <pthread.h>
+#include <stdlib.h>
+
+template<class T>
+class BlockingList : public List<T>
+{
+public:
+
+	~BlockingList()
+	{
+		pthread_mutex_destroy(&_mutex);
+		pthread_cond_destroy(&_cv);
+	}
+
+	void add(T newNode)
+	{
+		LockGuard lg{_mutex};
+		List<T>::add(newNode);
+	}
+
+	bool remove(T removeNode)
+	{
+		LockGuard lg{_mutex};
+		return List<T>::remove(removeNode);
+	}
+
+	size_t size() const
+	{
+		LockGuard lg{_mutex};
+		return List<T>::size();
+	}
+
+	void clear()
+	{
+		LockGuard lg{_mutex};
+		List<T>::clear();
+	}
+
+	LockGuard getLockGuard() { return LockGuard{_mutex}; }
+
+private:
+
+	pthread_mutex_t	_mutex = PTHREAD_MUTEX_INITIALIZER;
+	pthread_cond_t	_cv = PTHREAD_COND_INITIALIZER;
+
+};

src/include/containers/BlockingQueue.hpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   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_sem.h>
+
+#include "LockGuard.hpp"
+
+template<class T, size_t N>
+class BlockingQueue
+{
+public:
+
+	BlockingQueue()
+	{
+		px4_sem_init(&_sem_head, 0, N);
+		px4_sem_init(&_sem_tail, 0, 0);
+	}
+
+	~BlockingQueue()
+	{
+		px4_sem_destroy(&_sem_head);
+		px4_sem_destroy(&_sem_tail);
+	}
+
+	void push(T newItem)
+	{
+		px4_sem_wait(&_sem_head);
+
+		_data[_tail] = newItem;
+		_tail = (_tail + 1) % N;
+
+		px4_sem_post(&_sem_tail);
+	}
+
+	T pop()
+	{
+		px4_sem_wait(&_sem_tail);
+
+		T ret = _data[_head];
+		_head = (_head + 1) % N;
+
+		px4_sem_post(&_sem_head);
+
+		return ret;
+	}
+
+private:
+
+	px4_sem_t	_sem_head;
+	px4_sem_t	_sem_tail;
+
+	T _data[N];
+
+	size_t _head{0};
+	size_t _tail{0};
+
+};

src/include/containers/List.hpp

@@ -46,12 +46,12 @@ class ListNode
 {
 public:
 
-	void setSibling(T sibling) { _sibling = sibling; }
+	void setSibling(T sibling) { _list_node_sibling = sibling; }
-	const T getSibling() const { return _sibling; }
+	const T getSibling() const { return _list_node_sibling; }
 
 protected:
 
-	T _sibling{nullptr};
+	T _list_node_sibling{nullptr};
 
 };
 

src/include/containers/LockGuard.hpp

@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   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 <pthread.h>
+
+class LockGuard
+{
+public:
+	explicit LockGuard(pthread_mutex_t &mutex) :
+		_mutex(mutex)
+	{
+		pthread_mutex_lock(&_mutex);
+	}
+
+	~LockGuard()
+	{
+		pthread_mutex_unlock(&_mutex);
+	}
+
+private:
+	pthread_mutex_t &_mutex;
+};

src/platforms/common/CMakeLists.txt

@@ -52,4 +52,5 @@ 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)

src/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)

src/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 "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

src/platforms/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

src/platforms/common/px4_work_queue/WorkItem.cpp

@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   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 "WorkItem.hpp"
+
+#include "WorkQueue.hpp"
+#include "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");
+	}
+}
+
+bool WorkItem::Init(const wq_config_t &config)
+{
+	px4::WorkQueue *wq = WorkQueueFindOrCreate(config);
+
+	if (wq == nullptr) {
+		PX4_ERR("%s not available", config.name);
+
+	} else {
+		_wq = wq;
+		return true;
+	}
+
+	return false;
+}
+
+} // namespace px4

src/platforms/common/px4_work_queue/WorkItem.hpp

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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() = default;
+
+	inline void ScheduleNow() { if (_wq != nullptr) _wq->Add(this); }
+
+	virtual void Run() = 0;
+
+protected:
+
+	bool Init(const wq_config_t &config);
+
+private:
+
+	WorkQueue *_wq{nullptr};
+
+};
+
+} // namespace px4

src/platforms/common/px4_work_queue/WorkQueue.cpp

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   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 "WorkQueue.hpp"
+#include "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)
+{
+	work_lock();
+	_q.push(item);
+	work_unlock();
+
+	// Wake up the worker thread
+	px4_sem_post(&_process_lock);
+}
+
+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

src/platforms/common/px4_work_queue/WorkQueue.hpp

@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   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);
+
+	// TODO: need helpers to handle clean shutdown - remove and clear?
+	//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

src/platforms/common/px4_work_queue/WorkQueueManager.cpp

@@ -0,0 +1,289 @@
+/****************************************************************************
+ *
+ *   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 "WorkQueueManager.hpp"
+
+#include "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
+			const size_t stacksize = math::max(PTHREAD_STACK_MIN, PX4_STACK_ADJUSTED(wq->stacksize));
+			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);
+			}
+
+			// 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);
+			}
+
+			// 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

src/platforms/common/px4_work_queue/WorkQueueManager.hpp

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   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", 1500, 0}; // PX4 inner loop highest priority
+
+static constexpr wq_config_t SPI1{"wq:SPI1", 1200, -1};
+static constexpr wq_config_t SPI2{"wq:SPI2", 1200, -2};
+static constexpr wq_config_t SPI3{"wq:SPI3", 1200, -3};
+static constexpr wq_config_t SPI4{"wq:SPI4", 1200, -4};
+static constexpr wq_config_t SPI5{"wq:SPI5", 1200, -5};
+static constexpr wq_config_t SPI6{"wq:SPI6", 1200, -6};
+
+static constexpr wq_config_t I2C1{"wq:I2C1", 1200, -7};
+static constexpr wq_config_t I2C2{"wq:I2C2", 1200, -8};
+static constexpr wq_config_t I2C3{"wq:I2C3", 1200, -9};
+static constexpr wq_config_t I2C4{"wq:I2C4", 1200, -10};
+
+static constexpr wq_config_t hp_default{"wq:hp_default", 1200, -11};
+static constexpr wq_config_t lp_default{"wq:lp_default", 1200, -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

src/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
+	)

src/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;
+}

src/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;
+}

src/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_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};
+};

src/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) ? (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;
+}

src/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;
+}

src/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_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};
+};

src/platforms/px4_tasks.h

@@ -100,6 +100,9 @@ typedef struct {
 #error "No target OS defined"
 #endif
 
+// PX4 work queue starting high priority
+#define PX4_WQ_HP_BASE (SCHED_PRIORITY_MAX - 11)
+
 // Fast drivers - they need to run as quickly as possible to minimize control
 // latency.
 #define SCHED_PRIORITY_FAST_DRIVER		(SCHED_PRIORITY_MAX - 0)
@@ -108,37 +111,37 @@ typedef struct {
 // they should be the first to run on an update, using the current sensor
 // data and the *previous* attitude reference from the position controller
 // which typically runs at a slower rate
-#define SCHED_PRIORITY_ATTITUDE_CONTROL		(SCHED_PRIORITY_MAX - 4)
+#define SCHED_PRIORITY_ATTITUDE_CONTROL		(PX4_WQ_HP_BASE - 4)
 
 // Actuator outputs should run as soon as the rate controller publishes
 // the actuator controls topic
-#define SCHED_PRIORITY_ACTUATOR_OUTPUTS		(SCHED_PRIORITY_MAX - 3)
+#define SCHED_PRIORITY_ACTUATOR_OUTPUTS		(PX4_WQ_HP_BASE - 3)
 
 // Position controllers typically are in a blocking wait on estimator data
 // so when new sensor data is available they will run last. Keeping them
 // on a high priority ensures that they are the first process to be run
 // when the estimator updates.
-#define SCHED_PRIORITY_POSITION_CONTROL		(SCHED_PRIORITY_MAX - 5)
+#define SCHED_PRIORITY_POSITION_CONTROL		(PX4_WQ_HP_BASE - 5)
 
 // Estimators should run after the attitude controller but before anything
 // else in the system. They wait on sensor data which is either coming
 // from the sensor hub or from a driver. Keeping this class at a higher
 // priority ensures that the estimator runs first if it can, but will
 // wait for the sensor hub if its data is coming from it.
-#define SCHED_PRIORITY_ESTIMATOR		(SCHED_PRIORITY_MAX - 5)
+#define SCHED_PRIORITY_ESTIMATOR		(PX4_WQ_HP_BASE - 5)
 
 // The sensor hub conditions sensor data. It is not the fastest component
 // in the controller chain, but provides easy-to-use data to the more
 // complex downstream consumers
-#define SCHED_PRIORITY_SENSOR_HUB		(SCHED_PRIORITY_MAX - 6)
+#define SCHED_PRIORITY_SENSOR_HUB		(PX4_WQ_HP_BASE - 6)
 
 // The log capture (which stores log data into RAM) should run faster
 // than other components, but should not run before the control pipeline
-#define SCHED_PRIORITY_LOG_CAPTURE		(SCHED_PRIORITY_MAX - 10)
+#define SCHED_PRIORITY_LOG_CAPTURE		(PX4_WQ_HP_BASE - 10)
 
 // Slow drivers should run at a rate where they do not impact the overall
 // system execution
-#define SCHED_PRIORITY_SLOW_DRIVER		(SCHED_PRIORITY_MAX - 35)
+#define SCHED_PRIORITY_SLOW_DRIVER		(PX4_WQ_HP_BASE - 35)
 
 // The navigation system needs to execute regularly but has no realtime needs
 #define SCHED_PRIORITY_NAVIGATION		(SCHED_PRIORITY_DEFAULT + 5)