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/include/px4_platform_common/px4_work_queue/WorkQueue.hpp

@@ -100,6 +100,10 @@ private:
 	BlockingList<WorkItem *>	_work_items;
 	px4::atomic_bool		_should_exit{false};
 
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	int _lockstep_component {-1};
+#endif // ENABLE_LOCKSTEP_SCHEDULER
+
 };
 
 } // namespace px4

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();
 	}
 

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -75,7 +75,7 @@ class AttitudeEstimatorQ : public ModuleBase<AttitudeEstimatorQ>, public ModuleP
 public:
 
 	AttitudeEstimatorQ();
-	~AttitudeEstimatorQ() override;
+	~AttitudeEstimatorQ() override = default;
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -117,8 +117,6 @@ private:
 
 	uORB::Publication<vehicle_attitude_s>	_att_pub{ORB_ID(vehicle_attitude)};
 
-	int		_lockstep_component{-1};
-
 	float		_mag_decl{0.0f};
 	float		_bias_max{0.0f};
 
@@ -177,13 +175,6 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
 	_gyro_bias.zero();
 
 	update_parameters(true);
-
-	_lockstep_component = px4_lockstep_register_component();
-}
-
-AttitudeEstimatorQ::~AttitudeEstimatorQ()
-{
-	px4_lockstep_unregister_component(_lockstep_component);
 }
 
 bool
@@ -366,8 +357,6 @@ AttitudeEstimatorQ::Run()
 			_att_pub.publish(att);
 
 		}
-
-		px4_lockstep_progress(_lockstep_component);
 	}
 }
 

src/modules/ekf2/EKF2.cpp

@@ -194,10 +194,6 @@ EKF2::EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool
 
 EKF2::~EKF2()
 {
-	if (!_multi_mode) {
-		px4_lockstep_unregister_component(_lockstep_component);
-	}
-
 	perf_free(_ecl_ekf_update_perf);
 	perf_free(_ecl_ekf_update_full_perf);
 }
@@ -468,14 +464,6 @@ void EKF2::Run()
 
 		// publish ekf2_timestamps
 		_ekf2_timestamps_pub.publish(ekf2_timestamps);
-
-		if (!_multi_mode) {
-			if (_lockstep_component == -1) {
-				_lockstep_component = px4_lockstep_register_component();
-			}
-
-			px4_lockstep_progress(_lockstep_component);
-		}
 	}
 }
 

src/modules/ekf2/EKF2.hpp

@@ -224,7 +224,6 @@ private:
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
 
 	bool _callback_registered{false};
-	int _lockstep_component{-1};
 
 	bool _distance_sensor_selected{false}; // because we can have several distance sensor instances with different orientations
 	bool _armed{false};

src/modules/ekf2/EKF2Selector.cpp

@@ -49,8 +49,6 @@ EKF2Selector::EKF2Selector() :
 EKF2Selector::~EKF2Selector()
 {
 	Stop();
-
-	px4_lockstep_unregister_component(_lockstep_component);
 }
 
 bool EKF2Selector::Start()
@@ -326,11 +324,6 @@ void EKF2Selector::PublishVehicleAttitude(bool reset)
 
 		attitude.timestamp = hrt_absolute_time();
 		_vehicle_attitude_pub.publish(attitude);
-
-		// register lockstep component on first attitude publish
-		if (_lockstep_component == -1) {
-			_lockstep_component = px4_lockstep_register_component();
-		}
 	}
 }
 
@@ -618,8 +611,6 @@ void EKF2Selector::Run()
 			}
 		}
 	}
-
-	px4_lockstep_progress(_lockstep_component);
 }
 
 void EKF2Selector::PrintStatus()

src/modules/ekf2/EKF2Selector.hpp

@@ -173,8 +173,6 @@ private:
 	uint8_t _lat_lon_reset_counter{0};
 	uint8_t _alt_reset_counter{0};
 
-	int _lockstep_component{-1};
-
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)};
 

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -111,11 +111,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_ref_lon(0.0),
 	_ref_alt(0.0)
 {
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-	_lockstep_component = px4_lockstep_register_component();
-#else
 	_sensors_sub.set_interval_ms(10); // main prediction loop, 100 hz (lockstep requires to run at full rate)
-#endif
 
 	// assign distance subs to array
 	_dist_subs[0] = &_sub_dist0;
@@ -145,11 +141,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 		 (_param_lpe_fusion.get() & FUSE_BARO) != 0);
 }
 
-BlockLocalPositionEstimator::~BlockLocalPositionEstimator()
-{
-	px4_lockstep_unregister_component(_lockstep_component);
-}
-
 bool
 BlockLocalPositionEstimator::init()
 {
@@ -161,7 +152,6 @@ BlockLocalPositionEstimator::init()
 	return true;
 }
 
-
 Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
 	float t,
 	const Vector<float, BlockLocalPositionEstimator::n_x> &x,
@@ -525,8 +515,6 @@ void BlockLocalPositionEstimator::Run()
 		_xDelay.update(_x);
 		_time_last_hist = _timeStamp;
 	}
-
-	px4_lockstep_progress(_lockstep_component);
 }
 
 void BlockLocalPositionEstimator::checkTimeouts()

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -112,7 +112,7 @@ class BlockLocalPositionEstimator : public ModuleBase<BlockLocalPositionEstimato
 public:
 
 	BlockLocalPositionEstimator();
-	~BlockLocalPositionEstimator() override;
+	~BlockLocalPositionEstimator() override = default;
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -338,8 +338,6 @@ private:
 	uint64_t _time_last_land;
 	uint64_t _time_last_target;
 
-	int _lockstep_component{-1};
-
 	// reference altitudes
 	float _altOrigin;
 	bool _altOriginInitialized;

src/modules/sensors/sensors.cpp

@@ -178,8 +178,6 @@ private:
 
 	VehicleIMU      *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
 
-	int _lockstep_component{-1};
-
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -282,8 +280,6 @@ Sensors::~Sensors()
 	}
 
 	perf_free(_loop_perf);
-
-	px4_lockstep_unregister_component(_lockstep_component);
 }
 
 bool Sensors::init()
@@ -626,12 +622,6 @@ void Sensors::Run()
 		parameter_update_poll();
 	}
 
-	if (_lockstep_component == -1) {
-		_lockstep_component = px4_lockstep_register_component();
-	}
-
-	px4_lockstep_progress(_lockstep_component);
-
 	perf_end(_loop_perf);
 }