diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 045d751f0a..ef4e7778f9 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -130,6 +130,8 @@ void Simulator::send_controls()
 	mavlink_hil_controls_t msg;
 	pack_actuator_message(msg);
 	send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
+	mavlink_heartbeat_t hb = {};
+	send_mavlink_message(MAVLINK_MSG_ID_HEARTBEAT, &hb, 200);
 }
 
 static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels)
@@ -526,11 +528,6 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 	// setup serial connection to autopilot (used to get manual controls)
 	int serial_fd = openUart(PIXHAWK_DEVICE, 115200);
 
-	if (serial_fd < 0) {
-		PX4_INFO("Not using %s for radio control input. Assuming joystick input via MAVLink.", PIXHAWK_DEVICE);
-
-	}
-
 	char serial_buf[1024];
 
 	struct pollfd fds[2];
@@ -544,6 +541,8 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 		fds[1].fd = serial_fd;
 		fds[1].events = POLLIN;
 		fd_count++;
+	} else {
+		PX4_INFO("Not using %s for radio control input. Assuming joystick input via MAVLink.", PIXHAWK_DEVICE);
 	}
 
 	int len = 0;
@@ -553,20 +552,48 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 	int pret = -1;
 	PX4_INFO("Waiting for initial data on UDP. Please start the flight simulator to proceed..");
 
-	while (pret <= 0) {
+	unsigned long long pstart_time = 0;
+
+	bool no_sim_data = true;
+
+	while (!px4_exit_requested() && no_sim_data/*pret <= 0 || (pstart_time == 0 || hrt_system_time() - pstart_time < 100000000)*/) {
 		pret = ::poll(&fds[0], fd_count, 100);
+
+		if (fds[0].revents & POLLIN) {
+			PX4_WARN(".");
+			pstart_time = hrt_system_time();
+			len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
+			// send first controls
+			send_controls();
+
+			if (len > 0) {
+				mavlink_message_t msg;
+				mavlink_status_t udp_status = {};
+
+				for (int i = 0; i < len; i++) {
+					if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &udp_status)) {
+						// have a message, handle it
+						handle_message(&msg, publish);
+						warnx("Got: %u", msg.msgid);
+
+						if (msg.msgid != 0) {
+							PX4_INFO("Got initial simuation data, running sim..");
+							no_sim_data = false;
+						}
+					}
+				}
+			}
+		}
+	}
+
+	if (px4_exit_requested()) {
+		return;
 	}
 
 	_initialized = true;
 	// reset system time
 	(void)hrt_reset();
 
-	if (fds[0].revents & POLLIN) {
-		len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
-		PX4_INFO("Sending initial controls message to simulator");
-		send_controls();
-	}
-
 	// subscribe to topics
 	_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -625,6 +652,7 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 					if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &udp_status)) {
 						// have a message, handle it
 						handle_message(&msg, publish);
+						warnx("Got: %u", msg.msgid);
 					}
 				}
 			}
diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index 72634c356c..903aa42901 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -165,6 +165,10 @@ static void restore_term(void)
 	tcsetattr(0, TCSANOW, &orig_term);
 }
 
+bool px4_exit_requested(void) {
+	return _ExitFlag;
+}
+
 int main(int argc, char **argv)
 {
 	bool daemon_mode = false;
@@ -237,6 +241,10 @@ int main(int argc, char **argv)
 
 		if (infile.is_open()) {
 			for (string line; getline(infile, line, '\n');) {
+
+				if (px4_exit_requested()) {
+					break;
+				}
 				// TODO: this should be true but for that we have to check all startup files
 				process_line(line, false);
 			}
diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c
index 7af4431b9c..d40b540b2c 100644
--- a/src/platforms/posix/px4_layer/drv_hrt.c
+++ b/src/platforms/posix/px4_layer/drv_hrt.c
@@ -130,6 +130,16 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
 
 #endif
 
+/*
+ * Get system time in us
+ */
+unsigned long long hrt_system_time(void)
+{
+	struct timespec ts;
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
+	return ts_to_abstime(&ts) - px4_timestart;
+}
+
 /*
  * Get absolute time.
  */