px4_work_queue: directly support SITL lockstep

- the purpose is to ensure that every WorkItem (and WorkItems scheduled
by WorkItems) is allowed to run to completion every step
 - per workqueue register a lockstep component whenever a work item is
added (if not already registered)
 - once the work queue is empty unregister component

platforms/common/px4_work_queue/WorkQueue.cpp

@@ -106,6 +106,15 @@ void WorkQueue::Detach(WorkItem *item)
 void WorkQueue::Add(WorkItem *item)
 {
 	work_lock();
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+
+	if (_lockstep_component == -1) {
+		_lockstep_component = px4_lockstep_register_component();
+	}
+
+#endif // ENABLE_LOCKSTEP_SCHEDULER
+
 	_q.push(item);
 	work_unlock();
 
@@ -158,6 +167,15 @@ void WorkQueue::Run()
 			work_lock(); // re-lock
 		}
 
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+
+		if (_q.empty()) {
+			px4_lockstep_unregister_component(_lockstep_component);
+			_lockstep_component = -1;
+		}
+
+#endif // ENABLE_LOCKSTEP_SCHEDULER
+
 		work_unlock();
 	}