From 746250d86d1d05fce3d416170101e22b62e15cbd Mon Sep 17 00:00:00 2001
From: TSC21 <n.marques21@hotmail.com>
Date: Thu, 26 Jul 2018 17:55:03 +0100
Subject: [PATCH] use static_assert over covariance matrices URT array size
---
src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++++++++--------
src/modules/simulator/simulator_mavlink.cpp | 10 +++++++++-
2 files changed, 21 insertions(+), 9 deletions(-)
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c7ea421b70..471e5a05f1 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -835,8 +835,9 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
mocap_odom.q[2] = mocap.q[2];
mocap_odom.q[3] = mocap.q[3];
- 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])));
+ const size_t URT_SIZE = sizeof(mocap_odom.pose_covariance) / sizeof(mocap_odom.pose_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++) {
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));
q.copyTo(visual_odom.q);
- 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])));
+ const size_t URT_SIZE = sizeof(visual_odom.pose_covariance) / sizeof(visual_odom.pose_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++) {
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);
q.copyTo(odometry.q);
- 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]);
- assert(POS_URT_SIZE == (sizeof(odom.covariance) / sizeof(odom.covariance[0])));
- assert(VEL_URT_SIZE == (sizeof(odom.twist_covariance) / sizeof(odom.twist_covariance[0])));
+ const size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]);
+ const size_t VEL_URT_SIZE = sizeof(odometry.velocity_covariance) / sizeof(odometry.velocity_covariance[0]);
+ static_assert(POS_URT_SIZE == (sizeof(odom.pose_covariance) / sizeof(odom.pose_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
for (size_t i = 0; i < POS_URT_SIZE; i++) {
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 52c933dc64..a991df78bd 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -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]);
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 */
- 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];
}
@@ -1169,6 +1173,10 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
odom.pitchspeed = odom_msg.pitchspeed;
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 */
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];
--
GitLab