diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp
index d655fe5048..fc910959da 100644
--- a/src/modules/simulator/simulator.cpp
+++ b/src/modules/simulator/simulator.cpp
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -42,14 +42,7 @@
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/tasks.h>
 #include <px4_platform_common/time.h>
-#include <pthread.h>
-#include <poll.h>
 #include <systemlib/err.h>
-#include <errno.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/types.h>
 #include <drivers/drv_board_led.h>
 
 #include "simulator.h"
@@ -106,10 +99,8 @@ int Simulator::start(int argc, char *argv[])
 		}
 
 #ifndef __PX4_QURT
-		// Update sensor data
-		_instance->poll_for_MAVLink_messages();
+		_instance->run();
 #endif
-
 		return 0;
 
 	} else {
@@ -130,58 +121,61 @@ void Simulator::set_port(unsigned port)
 
 static void usage()
 {
-	PX4_WARN("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop}");
-	PX4_WARN("Start simulator:     simulator start");
-	PX4_WARN("Connect using UDP: simulator start -u udp_port");
-	PX4_WARN("Connect using TCP: simulator start -c tcp_port");
+	PX4_INFO("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop}");
+	PX4_INFO("Start simulator:     simulator start");
+	PX4_INFO("Connect using UDP: simulator start -u udp_port");
+	PX4_INFO("Connect using TCP: simulator start -c tcp_port");
 }
 
 __BEGIN_DECLS
 extern int simulator_main(int argc, char *argv[]);
 __END_DECLS
 
-extern "C" {
 
-	int simulator_main(int argc, char *argv[])
-	{
-		if (argc > 1 && strcmp(argv[1], "start") == 0) {
+int simulator_main(int argc, char *argv[])
+{
+	if (argc > 1 && strcmp(argv[1], "start") == 0) {
 
-			if (g_sim_task >= 0) {
-				PX4_WARN("Simulator already started");
-				return 0;
-			}
-
-			g_sim_task = px4_task_spawn_cmd("simulator",
-							SCHED_DEFAULT,
-							SCHED_PRIORITY_DEFAULT,
-							1500,
-							Simulator::start,
-							argv);
-
-			while (true) {
-				if (Simulator::getInstance()) {
-					break;
-
-				} else {
-					system_usleep(100);
-				}
-			}
-
-		} else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
-			if (g_sim_task < 0) {
-				PX4_WARN("Simulator not running");
-
-			} else {
-				px4_task_delete(g_sim_task);
-				g_sim_task = -1;
-			}
-
-		} else {
-			usage();
-			return 1;
+		if (g_sim_task >= 0) {
+			PX4_WARN("Simulator already started");
+			return 0;
 		}
 
-		return 0;
+		g_sim_task = px4_task_spawn_cmd("simulator",
+						SCHED_DEFAULT,
+						SCHED_PRIORITY_DEFAULT,
+						1500,
+						Simulator::start,
+						argv);
+
+#if !defined(__PX4_QURT) && defined(ENABLE_LOCKSTEP_SCHEDULER)
+
+		// We want to prevent the rest of the startup script from running until time
+		// is initialized by the HIL_SENSOR messages from the simulator.
+		while (true) {
+			if (Simulator::getInstance() && Simulator::getInstance()->has_initialized()) {
+				break;
+			}
+
+			system_usleep(100);
+		}
+
+#endif
+
+	} else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
+		if (g_sim_task < 0) {
+			PX4_WARN("Simulator not running");
+			return 1;
+
+		} else {
+			px4_task_delete(g_sim_task);
+			g_sim_task = -1;
+		}
+
+	} else {
+		usage();
+		return 1;
 	}
 
+	return 0;
 }
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index 2f73ecb92e..b072ff5072 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -51,6 +51,7 @@
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 #include <lib/ecl/geo/geo.h>
 #include <perf/perf_counter.h>
+#include <px4_platform_common/atomic.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/posix.h>
 #include <uORB/Publication.hpp>
@@ -173,6 +174,7 @@ public:
 
 	void set_ip(InternetProtocol ip);
 	void set_port(unsigned port);
+	bool has_initialized() {return _has_initialized.load(); }
 
 private:
 	Simulator() :
@@ -261,8 +263,7 @@ private:
 
 #ifndef __PX4_QURT
 
-	mavlink_hil_actuator_controls_t actuator_controls_from_outputs(const actuator_outputs_s &actuators);
-
+	void run();
 	void handle_message(const mavlink_message_t *msg);
 	void handle_message_distance_sensor(const mavlink_message_t *msg);
 	void handle_message_hil_gps(const mavlink_message_t *msg);
@@ -275,7 +276,6 @@ private:
 	void handle_message_vision_position_estimate(const mavlink_message_t *msg);
 
 	void parameters_update(bool force);
-
 	void poll_for_MAVLink_messages();
 	void request_hil_state_quaternion();
 	void send();
@@ -287,6 +287,8 @@ private:
 
 	static void *sending_trampoline(void *);
 
+	mavlink_hil_actuator_controls_t actuator_controls_from_outputs(const actuator_outputs_s &actuators);
+
 	// uORB publisher handlers
 	uORB::Publication<vehicle_angular_velocity_s>	_vehicle_angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
 	uORB::Publication<vehicle_attitude_s>		_attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
@@ -311,6 +313,10 @@ private:
 	// uORB data containers
 	vehicle_status_s _vehicle_status{};
 
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	px4::atomic<bool> _has_initialized {false};
+#endif
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::SIM_BAT_DRAIN>) _param_sim_bat_drain, ///< battery drain interval
 		(ParamFloat<px4::params::SIM_BAT_MIN_PCT>) _battery_min_percentage, //< minimum battery percentage
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index bce060cb9b..d8a048088a 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -2,7 +2,7 @@
  *
  *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
  *   Copyright (c) 2016 Anton Matosov. All rights reserved.
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2017-2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -374,6 +374,14 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg)
 
 		_last_battery_timestamp = now_us;
 	}
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+
+	if (!_has_initialized.load()) {
+		_has_initialized.store(true);
+	}
+
+#endif
 }
 
 void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg)
@@ -630,7 +638,7 @@ void Simulator::send_heartbeat()
 	send_mavlink_message(message);
 }
 
-void Simulator::poll_for_MAVLink_messages()
+void Simulator::run()
 {
 #ifdef __PX4_DARWIN
 	pthread_setname_np("sim_rcv");