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.
  *