Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: add vicon position offsets

This commit is contained in:
Randy Mackay 2020-04-13 15:04:26 +09:00 committed by Andrew Tridgell
parent 2164293cc0
commit 3ccb3c69a6
3 changed files with 19 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/SITL/SIM_Vicon.cpp
View File

@ -93,6 +93,13 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
float yaw; float yaw;
attitude.to_euler(roll, pitch, 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 #if USE_VISION_POSITION_ESTIMATE
// use the more recent VISION_POSITION_ESTIMATE message // use the more recent VISION_POSITION_ESTIMATE message
mavlink_msg_vision_position_estimate_pack_chan( mavlink_msg_vision_position_estimate_pack_chan(
@ -101,9 +108,9 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
mavlink_ch, mavlink_ch,
&obs_msg, &obs_msg,
now_us + time_offset_us, now_us + time_offset_us,
position.x, pos_corrected.x,
position.y, pos_corrected.y,
position.z, pos_corrected.z,
roll, roll,
pitch, pitch,
yaw, yaw,
@ -115,9 +122,9 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
mavlink_ch, mavlink_ch,
&obs_msg, &obs_msg,
now_us + time_offset_us, now_us + time_offset_us,
position.x, pos_corrected.x,
position.y, pos_corrected.y,
position.z, pos_corrected.z,
roll, roll,
pitch, pitch,
yaw, yaw,

3
libraries/SITL/SITL.cpp
View File

@ -235,6 +235,9 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
AP_GROUPINFO("GPS2_HDG", 13, SITL, gps_hdg_enabled[1], 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 AP_GROUPEND
}; };

1
libraries/SITL/SITL.h
View File

@ -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 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 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 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 // temperature control
AP_Float temp_start; AP_Float temp_start;