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