AP_HAL_SITL: add vicon-over-mavlink support

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();

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};