From 7cc71dc573a02f591b28f67829ccc819eeba0f70 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 29 Dec 2020 21:30:34 +1100
Subject: [PATCH] HAL_SITL: support VectorNav simulation

---
 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 8ebd905d48..548b5e872f 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -376,6 +376,12 @@ int SITL_State::sim_fd(const char *name, const char *arg)
         }
         gyus42v2 = new SITL::RF_GYUS42v2();
         return gyus42v2->fd();
+    } else if (streq(name, "VectorNav")) {
+        if (vectornav != nullptr) {
+            AP_HAL::panic("Only one VectorNav at a time");
+        }
+        vectornav = new SITL::VectorNav();
+        return vectornav->fd();
     }
 
     AP_HAL::panic("unknown simulated device: %s", name);
@@ -491,6 +497,11 @@ int SITL_State::sim_fd_write(const char *name)
             AP_HAL::panic("No gyus42v2 created");
         }
         return gyus42v2->write_fd();
+    } else if (streq(name, "VectorNav")) {
+        if (vectornav == nullptr) {
+            AP_HAL::panic("No VectorNav created");
+        }
+        return vectornav->write_fd();
     }
     AP_HAL::panic("unknown simulated device: %s", name);
 }
@@ -700,6 +711,9 @@ void SITL_State::_fdm_input_local(void)
     if (sf45b != nullptr) {
         sf45b->update(sitl_model->get_location());
     }
+    if (vectornav != nullptr) {
+        vectornav->update();
+    }
 
     if (_sitl) {
         _sitl->efi_ms.update();
diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h
index 5da8ed5896..d1be45927a 100644
--- a/libraries/AP_HAL_SITL/SITL_State.h
+++ b/libraries/AP_HAL_SITL/SITL_State.h
@@ -43,6 +43,7 @@
 #include <SITL/SIM_RF_NMEA.h>
 #include <SITL/SIM_RF_MAVLink.h>
 #include <SITL/SIM_RF_GYUS42v2.h>
+#include <SITL/SIM_VectorNav.h>
 
 #include <SITL/SIM_Frsky_D.h>
 #include <SITL/SIM_CRSF.h>
@@ -300,6 +301,9 @@ private:
     // simulated CRSF devices
     SITL::CRSF *crsf;
 
+    // simulated VectorNav system:
+    SITL::VectorNav *vectornav;
+    
     // output socket for flightgear viewing
     SocketAPM fg_socket{true};