simulator: cleanup and wait for startup

Unfortunately this commit contains two things:
1. Some cleanup and renaiming.
2. An additional wait until lockstep has been initialized.
   By waiting until HIL_SENSOR messages arrive including timestamps we
   stop the startup script and prevent other modules from running until
   time is set up. This should resolve some busy waiting by various
   modules and prevent races on initialization (e.g. the landing state
   being subscribed by mavlink before being published by the land
   detector).

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;
 }

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

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");