From 581b92dd3f661d06467045296385adf719a79392 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 3 Mar 2020 09:00:32 +0100
Subject: [PATCH] attitude_estimator_q: enable lockstep without LPE

When lockstep is enabled and LPE is not running, the
attitude_estimator_q needs to publish ekf2_timestamps in order to signal
to the simulator module that it has fully run.

This essentially fixes flying in lockstep when SYS_MC_EST_GROUP is 3.
---
 .../attitude_estimator_q_main.cpp             | 26 +++++++++++++++++++
 1 file changed, 26 insertions(+)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 47db41d7f6..4af7b511f0 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -54,6 +54,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -115,6 +116,10 @@ private:
 
 	uORB::Publication<vehicle_attitude_s>	_att_pub{ORB_ID(vehicle_attitude)};
 
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	uORB::Publication<ekf2_timestamps_s>	_ekf2_timestamps_pub {ORB_ID(ekf2_timestamps)};
+#endif
+
 	float		_mag_decl{0.0f};
 	float		_bias_max{0.0f};
 
@@ -151,6 +156,9 @@ private:
 		(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp,
 		(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas,
 		(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+		, (ParamInt<px4::params::SYS_MC_EST_GROUP>) _param_est_group
+#endif
 	)
 };
 
@@ -345,6 +353,24 @@ AttitudeEstimatorQ::Run()
 
 			/* the instance count is not used here */
 			_att_pub.publish(att);
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+
+			if (_param_est_group.get() == 3) {
+				// In this case the estimator_q is running without LPE and needs
+				// to publish ekf2_timestamps because SITL lockstep requires it.
+				ekf2_timestamps_s ekf2_timestamps;
+				ekf2_timestamps.timestamp = now;
+				ekf2_timestamps.airspeed_timestamp_rel = 0;
+				ekf2_timestamps.distance_sensor_timestamp_rel = 0;
+				ekf2_timestamps.optical_flow_timestamp_rel = 0;
+				ekf2_timestamps.vehicle_air_data_timestamp_rel = 0;
+				ekf2_timestamps.vehicle_magnetometer_timestamp_rel = 0;
+				ekf2_timestamps.visual_odometry_timestamp_rel = 0;
+				_ekf2_timestamps_pub.publish(ekf2_timestamps);
+			}
+
+#endif
 		}
 	}
 }