SITL: add VICON_YAWERR

this allows simulating an error in the camera's reported yaw

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(

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

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