sitl: use lockstep components API

- avoids the need for ekf2_timestamp publications by q and lpe
- adds logger to the lockstep cycle and makes it poll on ekf2_timestamps
  or vehicle_attitude. This avoids dropped samples (required for replay).

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -112,11 +112,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_ref_alt(0.0)
 {
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
-	// For lockstep 250 Hz are needed because ekf2_timestamps need to be
-	// published at 250 Hz.
-	_sensors_sub.set_interval_ms(4);
+	_lockstep_component = px4_lockstep_register_component();
 #else
-	_sensors_sub.set_interval_ms(10); // main prediction loop, 100 hz
+	_sensors_sub.set_interval_ms(10); // main prediction loop, 100 hz (lockstep requires to run at full rate)
 #endif
 
 	// assign distance subs to array
@@ -147,6 +145,11 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 		 (_param_lpe_fusion.get() & FUSE_BARO) != 0);
 }
 
+BlockLocalPositionEstimator::~BlockLocalPositionEstimator()
+{
+	px4_lockstep_unregister_component(_lockstep_component);
+}
+
 bool
 BlockLocalPositionEstimator::init()
 {
@@ -504,7 +507,6 @@ void BlockLocalPositionEstimator::Run()
 
 		if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) {
 			publishGlobalPos();
-			publishEk2fTimestamps();
 		}
 	}
 
@@ -519,6 +521,8 @@ void BlockLocalPositionEstimator::Run()
 		_xDelay.update(_x);
 		_time_last_hist = _timeStamp;
 	}
+
+	px4_lockstep_progress(_lockstep_component);
 }
 
 void BlockLocalPositionEstimator::checkTimeouts()
@@ -755,22 +759,6 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
 	_pub_est_status.update();
 }
 
-void BlockLocalPositionEstimator::publishEk2fTimestamps()
-{
-	_pub_ekf2_timestamps.get().timestamp = _timeStamp;
-
-	// We only really publish this as a dummy because lockstep simulation
-	// requires it to be published.
-	_pub_ekf2_timestamps.get().airspeed_timestamp_rel = 0;
-	_pub_ekf2_timestamps.get().distance_sensor_timestamp_rel = 0;
-	_pub_ekf2_timestamps.get().optical_flow_timestamp_rel = 0;
-	_pub_ekf2_timestamps.get().vehicle_air_data_timestamp_rel = 0;
-	_pub_ekf2_timestamps.get().vehicle_magnetometer_timestamp_rel = 0;
-	_pub_ekf2_timestamps.get().visual_odometry_timestamp_rel = 0;
-
-	_pub_ekf2_timestamps.update();
-}
-
 void BlockLocalPositionEstimator::publishGlobalPos()
 {
 	// publish global position

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -35,7 +35,6 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/estimator_innovations.h>
-#include <uORB/topics/ekf2_timestamps.h>
 
 using namespace matrix;
 using namespace control;
@@ -112,7 +111,7 @@ class BlockLocalPositionEstimator : public ModuleBase<BlockLocalPositionEstimato
 public:
 
 	BlockLocalPositionEstimator();
-	~BlockLocalPositionEstimator() override = default;
+	~BlockLocalPositionEstimator() override;
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -287,7 +286,6 @@ private:
 	uORB::PublicationData<vehicle_global_position_s> _pub_gpos{ORB_ID(vehicle_global_position)};
 	uORB::PublicationData<vehicle_odometry_s> _pub_odom{ORB_ID(vehicle_odometry)};
 	uORB::PublicationData<estimator_status_s> _pub_est_status{ORB_ID(estimator_status)};
-	uORB::PublicationData<ekf2_timestamps_s> _pub_ekf2_timestamps{ORB_ID(ekf2_timestamps)};
 	uORB::PublicationData<estimator_innovations_s> _pub_innov{ORB_ID(estimator_innovations)};
 	uORB::PublicationData<estimator_innovations_s> _pub_innov_var{ORB_ID(estimator_innovation_variances)};
 
@@ -338,6 +336,8 @@ private:
 	uint64_t _time_last_land;
 	uint64_t _time_last_target;
 
+	int _lockstep_component{-1};
+
 	// reference altitudes
 	float _altOrigin;
 	bool _altOriginInitialized;