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

add covariance matrices index aliases

Browse Source
This commit is contained in:
TSC21
2018-09-18 09:52:33 +02:00
committed by Lorenz Meier
parent 1cfaccfd7b
commit cc73f214d1
7 changed files with 98 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator_mavlink.cpp
+6 -2
View File
@@ -1131,6 +1131,8 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
odom.timestamp = timestamp;
const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
if (odom_mavlink->msgid == MAVLINK_MSG_ID_ODOMETRY) {
mavlink_odometry_t odom_msg;
mavlink_msg_odometry_decode(odom_mavlink, &odom_msg);
@@ -1155,7 +1157,6 @@ 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);
const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
static_assert(POS_URT_SIZE == (sizeof(odom_msg.pose_covariance) / sizeof(odom_msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
@@ -1194,8 +1195,11 @@ 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);
static_assert(POS_URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0])),
"Vision Position Estimate Pose Covariance matrix URT array size mismatch");
/* The pose covariance URT */
for (size_t i = 0; i < (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])); i++) {
for (size_t i = 0; i < POS_URT_SIZE; i++) {
odom.pose_covariance[i] = ev.covariance[i];
}