fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 19:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink odometry: properly set the frame id fields

Browse Source
This commit is contained in:
TSC21
2020-05-28 15:45:13 +01:00
committed by Nuno Marques
parent 562d57fee8
commit ad4d4287fa
3 changed files with 25 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator_mavlink.cpp
+6 -1
View File
@@ -1037,7 +1037,12 @@ int Simulator::publish_odometry_topic(const 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 = vehicle_odometry_s::LOCAL_FRAME_FRD;
if (odom_msg.frame_id == MAV_FRAME_LOCAL_NED) {
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
} else {
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
}
static_assert(POS_URT_SIZE == (sizeof(odom_msg.pose_covariance) / sizeof(odom_msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");