From 22e3ce7d7a7be71a5785236b5aa13f9394312e67 Mon Sep 17 00:00:00 2001
From: korotkoves <korotkov.es@mipt.ru>
Date: Mon, 4 Apr 2016 23:06:09 +0300
Subject: [PATCH] simulator udp port from command line

---
 src/modules/simulator/simulator.cpp         | 13 +++++++++----
 src/modules/simulator/simulator.h           |  2 +-
 src/modules/simulator/simulator_mavlink.cpp | 10 ++++++----
 3 files changed, 16 insertions(+), 9 deletions(-)

diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp
index 9eaa725cdd..a8499e6a2b 100644
--- a/src/modules/simulator/simulator.cpp
+++ b/src/modules/simulator/simulator.cpp
@@ -44,6 +44,7 @@
 #include <systemlib/err.h>
 #include <errno.h>
 #include <stdio.h>
+#include <stdlib.h>
 #include <string.h>
 #include <sys/types.h>
 #include <drivers/drv_led.h>
@@ -124,21 +125,25 @@ void Simulator::write_airspeed_data(void *buf)
 int Simulator::start(int argc, char *argv[])
 {
 	int ret = 0;
+	int udp_port = 0;
 	_instance = new Simulator();
 
 	if (_instance) {
 		drv_led_start();
 
+		if (argc == 5 && strcmp(argv[3], "-u") == 0)
+			udp_port = atoi(argv[4]);
+
 		if (argv[2][1] == 's') {
 			_instance->initializeSensorData();
 #ifndef __PX4_QURT
 			// Update sensor data
-			_instance->pollForMAVLinkMessages(false);
+			_instance->pollForMAVLinkMessages(false,udp_port);
 #endif
 
 		} else if (argv[2][1] == 'p') {
 			// Update sensor data
-			_instance->pollForMAVLinkMessages(true);
+			_instance->pollForMAVLinkMessages(true,udp_port);
 
 		} else {
 			_instance->initializeSensorData();
@@ -155,7 +160,7 @@ int Simulator::start(int argc, char *argv[])
 
 static void usage()
 {
-	PX4_WARN("Usage: simulator {start -[spt] |stop}");
+	PX4_WARN("Usage: simulator {start -[spt] [-u udp_port] |stop}");
 	PX4_WARN("Simulate raw sensors:     simulator start -s");
 	PX4_WARN("Publish sensors combined: simulator start -p");
 	PX4_WARN("Dummy unit test data:     simulator start -t");
@@ -171,7 +176,7 @@ extern "C" {
 	{
 		int ret = 0;
 
-		if (argc == 3 && strcmp(argv[1], "start") == 0) {
+		if (argc > 2 && strcmp(argv[1], "start") == 0) {
 			if (strcmp(argv[2], "-s") == 0 ||
 			    strcmp(argv[2], "-p") == 0 ||
 			    strcmp(argv[2], "-t") == 0) {
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index a5afa72c7a..6111af7c88 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -314,7 +314,7 @@ private:
 	void poll_topics();
 	void handle_message(mavlink_message_t *msg, bool publish);
 	void send_controls();
-	void pollForMAVLinkMessages(bool publish);
+	void pollForMAVLinkMessages(bool publish, int udp_port);
 
 	void pack_actuator_message(mavlink_hil_controls_t &actuator_msg);
 	void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 9be4da1329..f90ec7025c 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -462,7 +462,7 @@ void Simulator::initializeSensorData()
 	write_airspeed_data(&airspeed);
 }
 
-void Simulator::pollForMAVLinkMessages(bool publish)
+void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 {
 	// set the threads name
 #ifdef __PX4_DARWIN
@@ -473,13 +473,15 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 
 	// udp socket data
 	struct sockaddr_in _myaddr;
-	const int _port = UDP_PORT;
+
+	if (udp_port < 1)
+		udp_port = UDP_PORT;
 
 	// try to setup udp socket for communcation with simulator
 	memset((char *)&_myaddr, 0, sizeof(_myaddr));
 	_myaddr.sin_family = AF_INET;
 	_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
-	_myaddr.sin_port = htons(_port);
+	_myaddr.sin_port = htons(udp_port);
 
 	if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
 		PX4_WARN("create socket failed\n");
@@ -532,7 +534,7 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 	// wait for first data from simulator and respond with first controls
 	// this is important for the UDP communication to work
 	int pret = -1;
-	PX4_INFO("Waiting for initial data on UDP. Please start the flight simulator to proceed..");
+	PX4_INFO("Waiting for initial data on UDP port %i. Please start the flight simulator to proceed..",udp_port);
 
 	uint64_t pstart_time = 0;