diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 50f8f3dada..0554d792d6 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -60,7 +60,7 @@ void SITL_State::_set_param_default(const char *parm) /* setup for SITL handling */ -void SITL_State::_sitl_setup(void) +void SITL_State::_sitl_setup(const char *home_str) { #ifndef __CYGWIN__ _parent_pid = getppid(); @@ -93,6 +93,9 @@ void SITL_State::_sitl_setup(void) if (enable_gimbal) { gimbal = new Gimbal(_sitl->state); } + if (enable_ADSB) { + adsb = new ADSB(_sitl->state, home_str); + } } if (_synthetic_clock_mode) { @@ -252,6 +255,9 @@ void SITL_State::_fdm_input_local(void) if (gimbal != NULL) { gimbal->update(); } + if (adsb != NULL) { + adsb->update(); + } // update simulation time hal.scheduler->stop_clock(_sitl->state.timestamp_us); diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 7dca56615d..4a74af8206 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -23,6 +23,7 @@ #include #include #include +#include class HAL_SITL; @@ -71,7 +72,7 @@ private: void _parse_command_line(int argc, char * const argv[]); void _set_param_default(const char *parm); void _usage(void); - void _sitl_setup(void); + void _sitl_setup(const char *home_str); void _setup_fdm(void); void _setup_timer(void); void _setup_adc(void); @@ -200,6 +201,10 @@ private: bool enable_gimbal; SITL::Gimbal *gimbal; + // simulated gimbal + bool enable_ADSB; + SITL::ADSB *adsb; + // TCP address to connect uartC to const char *_client_address; }; diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index e85e76752f..30c4cb962f 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -46,6 +46,7 @@ void SITL_State::_usage(void) "\t--instance N set instance of SITL (adds 10*instance to all port numbers)\n" "\t--speedup SPEEDUP set simulation speedup\n" "\t--gimbal enable simulated MAVLink gimbal\n" + "\t--adsb enable simulated ADSB peripheral\n" "\t--autotest-dir DIR set directory for additional files\n" "\t--uartA device set device string for UARTA\n" "\t--uartB device set device string for UARTB\n" @@ -113,7 +114,8 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) CMDLINE_UARTB, CMDLINE_UARTC, CMDLINE_UARTD, - CMDLINE_UARTE + CMDLINE_UARTE, + CMDLINE_ADSB, }; const struct GetOptLong::option options[] = { @@ -134,6 +136,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) {"uartE", true, 0, CMDLINE_UARTE}, {"client", true, 0, CMDLINE_CLIENT}, {"gimbal", false, 0, CMDLINE_GIMBAL}, + {"adsb", false, 0, CMDLINE_ADSB}, {"autotest-dir", true, 0, CMDLINE_AUTOTESTDIR}, {0, false, 0, 0} }; @@ -184,6 +187,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) case CMDLINE_GIMBAL: enable_gimbal = true; break; + case CMDLINE_ADSB: + enable_ADSB = true; + break; case CMDLINE_AUTOTESTDIR: autotest_dir = strdup(gopt.optarg); break; @@ -240,7 +246,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) } } - _sitl_setup(); + _sitl_setup(home_str); } #endif