SITL: add vicon position offsets

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,

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

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;