SITL: support multiple instances of SITL running at once

Tools/autotest/sim_arduplane.sh

@@ -4,6 +4,9 @@ killall -q JSBSim
 killall -q ArduPlane.elf
 pkill -f runsim.py
 
+# check the instance number to allow for multiple copies of the sim running at once
+INSTANCE=0
+
 set -e
 set -x
 
@@ -14,7 +17,7 @@ make clean sitl
 tfile=$(mktemp)
 echo r > $tfile
 #gnome-terminal -e "gdb -x $tfile --args /tmp/ArduPlane.build/ArduPlane.elf"
-gnome-terminal -e /tmp/ArduPlane.build/ArduPlane.elf
+gnome-terminal -e "/tmp/ArduPlane.build/ArduPlane.elf -I$INSTANCE"
 #gnome-terminal -e "valgrind -q /tmp/ArduPlane.build/ArduPlane.elf"
 sleep 2
 rm -f $tfile

libraries/AP_HAL_AVR_SITL/SITL_State.cpp

@@ -27,6 +27,9 @@ using namespace AVR_SITL;
 
 enum SITL_State::vehicle_type SITL_State::_vehicle;
 uint16_t SITL_State::_framerate;
+uint16_t SITL_State::_base_port = 5760;
+uint16_t SITL_State::_rcout_port = 5502;
+uint16_t SITL_State::_simin_port = 5501;
 struct sockaddr_in SITL_State::_rcout_addr;
 pid_t SITL_State::_parent_pid;
 uint32_t SITL_State::_update_count;
@@ -58,6 +61,7 @@ void SITL_State::_usage(void)
 	fprintf(stdout, "\t-r RATE     set SITL framerate\n");
 	fprintf(stdout, "\t-H HEIGHT   initial barometric height\n");
 	fprintf(stdout, "\t-C          use console instead of TCP ports\n");
+	fprintf(stdout, "\t-I          set instance of SITL (adds 10*instance to all port numbers)\n");
 }
 
 void SITL_State::_parse_command_line(int argc, char * const argv[])
@@ -69,7 +73,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
     setvbuf(stdout, (char *)0, _IONBF, 0);
     setvbuf(stderr, (char *)0, _IONBF, 0);
 
-	while ((opt = getopt(argc, argv, "swhr:H:C")) != -1) {
+	while ((opt = getopt(argc, argv, "swhr:H:CI:")) != -1) {
 		switch (opt) {
 		case 'w':
 			AP_Param::erase_all();
@@ -84,6 +88,13 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
 		case 'C':
 			AVR_SITL::SITLUARTDriver::_console = true;
 			break;
+		case 'I': {
+            uint8_t instance = atoi(optarg);
+            _base_port  += instance * 10;
+            _rcout_port += instance * 10;
+            _simin_port += instance * 10;
+        }
+			break;
 		default:
 			_usage();
 			exit(1);

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -39,6 +39,7 @@ public:
     static uint16_t pwm_input[8];
     static bool pwm_valid;
     static void loop_hook(void);
+    uint16_t base_port(void) const { return _base_port; }
 
     // simulated airspeed
     static uint16_t airspeed_pin_value;
@@ -100,6 +101,7 @@ private:
     // internal state
     static enum vehicle_type _vehicle;
     static uint16_t _framerate;
+    static uint16_t _base_port;
     float _initial_height;
     static struct sockaddr_in _rcout_addr;
     static pid_t _parent_pid;
@@ -113,8 +115,8 @@ private:
 
     static int _sitl_fd;
     static SITL *_sitl;
-    static const uint16_t _rcout_port = 5502;
-    static const uint16_t _simin_port = 5501;
+    static uint16_t _rcout_port;
+    static uint16_t _simin_port;
 };
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL

libraries/AP_HAL_AVR_SITL/UARTDriver.cpp

@@ -31,8 +31,6 @@
 
 using namespace AVR_SITL;
 
-#define LISTEN_BASE_PORT 5760
-
 // On OSX, MSG_NOSIGNAL doesn't exist. The equivalent is to set SO_NOSIGPIPE
 // in setsockopt for the socket. However, if we just skip that, and don't use
 // MSG_NOSIGNAL, everything seems to work fine and SIGPIPE doesn't seem to be
@@ -236,7 +234,7 @@ void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection)
 #ifdef HAVE_SOCK_SIN_LEN
             sockaddr.sin_len = sizeof(sockaddr);
 #endif
-            sockaddr.sin_port = htons(LISTEN_BASE_PORT + _portNumber);
+            sockaddr.sin_port = htons(_sitlState->base_port() + _portNumber);
             sockaddr.sin_family = AF_INET;
 
             _listen_fd = socket(AF_INET, SOCK_STREAM, 0);
@@ -266,7 +264,8 @@ void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection)
                 exit(1);
             }
 
-            fprintf(stderr, "Serial port %u on TCP port %u\n", _portNumber, LISTEN_BASE_PORT + _portNumber);
+            fprintf(stderr, "Serial port %u on TCP port %u\n", _portNumber, 
+                    _sitlState->base_port() + _portNumber);
             fflush(stdout);
         }