From 3ccb3c69a62c00af001cb83d2049b1a5a880ae72 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 13 Apr 2020 15:04:26 +0900
Subject: [PATCH] SITL: add vicon position offsets

---
 libraries/SITL/SIM_Vicon.cpp | 19 +++++++++++++------
 libraries/SITL/SITL.cpp      |  7 +++++--
 libraries/SITL/SITL.h        |  1 +
 3 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/libraries/SITL/SIM_Vicon.cpp b/libraries/SITL/SIM_Vicon.cpp
index a0f7b78830..217c9de521 100644
--- a/libraries/SITL/SIM_Vicon.cpp
+++ b/libraries/SITL/SIM_Vicon.cpp
@@ -93,6 +93,13 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
     float yaw;
     attitude.to_euler(roll, pitch, yaw);
 
+    // calculate sensor offset in earth frame and add to position
+    const Vector3f& pos_offset = _sitl->vicon_pos_offset.get();
+    Matrix3f rot;
+    rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(_sitl->state.yawDeg));
+    Vector3f pos_offset_ef = rot * pos_offset;
+    const Vector3f pos_corrected = position + pos_offset_ef;
+
 #if USE_VISION_POSITION_ESTIMATE
     // use the more recent VISION_POSITION_ESTIMATE message
     mavlink_msg_vision_position_estimate_pack_chan(
@@ -101,9 +108,9 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
         mavlink_ch,
         &obs_msg,
         now_us + time_offset_us,
-        position.x,
-        position.y,
-        position.z,
+        pos_corrected.x,
+        pos_corrected.y,
+        pos_corrected.z,
         roll,
         pitch,
         yaw,
@@ -115,9 +122,9 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
         mavlink_ch,
         &obs_msg,
         now_us + time_offset_us,
-        position.x,
-        position.y,
-        position.z,
+        pos_corrected.x,
+        pos_corrected.y,
+        pos_corrected.z,
         roll,
         pitch,
         yaw,
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 3594464c8b..2a38ca090e 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -231,10 +231,13 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
     AP_GROUPINFO("LED_LAYOUT",    11, SITL, led_layout, 0),
 
     // Scenario for thermalling simulation, for soaring
-    AP_GROUPINFO("THML_SCENARI",12, SITL,  thermal_scenario, 0),
-    
+    AP_GROUPINFO("THML_SCENARI",  12, SITL,  thermal_scenario, 0),
+
     AP_GROUPINFO("GPS2_HDG",      13, SITL,  gps_hdg_enabled[1], 0),
 
+    // vicon sensor position (position offsets in body frame)
+    AP_GROUPINFO("VICON_POS",     14, SITL,  vicon_pos_offset, 0),
+
     AP_GROUPEND
 
 };
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index fb506ee937..60d9097316 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -238,6 +238,7 @@ public:
     AP_Vector3f gps_pos_offset[2];  // XYZ position of the GPS antenna phase centre relative to the body frame origin (m)
     AP_Vector3f rngfnd_pos_offset;  // XYZ position of the range finder zero range datum relative to the body frame origin (m)
     AP_Vector3f optflow_pos_offset; // XYZ position of the optical flow sensor focal point relative to the body frame origin (m)
+    AP_Vector3f vicon_pos_offset;   // XYZ position of the vicon sensor relative to the body frame origin (m)
 
     // temperature control
     AP_Float temp_start;