From 1bb4e3c9b7cba4740eab364731a25c62b735cce1 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 17 Nov 2016 19:05:04 +0100 Subject: [PATCH] AP_IRLock_SITL: make port variable --- Tools/autotest/default_params/gazebo-iris.parm | 9 ++++++++- libraries/AP_HAL_SITL/SITL_State.cpp | 1 + libraries/AP_HAL_SITL/SITL_State.h | 1 + libraries/AP_HAL_SITL/SITL_cmdline.cpp | 2 ++ libraries/AP_IRLock/AP_IRLock_SITL.cpp | 5 +++-- libraries/AP_IRLock/AP_IRLock_SITL.h | 3 ++- libraries/SITL/SITL.h | 2 ++ 7 files changed, 19 insertions(+), 4 deletions(-) diff --git a/Tools/autotest/default_params/gazebo-iris.parm b/Tools/autotest/default_params/gazebo-iris.parm index b6bf0d60be..912983f697 100644 --- a/Tools/autotest/default_params/gazebo-iris.parm +++ b/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 diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 07fe738374..2b779549fd 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/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) { diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 219cf2abfd..dc172add03 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/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; diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index b1283e8f4c..ec9b40abe0 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/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': diff --git a/libraries/AP_IRLock/AP_IRLock_SITL.cpp b/libraries/AP_IRLock/AP_IRLock_SITL.cpp index f02d52d6ef..32d44c02df 100644 --- a/libraries/AP_IRLock/AP_IRLock_SITL.cpp +++ b/libraries/AP_IRLock/AP_IRLock_SITL.cpp @@ -23,7 +23,7 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include "AP_IRLock_SITL.h" - +#include #include #include @@ -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); diff --git a/libraries/AP_IRLock/AP_IRLock_SITL.h b/libraries/AP_IRLock/AP_IRLock_SITL.h index 2f5d5cd4bc..e43a79470a 100644 --- a/libraries/AP_IRLock/AP_IRLock_SITL.h +++ b/libraries/AP_IRLock/AP_IRLock_SITL.h @@ -7,7 +7,7 @@ #pragma once #include - +#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 diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 99ccb8b769..3b0e8b4e09 100644 --- a/libraries/SITL/SITL.h +++ b/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);