fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-15 12:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

update mavlink and sitl_gazebo to latest with odometry velocity covariance

Browse Source
This commit is contained in:
Daniel Agar
2019-02-10 16:32:59 -05:00
committed by GitHub
parent fafadd4686
commit 7c3999e00e
6 changed files with 29 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+7 -3
View File
@@ -2361,11 +2361,15 @@ protected:
msg.yawspeed = odom.yawspeed;
// get the covariance matrix size
// pose_covariance
static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
static_assert(POS_URT_SIZE == (sizeof(msg.pose_covariance) / sizeof(msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
static_assert(VEL_URT_SIZE == (sizeof(msg.twist_covariance) / sizeof(msg.twist_covariance[0])),
// velocity_covariance
static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
static_assert(VEL_URT_SIZE == (sizeof(msg.velocity_covariance) / sizeof(msg.velocity_covariance[0])),
"Odometry Velocity Covariance matrix URT array size mismatch");
// copy pose covariances
@@ -2376,7 +2380,7 @@ protected:
// copy velocity covariances
//TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
msg.twist_covariance[i] = odom.velocity_covariance[i];
msg.velocity_covariance[i] = odom.velocity_covariance[i];
}
mavlink_msg_odometry_send_struct(_mavlink->get_channel(), &msg);