WorkQueue: avoid potential semaphore counter overflow

This could happen in the following cases:
- IRQ/publisher rate is faster than the processing rate, and therefore
  WorkQueue::Add is called at a higher rate
- a long-running or stuck task that blocks the work queue a long time

Both cases are not expected to happen under 'normal' circumstances (if the
system runs as expected).

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

@@ -76,6 +76,8 @@ private:
 
 	bool should_exit() const { return _should_exit.load(); }
 
+	inline void signal_worker_thread();
+
 #ifdef __PX4_NUTTX
 	// In NuttX work can be enqueued from an ISR
 	void work_lock() { _flags = enter_critical_section(); }

platforms/common/px4_work_queue/WorkQueue.cpp

@@ -99,9 +99,7 @@ WorkQueue::Detach(WorkItem *item)
 		PX4_DEBUG("stopping: %s, last active WorkItem closing", _config.name);
 
 		request_stop();
-
-		// Wake up the worker thread
-		px4_sem_post(&_process_lock);
+		signal_worker_thread();
 	}
 
 	work_unlock();
@@ -114,8 +112,17 @@ WorkQueue::Add(WorkItem *item)
 	_q.push(item);
 	work_unlock();
 
-	// Wake up the worker thread
-	px4_sem_post(&_process_lock);
+	signal_worker_thread();
+}
+
+void
+WorkQueue::signal_worker_thread()
+{
+	int sem_val;
+
+	if (px4_sem_getvalue(&_process_lock, &sem_val) == 0 && sem_val <= 0) {
+		px4_sem_post(&_process_lock);
+	}
 }
 
 void