simulator: fix TCP on Cygwin-Windows

It turns out that `sendto` does not work for TCP on Cygwin-Windows,
instead we need to use `send`. This required some refactoring since we
need to have the internet protocol and the port stored as a member
variable.

This should fix that lockstep SITL simulation was not working on
Windows.

src/modules/simulator/simulator.cpp

@@ -149,29 +149,26 @@ int Simulator::start(int argc, char *argv[])
 	if (_instance) {
 		drv_led_start();
 
-		InternetProtocol ip = InternetProtocol::UDP;
-		unsigned port = 14560;
-
 		if (argc == 5 && strcmp(argv[3], "-u") == 0) {
-			ip = InternetProtocol::UDP;
-			port = atoi(argv[4]);
+			_instance->set_ip(InternetProtocol::UDP);
+			_instance->set_port(atoi(argv[4]));
 		}
 
 		if (argc == 5 && strcmp(argv[3], "-c") == 0) {
-			ip = InternetProtocol::TCP;
-			port = atoi(argv[4]);
+			_instance->set_ip(InternetProtocol::TCP);
+			_instance->set_port(atoi(argv[4]));
 		}
 
 		if (argv[2][1] == 's') {
 			_instance->initializeSensorData();
 #ifndef __PX4_QURT
 			// Update sensor data
-			_instance->pollForMAVLinkMessages(false, ip, port);
+			_instance->pollForMAVLinkMessages(false);
 #endif
 
 		} else if (argv[2][1] == 'p') {
 			// Update sensor data
-			_instance->pollForMAVLinkMessages(true, ip, port);
+			_instance->pollForMAVLinkMessages(true);
 
 		} else {
 			_instance->initializeSensorData();
@@ -186,6 +183,16 @@ int Simulator::start(int argc, char *argv[])
 	}
 }
 
+void Simulator::set_ip(InternetProtocol ip)
+{
+	_ip = ip;
+}
+
+void Simulator::set_port(unsigned port)
+{
+	_port = port;
+}
+
 static void usage()
 {
 	PX4_WARN("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop}");

src/modules/simulator/simulator.h

@@ -212,6 +212,11 @@ public:
 		sample(float a, float b, float c) : x(a), y(b), z(c) {}
 	};
 
+	enum class InternetProtocol {
+		TCP,
+		UDP
+	};
+
 	static int start(int argc, char *argv[]);
 
 	bool getRawAccelReport(uint8_t *buf, int len);
@@ -230,6 +235,9 @@ public:
 
 	bool isInitialized() { return _initialized; }
 
+	void set_ip(InternetProtocol ip);
+	void set_port(unsigned port);
+
 private:
 	Simulator() : ModuleParams(nullptr),
 		_accel(1),
@@ -336,6 +344,9 @@ private:
 
 	int				_param_sub;
 
+	unsigned _port = 14560;
+	InternetProtocol _ip = InternetProtocol::UDP;
+
 	bool _initialized;
 	double _realtime_factor;		///< How fast the simulation runs in comparison to real system time
 	hrt_abstime _last_sim_timestamp;
@@ -385,17 +396,12 @@ private:
 
 	)
 
-	enum class InternetProtocol {
-		TCP,
-		UDP
-	};
-
 	void poll_topics();
 	void handle_message(mavlink_message_t *msg, bool publish);
 	void send_controls();
 	void send_heartbeat();
 	void request_hil_state_quaternion();
-	void pollForMAVLinkMessages(bool publish, InternetProtocol ip, int port);
+	void pollForMAVLinkMessages(bool publish);
 
 	void pack_actuator_message(mavlink_hil_actuator_controls_t &actuator_msg, unsigned index);
 	void send_mavlink_message(const mavlink_message_t &aMsg);

src/modules/simulator/simulator_mavlink.cpp

@@ -511,7 +511,14 @@ void Simulator::send_mavlink_message(const mavlink_message_t &aMsg)
 
 	bufLen = mavlink_msg_to_send_buffer(buf, &aMsg);
 
-	ssize_t len = sendto(_fd, buf, bufLen, 0, (struct sockaddr *)&_srcaddr, sizeof(_srcaddr));
+	ssize_t len;
+
+	if (_ip == InternetProtocol::UDP) {
+		len = ::sendto(_fd, buf, bufLen, 0, (struct sockaddr *)&_srcaddr, sizeof(_srcaddr));
+
+	} else {
+		len = ::send(_fd, buf, bufLen, 0);
+	}
 
 	if (len <= 0) {
 		PX4_WARN("Failed sending mavlink message: %s", strerror(errno));
@@ -643,7 +650,7 @@ void Simulator::initializeSensorData()
 	write_airspeed_data(&airspeed);
 }
 
-void Simulator::pollForMAVLinkMessages(bool publish, InternetProtocol ip, int port)
+void Simulator::pollForMAVLinkMessages(bool publish)
 {
 #ifdef __PX4_DARWIN
 	pthread_setname_np("sim_rcv");
@@ -654,9 +661,9 @@ void Simulator::pollForMAVLinkMessages(bool publish, InternetProtocol ip, int po
 	struct sockaddr_in _myaddr {};
 	_myaddr.sin_family = AF_INET;
 	_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
-	_myaddr.sin_port = htons(port);
+	_myaddr.sin_port = htons(_port);
 
-	if (ip == InternetProtocol::UDP) {
+	if (_ip == InternetProtocol::UDP) {
 
 		if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
 			PX4_ERR("Creating UDP socket failed: %s", strerror(errno));
@@ -664,11 +671,11 @@ void Simulator::pollForMAVLinkMessages(bool publish, InternetProtocol ip, int po
 		}
 
 		if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
-			PX4_ERR("bind for UDP port %i failed (%i)", port, errno);
+			PX4_ERR("bind for UDP port %i failed (%i)", _port, errno);
 			return;
 		}
 
-		PX4_INFO("Waiting for simulator to connect on UDP port %u", port);
+		PX4_INFO("Waiting for simulator to connect on UDP port %u", _port);
 
 		while (true) {
 			// Once we receive something, we're most probably good and can carry on.
@@ -683,11 +690,11 @@ void Simulator::pollForMAVLinkMessages(bool publish, InternetProtocol ip, int po
 			}
 		}
 
-		PX4_INFO("Simulator connected on UDP port %u.", port);
+		PX4_INFO("Simulator connected on UDP port %u.", _port);
 
 	} else {
 
-		PX4_INFO("Waiting for simulator to connect on TCP port %u", port);
+		PX4_INFO("Waiting for simulator to connect on TCP port %u", _port);
 
 		while (true) {
 			if ((_fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
@@ -714,7 +721,7 @@ void Simulator::pollForMAVLinkMessages(bool publish, InternetProtocol ip, int po
 			}
 		}
 
-		PX4_INFO("Simulator connected on TCP port %u.", port);
+		PX4_INFO("Simulator connected on TCP port %u.", _port);
 
 	}