SITL: add VICON_GLIT_XYZ, FAIL and YAW

glitches are in meters in vicon's frame
fail disables sending of vision-position-estimate messages
yaw is the vicon's heading in degrees.  i.e. if vicon's "forward" is
pointing to the east, yaw should be set to 90

libraries/SITL/SIM_Vicon.cpp

@@ -88,17 +88,34 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
         return;
     }
 
+    // failure simulation
+    if (_sitl->vicon_fail.get() != 0) {
+        return;
+    }
+
     float roll;
     float pitch;
     float yaw;
     attitude.to_euler(roll, pitch, yaw);
 
-    // calculate sensor offset in earth frame and add to position
+    // calculate sensor offset in earth frame
     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;
+
+    // add earth frame sensor offset and glitch to position
+    Vector3f pos_corrected = position + pos_offset_ef + _sitl->vicon_glitch.get();
+
+    // adjust yaw and position to account for vicon's yaw
+    const int16_t vicon_yaw_deg = _sitl->vicon_yaw.get();
+    if (vicon_yaw_deg != 0) {
+        const float vicon_yaw_rad = radians(vicon_yaw_deg);
+        yaw = wrap_PI(yaw - vicon_yaw_rad);
+        Matrix3f vicon_yaw_rot;
+        vicon_yaw_rot.from_euler(0, 0, -vicon_yaw_rad);
+        pos_corrected = vicon_yaw_rot * pos_corrected;
+    }
 
 #if USE_VISION_POSITION_ESTIMATE
     // use the more recent VISION_POSITION_ESTIMATE message

libraries/SITL/SITL.cpp

@@ -241,6 +241,15 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
     // Buyoancy for submarines
     AP_GROUPINFO_FRAME("BUOYANCY", 15, SITL, buoyancy, 1, AP_PARAM_FRAME_SUB),
 
+    // vicon glitch in NED frame
+    AP_GROUPINFO("VICON_GLIT",    16, SITL,  vicon_glitch, 0),
+
+    // vicon failure
+    AP_GROUPINFO("VICON_FAIL",    17, SITL,  vicon_fail, 0),
+
+    // vicon yaw (in earth frame)
+    AP_GROUPINFO("VICON_YAW",     18, SITL,  vicon_yaw, 0),
+
     AP_GROUPEND
 
 };

libraries/SITL/SITL.h

@@ -363,6 +363,11 @@ public:
     EFI_MegaSquirt efi_ms;
 
     AP_Int8 led_layout;
+
+    // vicon parameters
+    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
 };
 
 } // namespace SITL