px4_work_queue: command line status output and shutdown empty queues

* adds a work_queue systemcmd that will bring a tree view of all active work queues and work items
 * WorkQueues now track attached WorkItems and will shutdown when the last WorkItem is detached

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

@@ -44,9 +44,6 @@ class ScheduledWorkItem : public WorkItem
 {
 public:
 
-	ScheduledWorkItem(const wq_config_t &config) : WorkItem(config) {}
-	virtual ~ScheduledWorkItem() override;
-
 	/**
 	 * Schedule next run with a delay in microseconds.
 	 *
@@ -58,7 +55,7 @@ public:
 	 * Schedule repeating run with optional delay.
 	 *
 	 * @param interval_us		The interval in microseconds.
-	 * @param delay_us			The delay (optional) in microseconds.
+	 * @param delay_us		The delay (optional) in microseconds.
 	 */
 	void ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us = 0);
 
@@ -67,14 +64,20 @@ public:
 	 */
 	void ScheduleClear();
 
-	virtual void Run() override = 0;
+protected:
+
+	ScheduledWorkItem(const char *name, const wq_config_t &config) : WorkItem(name, config) {}
+	virtual ~ScheduledWorkItem() override;
+
+	virtual void print_run_status() const override;
 
 private:
 
+	virtual void Run() override = 0;
+
 	static void	schedule_trampoline(void *arg);
 
 	hrt_call	_call{};
-
 };
 
 } // namespace px4

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

@@ -33,7 +33,6 @@
 
 #pragma once
 
-
 #include "WorkQueueManager.hpp"
 #include "WorkQueue.hpp"
 
@@ -41,21 +40,18 @@
 #include <px4_defines.h>
 #include <drivers/drv_hrt.h>
 
+#include <lib/perf/perf_counter.h>
+
 namespace px4
 {
 
-class WorkItem : public IntrusiveQueueNode<WorkItem *>
+class WorkItem : public ListNode<WorkItem *>, public IntrusiveQueueNode<WorkItem *>
 {
 public:
 
-	explicit WorkItem(const wq_config_t &config);
-	WorkItem() = delete;
-
-	virtual ~WorkItem();
-
 	inline void ScheduleNow() { if (_wq != nullptr) _wq->Add(this); }
 
-	virtual void Run() = 0;
+	virtual void print_run_status() const;
 
 	/**
 	 * Switch to a different WorkQueue.
@@ -68,6 +64,18 @@ public:
 
 protected:
 
+	explicit WorkItem(const char *name, const wq_config_t &config);
+	WorkItem() = delete;
+
+	virtual ~WorkItem();
+
+protected:
+
+	void RunPreamble() { _run_count++; }
+
+	friend void WorkQueue::Run();
+	virtual void Run() = 0;
+
 	/**
 	 * Initialize WorkItem given a WorkQueue config. This call
 	 * can also be used to switch to a different WorkQueue.
@@ -79,9 +87,18 @@ protected:
 	bool Init(const wq_config_t &config);
 	void Deinit();
 
+	float elapsed_time() const;
+	float average_rate() const;
+	float average_interval() const;
+
+
+	hrt_abstime	_start_time{0};
+	unsigned	_run_count{0};
+	const char 	*_item_name;
+
 private:
 
-	WorkQueue *_wq{nullptr};
+	WorkQueue	*_wq{nullptr};
 
 };
 

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

@@ -35,6 +35,7 @@
 
 #include "WorkQueueManager.hpp"
 
+#include <containers/BlockingList.hpp>
 #include <containers/List.hpp>
 #include <containers/IntrusiveQueue.hpp>
 #include <px4_atomic.h>
@@ -57,6 +58,9 @@ public:
 
 	const char *get_name() { return _config.name; }
 
+	bool Attach(WorkItem *item);
+	void Detach(WorkItem *item);
+
 	void Add(WorkItem *item);
 	void Remove(WorkItem *item);
 
@@ -66,7 +70,7 @@ public:
 
 	void request_stop() { _should_exit.store(true); }
 
-	void print_status();
+	void print_status(bool last = false);
 
 private:
 
@@ -84,10 +88,10 @@ private:
 #endif
 
 	IntrusiveQueue<WorkItem *>	_q;
-	px4_sem_t		_process_lock;
-
-	px4::atomic_bool	_should_exit{false};
-	const wq_config_t	&_config;
+	px4_sem_t			_process_lock;
+	const wq_config_t		&_config;
+	BlockingList<WorkItem *>	_work_items;
+	px4::atomic_bool		_should_exit{false};
 
 };
 

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

@@ -82,6 +82,11 @@ int WorkQueueManagerStart();
  */
 int WorkQueueManagerStop();
 
+/**
+ * Work queue manager status.
+ */
+int WorkQueueManagerStatus();
+
 /**
  * Create (or find) a work queue with a particular configuration.
  *

platforms/common/px4_work_queue/ScheduledWorkItem.cpp

@@ -41,25 +41,44 @@ ScheduledWorkItem::~ScheduledWorkItem()
 	ScheduleClear();
 }
 
-void ScheduledWorkItem::schedule_trampoline(void *arg)
+void
+ScheduledWorkItem::schedule_trampoline(void *arg)
 {
 	ScheduledWorkItem *dev = reinterpret_cast<ScheduledWorkItem *>(arg);
 	dev->ScheduleNow();
 }
 
-void ScheduledWorkItem::ScheduleDelayed(uint32_t delay_us)
+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)
+void
+ScheduledWorkItem::ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us)
 {
+	// reset start time to first deadline (approximately)
+	_start_time = hrt_absolute_time() + interval_us + delay_us;
+
 	hrt_call_every(&_call, delay_us, interval_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this);
 }
 
-void ScheduledWorkItem::ScheduleClear()
+void
+ScheduledWorkItem::ScheduleClear()
 {
 	hrt_cancel(&_call);
 }
 
+void
+ScheduledWorkItem::print_run_status() const
+{
+	if (_call.period > 0) {
+		PX4_INFO_RAW("%-24s %8.1f Hz %12.1f us (%" PRId64 " us)\n", _item_name, (double)average_rate(),
+			     (double)average_interval(), _call.period);
+
+	} else {
+		WorkItem::print_run_status();
+	}
+}
+
 } // namespace px4

platforms/common/px4_work_queue/WorkItem.cpp

@@ -42,7 +42,8 @@
 namespace px4
 {
 
-WorkItem::WorkItem(const wq_config_t &config)
+WorkItem::WorkItem(const char *name, const wq_config_t &config) :
+	_item_name(name)
 {
 	if (!Init(config)) {
 		PX4_ERR("init failed");
@@ -54,25 +55,26 @@ WorkItem::~WorkItem()
 	Deinit();
 }
 
-bool WorkItem::Init(const wq_config_t &config)
+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 {
+	if ((wq != nullptr) && wq->Attach(this)) {
 		_wq = wq;
+		_start_time = hrt_absolute_time();
 		return true;
 	}
 
+	PX4_ERR("%s not available", config.name);
 	return false;
 }
 
-void WorkItem::Deinit()
+void
+WorkItem::Deinit()
 {
 	// remove any currently queued work
 	if (_wq != nullptr) {
@@ -80,8 +82,47 @@ void WorkItem::Deinit()
 		px4::WorkQueue *wq_temp = _wq;
 		_wq = nullptr;
 
+		// remove any queued work
 		wq_temp->Remove(this);
+
+		wq_temp->Detach(this);
 	}
 }
 
+float
+WorkItem::elapsed_time() const
+{
+	return hrt_elapsed_time(&_start_time) / 1e6f;
+}
+
+float
+WorkItem::average_rate() const
+{
+	const float rate = _run_count / elapsed_time();
+
+	if (PX4_ISFINITE(rate)) {
+		return rate;
+	}
+
+	return 0.0f;
+}
+
+float
+WorkItem::average_interval() const
+{
+	const float interval = 1000000.0f / average_rate();
+
+	if (PX4_ISFINITE(interval)) {
+		return interval;
+	}
+
+	return 0.0f;
+}
+
+void
+WorkItem::print_run_status() const
+{
+	PX4_INFO_RAW("%-24s %8.1f Hz %12.1f us\n", _item_name, (double)average_rate(), (double)average_interval());
+}
+
 } // namespace px4

platforms/common/px4_work_queue/WorkQueue.cpp

@@ -72,10 +72,44 @@ WorkQueue::~WorkQueue()
 #endif /* __PX4_NUTTX */
 }
 
-void WorkQueue::Add(WorkItem *item)
+bool
+WorkQueue::Attach(WorkItem *item)
 {
-	// TODO: prevent additions when shutting down
+	work_lock();
 
+	if (!should_exit()) {
+		_work_items.add(item);
+		work_unlock();
+		return true;
+	}
+
+	work_unlock();
+	return false;
+}
+
+void
+WorkQueue::Detach(WorkItem *item)
+{
+	work_lock();
+
+	_work_items.remove(item);
+
+	if (_work_items.size() == 0) {
+		// shutdown, no active WorkItems
+		PX4_DEBUG("stopping: %s, last active WorkItem closing", _config.name);
+
+		request_stop();
+
+		// Wake up the worker thread
+		px4_sem_post(&_process_lock);
+	}
+
+	work_unlock();
+}
+
+void
+WorkQueue::Add(WorkItem *item)
+{
 	work_lock();
 	_q.push(item);
 	work_unlock();
@@ -84,14 +118,16 @@ void WorkQueue::Add(WorkItem *item)
 	px4_sem_post(&_process_lock);
 }
 
-void WorkQueue::Remove(WorkItem *item)
+void
+WorkQueue::Remove(WorkItem *item)
 {
 	work_lock();
 	_q.remove(item);
 	work_unlock();
 }
 
-void WorkQueue::Clear()
+void
+WorkQueue::Clear()
 {
 	work_lock();
 
@@ -102,7 +138,8 @@ void WorkQueue::Clear()
 	work_unlock();
 }
 
-void WorkQueue::Run()
+void
+WorkQueue::Run()
 {
 	while (!should_exit()) {
 		px4_sem_wait(&_process_lock);
@@ -114,17 +151,43 @@ void WorkQueue::Run()
 			WorkItem *work = _q.pop();
 
 			work_unlock(); // unlock work queue to run (item may requeue itself)
+			work->RunPreamble();
 			work->Run();
 			work_lock(); // re-lock
 		}
 
 		work_unlock();
 	}
+
+	PX4_DEBUG("%s: exiting", _config.name);
 }
 
-void WorkQueue::print_status()
+void
+WorkQueue::print_status(bool last)
 {
-	PX4_INFO("WorkQueue: %s running", get_name());
+	const size_t num_items = _work_items.size();
+	PX4_INFO_RAW("%-16s\n", get_name());
+	size_t i = 0;
+
+	for (WorkItem *item : _work_items) {
+		i++;
+
+		if (last) {
+			PX4_INFO_RAW("    ");
+
+		} else {
+			PX4_INFO_RAW("|   ");
+		}
+
+		if (i < num_items) {
+			PX4_INFO_RAW("|__ %zu) ", i);
+
+		} else {
+			PX4_INFO_RAW("\\__ %zu) ", i);
+		}
+
+		item->print_run_status();
+	}
 }
 
 } // namespace px4

platforms/common/px4_work_queue/WorkQueueManager.cpp

@@ -59,10 +59,11 @@ 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 px4::atomic_bool _wq_manager_should_exit{true};
 
 
-static WorkQueue *FindWorkQueueByName(const char *name)
+static WorkQueue *
+FindWorkQueueByName(const char *name)
 {
 	if (_wq_manager_wqs_list == nullptr) {
 		PX4_ERR("not running");
@@ -81,7 +82,8 @@ static WorkQueue *FindWorkQueueByName(const char *name)
 	return nullptr;
 }
 
-WorkQueue *WorkQueueFindOrCreate(const wq_config_t &new_wq)
+WorkQueue *
+WorkQueueFindOrCreate(const wq_config_t &new_wq)
 {
 	if (_wq_manager_create_queue == nullptr) {
 		PX4_ERR("not running");
@@ -116,7 +118,8 @@ WorkQueue *WorkQueueFindOrCreate(const wq_config_t &new_wq)
 	return wq;
 }
 
-const wq_config_t &device_bus_to_wq(uint32_t device_id_int)
+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;
@@ -155,7 +158,8 @@ const wq_config_t &device_bus_to_wq(uint32_t device_id_int)
 	return wq_configurations::hp_default;
 };
 
-static void *WorkQueueRunner(void *context)
+static void *
+WorkQueueRunner(void *context)
 {
 	wq_config_t *config = static_cast<wq_config_t *>(context);
 	WorkQueue wq(*config);
@@ -171,7 +175,8 @@ static void *WorkQueueRunner(void *context)
 	return nullptr;
 }
 
-static int WorkQueueManagerRun(int, char **)
+static int
+WorkQueueManagerRun(int, char **)
 {
 	_wq_manager_wqs_list = new BlockingList<WorkQueue *>();
 	_wq_manager_create_queue = new BlockingQueue<const wq_config_t *, 1>();
@@ -232,7 +237,7 @@ static int WorkQueueManagerRun(int, char **)
 			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);
+				PX4_DEBUG("starting: %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));
@@ -250,46 +255,113 @@ static int WorkQueueManagerRun(int, char **)
 	return 0;
 }
 
-int WorkQueueManagerStart()
+int
+WorkQueueManagerStart()
 {
-	int task_id = px4_task_spawn_cmd("wq:manager",
-					 SCHED_DEFAULT,
-					 PX4_WQ_HP_BASE,
-					 1200,
-					 (px4_main_t)&WorkQueueManagerRun,
-					 nullptr);
+	if (_wq_manager_should_exit.load() && (_wq_manager_create_queue == nullptr)) {
 
-	if (task_id < 0) {
-		PX4_ERR("task start failed (%i)", task_id);
-		return -errno;
-	}
+		_wq_manager_should_exit.store(false);
 
-	return 0;
-}
+		int task_id = px4_task_spawn_cmd("wq:manager",
+						 SCHED_DEFAULT,
+						 PX4_WQ_HP_BASE,
+						 1280,
+						 (px4_main_t)&WorkQueueManagerRun,
+						 nullptr);
 
-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();
+		if (task_id < 0) {
+			_wq_manager_should_exit.store(true);
+			PX4_ERR("task start failed (%i)", task_id);
+			return -errno;
 		}
 
-		delete _wq_manager_wqs_list;
+	} else {
+		PX4_WARN("already running");
+		return PX4_ERROR;
 	}
 
-	_wq_manager_should_exit.store(true);
+	return PX4_OK;
+}
 
-	if (_wq_manager_create_queue != nullptr) {
-		// push nullptr to wake the wq manager task
-		_wq_manager_create_queue->push(nullptr);
+int
+WorkQueueManagerStop()
+{
+	if (!_wq_manager_should_exit.load()) {
 
-		px4_usleep(1000);
+		// error can't shutdown until all WorkItems are removed/stopped
+		if ((_wq_manager_wqs_list != nullptr) && (_wq_manager_wqs_list->size() > 0)) {
+			PX4_ERR("can't shutdown with active WQs");
+			WorkQueueManagerStatus();
+			return PX4_ERROR;
+		}
 
-		delete _wq_manager_create_queue;
+		// first ask all WQs to stop
+		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();
+				}
+			}
+
+			// wait until they're all stopped (empty list)
+			while (_wq_manager_wqs_list->size() > 0) {
+				px4_usleep(1000);
+			}
+
+			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(10000);
+
+			delete _wq_manager_create_queue;
+		}
+
+	} else {
+		PX4_WARN("not running");
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+int
+WorkQueueManagerStatus()
+{
+	if (!_wq_manager_should_exit.load() && (_wq_manager_wqs_list != nullptr)) {
+
+		const size_t num_wqs = _wq_manager_wqs_list->size();
+		PX4_INFO_RAW("\nWork Queue: %-1zu threads                      RATE        INTERVAL\n", num_wqs);
+
+		auto lg = _wq_manager_wqs_list->getLockGuard();
+		size_t i = 0;
+
+		for (WorkQueue *wq : *_wq_manager_wqs_list) {
+			i++;
+
+			const bool last_wq = !(i < num_wqs);
+
+			if (!last_wq) {
+				PX4_INFO_RAW("|__ %zu) ", i);
+
+			} else {
+				PX4_INFO_RAW("\\__ %zu) ", i);
+			}
+
+			wq->print_status(last_wq);
+		}
+
+	} else {
+		PX4_INFO("not running");
 	}
 
 	return PX4_OK;

platforms/common/px4_work_queue/test/wqueue_scheduled_test.h

@@ -42,7 +42,7 @@ using namespace px4;
 class WQueueScheduledTest : public px4::ScheduledWorkItem
 {
 public:
-	WQueueScheduledTest() : px4::ScheduledWorkItem(px4::wq_configurations::test2) {}
+	WQueueScheduledTest() : px4::ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test2) {}
 	~WQueueScheduledTest() = default;
 
 	int main();

platforms/common/px4_work_queue/test/wqueue_test.h

@@ -42,7 +42,7 @@ using namespace px4;
 class WQueueTest : public px4::WorkItem
 {
 public:
-	WQueueTest() : px4::WorkItem(px4::wq_configurations::test1) {}
+	WQueueTest() : px4::WorkItem("WQueueTest", px4::wq_configurations::test1) {}
 	~WQueueTest() = default;
 
 	int main();