From 366d03a53374068a70c315116a008cd4a7e9aa62 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 13 May 2020 13:09:09 +0900
Subject: [PATCH] SITL: add VICON_YAWERR

this allows simulating an error in the camera's reported yaw
---
 libraries/SITL/SIM_Vicon.cpp | 3 +++
 libraries/SITL/SITL.cpp      | 3 +++
 libraries/SITL/SITL.h        | 1 +
 3 files changed, 7 insertions(+)

diff --git a/libraries/SITL/SIM_Vicon.cpp b/libraries/SITL/SIM_Vicon.cpp
index 85bef5ff4f..1123204a39 100644
--- a/libraries/SITL/SIM_Vicon.cpp
+++ b/libraries/SITL/SIM_Vicon.cpp
@@ -117,6 +117,9 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
         pos_corrected = vicon_yaw_rot * pos_corrected;
     }
 
+    // add yaw error reported to vehicle
+    yaw = wrap_PI(yaw + radians(_sitl->vicon_yaw_error.get()));
+
 #if USE_VISION_POSITION_ESTIMATE
     // use the more recent VISION_POSITION_ESTIMATE message
     mavlink_msg_vision_position_estimate_pack_chan(
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index cb07a4ac75..ad8d9db1b4 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -248,6 +248,9 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
     // vicon yaw (in earth frame)
     AP_GROUPINFO("VICON_YAW",     18, SITL,  vicon_yaw, 0),
 
+    // vicon yaw error in degrees (added to reported yaw sent to vehicle)
+    AP_GROUPINFO("VICON_YAWERR",  19, SITL,  vicon_yaw_error, 0),
+
     AP_GROUPEND
 
 };
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 06713c1eaf..5697a92a85 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -366,6 +366,7 @@ public:
     AP_Vector3f vicon_glitch;   // glitch in meters in vicon's local NED frame
     AP_Int8 vicon_fail;         // trigger vicon failure
     AP_Int16 vicon_yaw;         // vicon local yaw in degrees
+    AP_Int16 vicon_yaw_error;   // vicon yaw error in degrees (added to reported yaw sent to vehicle)
 };
 
 } // namespace SITL