From b53bd2b77f00d8f30a6d45a40b5b87e67c9c1322 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 26 Feb 2020 14:20:10 +0100
Subject: [PATCH] LPE: make compatible with lockstep simulation

This fixes SITL with lockstep when using LPE and q estimator.

The required changes are:
- Publish ekf2_timestamps because simulator_mavlink expects them.
- Run LPE at 250 Hz instead of 80 Hz in order to keep everything in
  lockstep running at 250 Hz.
---
 .../BlockLocalPositionEstimator.cpp           | 23 +++++++++++++++++++
 .../BlockLocalPositionEstimator.hpp           |  3 +++
 2 files changed, 26 insertions(+)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 042bee288c..1fa9a9c7f3 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -111,7 +111,13 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_ref_lon(0.0),
 	_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);
+#else
 	_sensors_sub.set_interval_ms(10); // main prediction loop, 100 hz
+#endif
 
 	// assign distance subs to array
 	_dist_subs[0] = &_sub_dist0;
@@ -498,6 +504,7 @@ void BlockLocalPositionEstimator::Run()
 
 		if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) {
 			publishGlobalPos();
+			publishEk2fTimestamps();
 		}
 	}
 
@@ -746,6 +753,22 @@ 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
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index 28976e98b9..9418b8059d 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -35,6 +35,7 @@
 #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;
@@ -254,6 +255,7 @@ private:
 	void publishGlobalPos();
 	void publishOdom();
 	void publishEstimatorStatus();
+	void publishEk2fTimestamps();
 
 	// attributes
 	// ----------------------------
@@ -285,6 +287,7 @@ 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)};