AP_IRLock_SITL: make port variable

Tools/autotest/default_params/gazebo-iris.parm

@@ -1,5 +1,5 @@
 PLND_ENABLED    1
-PLND_TYPE       2
+PLND_TYPE       3
 POS_XY_P        2
 VEL_XY_P        0.6
 # Iris is X frame
@@ -60,3 +60,10 @@ RC8_TRIM        1500.000000
 
 MOT_THST_EXPO 0.5
 MOT_THST_HOVER  0.36
+CH8_OPT 39
+
+SIM_SONAR_SCALE 10
+RNGFND_TYPE 1
+RNGFND_SCALING 10
+RNGFND_PIN 0
+RNGFND_MAX_CM 5000

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -100,6 +100,7 @@ void SITL_State::_sitl_setup(const char *home_str)
             fg_socket.connect("127.0.0.1", _fg_view_port);
         }
 
+        _sitl->irlock_port = _irlock_port;
     }
 
     if (_synthetic_clock_mode) {

libraries/AP_HAL_SITL/SITL_State.h

@@ -162,6 +162,7 @@ private:
     uint16_t _rcout_port;
     uint16_t _rcin_port;
     uint16_t _fg_view_port;
+    uint16_t _irlock_port;
     float _current;
 
     bool _synthetic_clock_mode;

libraries/AP_HAL_SITL/SITL_cmdline.cpp

@@ -142,6 +142,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
     _gazebo_address = "127.0.0.1";
     _gazebo_port_in = 9003;
     _gazebo_port_out = 9002;
+    _irlock_port = 9005;
     _instance = 0;
 
     enum long_options {
@@ -217,6 +218,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
             _fg_view_port += _instance * 10;
             _gazebo_port_in += _instance * 10;
             _gazebo_port_out += _instance * 10;
+            _irlock_port += _instance * 10;
         }
         break;
         case 'P':

libraries/AP_IRLock/AP_IRLock_SITL.cpp

@@ -23,7 +23,7 @@
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include "AP_IRLock_SITL.h"
-
+#include <SITL/SITL.h>
 #include <fcntl.h>
 #include <unistd.h>
 
@@ -38,10 +38,11 @@ AP_IRLock_SITL::AP_IRLock_SITL() :
 
 void AP_IRLock_SITL::init()
 {
+    SITL::SITL *sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
     // try to bind to a specific port so that if we restart ArduPilot
     // Gazebo keeps sending us packets. Not strictly necessary but
     // useful for debugging
-    sock.bind("127.0.0.1", 9005);
+    sock.bind("127.0.0.1", sitl->irlock_port);
 
     sock.reuseaddress();
     sock.set_blocking(false);

libraries/AP_IRLock/AP_IRLock_SITL.h

@@ -7,7 +7,7 @@
 #pragma once
 
 #include <AP_HAL/utility/Socket.h>
-
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include "IRLock.h"
 
 class AP_IRLock_SITL : public IRLock
@@ -38,3 +38,4 @@ private:
     uint32_t _last_timestamp;
     SocketAPM sock;
 };
+#endif // CONFIG_HAL_BOARD

libraries/SITL/SITL.h

@@ -140,6 +140,8 @@ public:
     AP_Vector3f rngfnd_pos_offset;  // XYZ position of the range finder zero range datum relative to the body frame origin (m)
     AP_Vector3f optflow_pos_offset; // XYZ position of the optical flow sensor focal point relative to the body frame origin (m)
 
+    uint16_t irlock_port;
+
     void simstate_send(mavlink_channel_t chan);
 
     void Log_Write_SIMSTATE(DataFlash_Class *dataflash);