From 309ac5380693e452702ec676a4c874fb5911f67d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 21 Jun 2016 11:00:36 +1000 Subject: [PATCH] SITL: correct adsb initialisation Parameters have not yet been initialised in _sitl_setup, so move should-run checks into update --- libraries/AP_HAL_SITL/SITL_State.cpp | 15 +++++++++++---- libraries/AP_HAL_SITL/SITL_State.h | 2 ++ 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 397857143a..64dffd1bd7 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -63,6 +63,8 @@ void SITL_State::_set_param_default(const char *parm) */ void SITL_State::_sitl_setup(const char *home_str) { + _home_str = home_str; + #ifndef __CYGWIN__ _parent_pid = getppid(); #endif @@ -97,10 +99,6 @@ void SITL_State::_sitl_setup(const char *home_str) gimbal = new SITL::Gimbal(_sitl->state); } - if (_sitl->adsb_plane_count >= 0) { - adsb = new SITL::ADSB(_sitl->state, home_str); - } - fg_socket.connect("127.0.0.1", 5503); } @@ -173,6 +171,15 @@ void SITL_State::_fdm_input_step(void) _update_barometer(_sitl->state.altitude); _update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg); _update_flow(); + + if (_sitl->adsb_plane_count >= 0 && + adsb == nullptr) { + adsb = new SITL::ADSB(_sitl->state, _home_str); + } else if (_sitl->adsb_plane_count == -1 && + adsb != nullptr) { + delete adsb; + adsb = nullptr; + } } // trigger all APM timers. diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 6d56cbd55b..32f0621307 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -222,6 +222,8 @@ private: const char *_client_address; const char *defaults_path = HAL_PARAM_DEFAULTS_PATH; + + const char *_home_str; }; #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL