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

use static_assert over covariance matrices URT array size

Browse Source
This commit is contained in:
TSC21
2018-09-18 09:52:33 +02:00
committed by Lorenz Meier
parent 7db57bedb7
commit 746250d86d
2 changed files with 21 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+12 -8
View File
@@ -835,8 +835,9 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
mocap_odom.q[2] = mocap.q[2]; mocap_odom.q[2] = mocap.q[2];
mocap_odom.q[3] = mocap.q[3]; mocap_odom.q[3] = mocap.q[3];
size_t URT_SIZE = sizeof(mocap_odom.pose_covariance) / sizeof(mocap_odom.pose_covariance[0]); const size_t URT_SIZE = sizeof(mocap_odom.pose_covariance) / sizeof(mocap_odom.pose_covariance[0]);
assert(URT_SIZE == (sizeof(mocap.covariance) / sizeof(mocap.covariance[0]))); static_assert(URT_SIZE == (sizeof(mocap.covariance) / sizeof(mocap.covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
for (size_t i = 0; i < URT_SIZE; i++) { for (size_t i = 0; i < URT_SIZE; i++) {
mocap_odom.pose_covariance[i] = mocap.covariance[i]; mocap_odom.pose_covariance[i] = mocap.covariance[i];
@@ -1174,8 +1175,9 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw)); matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw));
q.copyTo(visual_odom.q); q.copyTo(visual_odom.q);
size_t URT_SIZE = sizeof(visual_odom.pose_covariance) / sizeof(visual_odom.pose_covariance[0]); const size_t URT_SIZE = sizeof(visual_odom.pose_covariance) / sizeof(visual_odom.pose_covariance[0]);
assert(URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0]))); static_assert(URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
for (size_t i = 0; i < URT_SIZE; i++) { for (size_t i = 0; i < URT_SIZE; i++) {
visual_odom.pose_covariance[i] = ev.covariance[i]; visual_odom.pose_covariance[i] = ev.covariance[i];
@@ -1214,10 +1216,12 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
matrix::Quatf q(odom.q); matrix::Quatf q(odom.q);
q.copyTo(odometry.q); q.copyTo(odometry.q);
size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]); const size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]);
size_t VEL_URT_SIZE = sizeof(odometry.velocity_covariance) / sizeof(odometry.velocity_covariance[0]); const size_t VEL_URT_SIZE = sizeof(odometry.velocity_covariance) / sizeof(odometry.velocity_covariance[0]);
assert(POS_URT_SIZE == (sizeof(odom.covariance) / sizeof(odom.covariance[0]))); static_assert(POS_URT_SIZE == (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])),
assert(VEL_URT_SIZE == (sizeof(odom.twist_covariance) / sizeof(odom.twist_covariance[0]))); "Odometry Pose Covariance matrix URT array size mismatch");
static_assert(VEL_URT_SIZE == (sizeof(odom.twist_covariance) / sizeof(odom.twist_covariance[0])),
"Odometry Velocity Covariance matrix URT array size mismatch");
// create a method to simplify covariance copy // create a method to simplify covariance copy
for (size_t i = 0; i < POS_URT_SIZE; i++) { for (size_t i = 0; i < POS_URT_SIZE; i++) {
src/modules/simulator/simulator_mavlink.cpp
+9 -1
View File
@@ -1155,8 +1155,12 @@ 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]); matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]);
q.copyTo(odom.q); 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");
/* The pose covariance URT */ /* 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] = odom_msg.pose_covariance[i]; odom.pose_covariance[i] = odom_msg.pose_covariance[i];
} }
@@ -1169,6 +1173,10 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
odom.pitchspeed = odom_msg.pitchspeed; odom.pitchspeed = odom_msg.pitchspeed;
odom.yawspeed = odom_msg.yawspeed; odom.yawspeed = odom_msg.yawspeed;
const size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
static_assert(VEL_URT_SIZE == (sizeof(odom_msg.twist_covariance) / sizeof(odom_msg.twist_covariance[0])),
"Odometry Velocity Covariance matrix URT array size mismatch");
/* The velocity covariance URT */ /* The velocity covariance URT */
for (size_t i = 0; i < (sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0])); i++) { for (size_t i = 0; i < (sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0])); i++) {
odom.velocity_covariance[i] = odom_msg.twist_covariance[i]; odom.velocity_covariance[i] = odom_msg.twist_covariance[i];