AP_HAL_SITL: add vicon-over-mavlink support

SharSam · Mar 27, 2018 · 85b41a8 · 85b41a8
1 parent 1151d76
commit 85b41a8
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 0 deletions.
diff --git 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
@@ -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 @@ class HALSITL::SITL_State {
     // simulated ADSb
     SITL::ADSB *adsb;
 
+    // simulated vicon system:
+    SITL::Vicon *vicon;
+
     // output socket for flightgear viewing
     SocketAPM fg_socket{true};