uORB Subscription callbacks with WorkItem scheduling on publication (#12207)

src/platforms/common/px4_work_queue/WorkItem.cpp

@@ -49,8 +49,16 @@ WorkItem::WorkItem(const wq_config_t &config)
 	}
 }
 
+WorkItem::~WorkItem()
+{
+	Deinit();
+}
+
 bool WorkItem::Init(const wq_config_t &config)
 {
+	// clear any existing first
+	Deinit();
+
 	px4::WorkQueue *wq = WorkQueueFindOrCreate(config);
 
 	if (wq == nullptr) {
@@ -64,4 +72,16 @@ bool WorkItem::Init(const wq_config_t &config)
 	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

src/platforms/common/px4_work_queue/WorkItem.hpp

@@ -51,7 +51,7 @@ public:
 	explicit WorkItem(const wq_config_t &config);
 	WorkItem() = delete;
 
-	virtual ~WorkItem() = default;
+	virtual ~WorkItem();
 
 	inline void ScheduleNow() { if (_wq != nullptr) _wq->Add(this); }
 
@@ -60,6 +60,7 @@ public:
 protected:
 
 	bool Init(const wq_config_t &config);
+	void Deinit();
 
 private:
 

src/platforms/common/px4_work_queue/WorkQueue.cpp

@@ -74,6 +74,8 @@ WorkQueue::~WorkQueue()
 
 void WorkQueue::Add(WorkItem *item)
 {
+	// TODO: prevent additions when shutting down
+
 	work_lock();
 	_q.push(item);
 	work_unlock();
@@ -82,6 +84,24 @@ void WorkQueue::Add(WorkItem *item)
 	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()) {

src/platforms/common/px4_work_queue/WorkQueue.hpp

@@ -58,10 +58,9 @@ public:
 	const char *get_name() { return _config.name; }
 
 	void Add(WorkItem *item);
+	void Remove(WorkItem *item);
 
-	// TODO: need helpers to handle clean shutdown - remove and clear?
-	//void remove(WorkItem *item);
-	//void clear();
+	void Clear();
 
 	void Run();
 

src/platforms/common/px4_work_queue/WorkQueueManager.cpp

@@ -198,13 +198,18 @@ static void WorkQueueManagerRun()
 			}
 
 			// 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);
 
@@ -212,6 +217,8 @@ static void WorkQueueManagerRun()
 				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);

src/platforms/common/px4_work_queue/WorkQueueManager.hpp

@@ -48,7 +48,7 @@ struct wq_config_t {
 
 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 rate_ctrl{"wq:rate_ctrl", 1600, 0}; // PX4 inner loop highest priority
 
 static constexpr wq_config_t SPI1{"wq:SPI1", 1250, -1};
 static constexpr wq_config_t SPI2{"wq:SPI2", 1250, -2};
@@ -62,7 +62,9 @@ static constexpr wq_config_t I2C2{"wq:I2C2", 1250, -8};
 static constexpr wq_config_t I2C3{"wq:I2C3", 1250, -9};
 static constexpr wq_config_t I2C4{"wq:I2C4", 1250, -10};
 
-static constexpr wq_config_t hp_default{"wq:hp_default", 1500, -11};
+static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 7000, -11}; // PX4 att/pos controllers, highest priority after sensors
+
+static constexpr wq_config_t hp_default{"wq:hp_default", 1500, -12};
 static constexpr wq_config_t lp_default{"wq:lp_default", 1500, -50};
 
 static constexpr wq_config_t test1{"wq:test1", 800, 0};