diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 361f86d199..4ec8edb201 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -232,6 +232,14 @@ private:
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 	px4::atomic<bool> _has_initialized {false};
+
+	int _ekf2_timestamps_sub{-1};
+
+	enum class State {
+		WaitingForFirstEkf2Timestamp = 0,
+		WaitingForActuatorControls = 1,
+		WaitingForEkf2Timestamp = 2,
+	};
 #endif
 
 	DEFINE_PARAMETERS(
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 849c2a0fc2..44ad3bc64e 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -596,28 +596,70 @@ void Simulator::send()
 	fds_actuator_outputs[0].fd = _actuator_outputs_sub;
 	fds_actuator_outputs[0].events = POLLIN;
 
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	px4_pollfd_struct_t fds_ekf2_timestamps[1] = {};
+	fds_ekf2_timestamps[0].fd = _ekf2_timestamps_sub;
+	fds_ekf2_timestamps[0].events = POLLIN;
+
+	State state = State::WaitingForFirstEkf2Timestamp;
+#endif
+
 	while (true) {
 
-		// Wait for up to 100ms for data.
-		int pret = px4_poll(&fds_actuator_outputs[0], 1, 100);
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
 
-		if (pret == 0) {
-			// Timed out, try again.
-			continue;
+		if (state == State::WaitingForActuatorControls) {
+#endif
+			// Wait for up to 100ms for data.
+			int pret = px4_poll(&fds_actuator_outputs[0], 1, 100);
+
+			if (pret == 0) {
+				// Timed out, try again.
+				continue;
+			}
+
+			if (pret < 0) {
+				PX4_ERR("poll error %s", strerror(errno));
+				continue;
+			}
+
+			if (fds_actuator_outputs[0].revents & POLLIN) {
+				// Got new data to read, update all topics.
+				parameters_update(false);
+				_vehicle_status_sub.update(&_vehicle_status);
+				send_controls();
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+				state = State::WaitingForEkf2Timestamp;
+#endif
+			}
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
 		}
 
-		if (pret < 0) {
-			PX4_ERR("poll error %s", strerror(errno));
-			continue;
-		}
-
-		if (fds_actuator_outputs[0].revents & POLLIN) {
-			// Got new data to read, update all topics.
-			parameters_update(false);
-			_vehicle_status_sub.update(&_vehicle_status);
-			send_controls();
+#endif
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+
+		if (state == State::WaitingForFirstEkf2Timestamp || state == State::WaitingForEkf2Timestamp) {
+			int pret = px4_poll(&fds_ekf2_timestamps[0], 1, 100);
+
+			if (pret == 0) {
+				// Timed out, try again.
+				continue;
+			}
+
+			if (pret < 0) {
+				PX4_ERR("poll error %s", strerror(errno));
+				continue;
+			}
+
+			if (fds_ekf2_timestamps[0].revents & POLLIN) {
+				orb_copy(ORB_ID(ekf2_timestamps), _ekf2_timestamps_sub, nullptr);
+				state = State::WaitingForActuatorControls;
+			}
 		}
 
+#endif
 	}
 }
 
@@ -710,7 +752,7 @@ void Simulator::run()
 
 			} else {
 				::close(_fd);
-				system_usleep(100);
+				system_sleep(1);
 			}
 		}
 
@@ -761,6 +803,10 @@ void Simulator::run()
 	// Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS.
 	_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
 
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	_ekf2_timestamps_sub = orb_subscribe(ORB_ID(ekf2_timestamps));
+#endif
+
 	// got data from simulator, now activate the sending thread
 	pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, nullptr);
 	pthread_attr_destroy(&sender_thread_attr);
@@ -823,6 +869,9 @@ void Simulator::run()
 	}
 
 	orb_unsubscribe(_actuator_outputs_sub);
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	orb_unsubscribe(_ekf2_timestamps_sub);
+#endif
 }
 
 #ifdef ENABLE_UART_RC_INPUT