diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 051ac0147b..e7890636e5 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -194,6 +194,13 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) #define streq(a, b) (!strcmp(a, b)) int SITL_State::sim_fd(const char *name, const char *arg) { + if (streq(name, "vicon")) { + if (vicon != nullptr) { + AP_HAL::panic("Only one vicon system at a time"); + } + vicon = new SITL::Vicon(); + return vicon->fd(); + } AP_HAL::panic("unknown simulated device: %s", name); } @@ -298,6 +305,13 @@ void SITL_State::_fdm_input_local(void) if (adsb != nullptr) { adsb->update(); } + if (vicon != nullptr) { + Quaternion attitude; + sitl_model->get_attitude(attitude); + vicon->update(sitl_model->get_location(), + sitl_model->get_position(), + attitude); + } if (_sitl && _use_fg_view) { _output_to_flightgear(); diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 3f6400b194..fa21e32541 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -22,6 +22,7 @@ #include #include #include +#include #include class HAL_SITL; @@ -210,6 +211,9 @@ private: // simulated ADSb SITL::ADSB *adsb; + // simulated vicon system: + SITL::Vicon *vicon; + // output socket for flightgear viewing SocketAPM fg_socket{true};