yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

add a local frame of reference field to vehicle_odometry

Browse Source
This commit is contained in:
TSC21
2018-09-14 12:09:29 +01:00
committed by Lorenz Meier
parent cc73f214d1
commit 095cdeb4b0
3 changed files with 27 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -1157,6 +1157,8 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]);
q.copyTo(odom.q);
odom.local_frame = odom.LOCAL_FRAME_NED;
static_assert(POS_URT_SIZE == (sizeof(odom_msg.pose_covariance) / sizeof(odom_msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
@@ -1195,6 +1197,8 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw));
q.copyTo(odom.q);
odom.local_frame = odom.LOCAL_FRAME_NED;
static_assert(POS_URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0])),
"Vision Position Estimate Pose Covariance matrix URT array size mismatch");