From 85b41a8abb90fefe7d57310c75270a97000fc20d Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 8 Mar 2018 12:25:34 +1100
Subject: [PATCH] AP_HAL_SITL: add vicon-over-mavlink support

---
 libraries/AP_HAL_SITL/SITL_State.cpp | 14 ++++++++++++++
 libraries/AP_HAL_SITL/SITL_State.h   |  4 ++++
 2 files changed, 18 insertions(+)

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 <SITL/SITL.h>
 #include <SITL/SIM_Gimbal.h>
 #include <SITL/SIM_ADSB.h>
+#include <SITL/SIM_Vicon.h>
 #include <AP_HAL/utility/Socket.h>
 
 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};