fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

assert over the covariance matrices URT size matching

Browse Source
This commit is contained in:
TSC21 2018-07-26 16:12:40 +01:00 committed by Lorenz Meier
parent eeca8d4efe
commit 7db57bedb7
3 changed files with 18 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -835,6 +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])));
for (size_t i = 0; i < URT_SIZE; i++) {
mocap_odom.pose_covariance[i] = mocap.covariance[i];
}
@ -1171,6 +1174,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])));
for (size_t i = 0; i < URT_SIZE; i++) {
visual_odom.pose_covariance[i] = ev.covariance[i];
}
@ -1208,8 +1214,13 @@ 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])));
// create a method to simplify covariance copy
for (size_t i = 0; i < URT_SIZE; i++) {
for (size_t i = 0; i < POS_URT_SIZE; i++) {
odometry.pose_covariance[i] = odom.pose_covariance[i];
}
@ -1231,7 +1242,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
odometry.yawspeed = odom.yawspeed;
//TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame
for (size_t i = 0; i < URT_SIZE; i++) {
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odometry.velocity_covariance[i] = odom.twist_covariance[i];
}
@ -1253,7 +1264,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
odometry.yawspeed = odom.yawspeed;
//TODO: Apply rotation matrix to transform from body-fixed to earth-fixed NED frame
for (size_t i = 0; i < URT_SIZE; i++) {
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odometry.velocity_covariance[i] = odom.twist_covariance[i];
}
@ -1278,7 +1289,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
odometry.yawspeed = angvel_local(2);
//TODO: Apply rotation matrix to transform from earth-fixed to body-fixed NED frame
for (size_t i = 0; i < URT_SIZE; i++) {
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odometry.velocity_covariance[i] = odom.twist_covariance[i];
}

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -266,8 +266,6 @@ private:
param_t _p_bat_crit_thr;
param_t _p_bat_low_thr;
static const size_t URT_SIZE = 21;
MavlinkReceiver(const MavlinkReceiver &) = delete;
MavlinkReceiver operator=(const MavlinkReceiver &) = delete;
};

6
src/modules/simulator/simulator_mavlink.cpp
View File

@ -1156,7 +1156,7 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
q.copyTo(odom.q);
/* The pose covariance URT */
for (size_t i = 0; i < 21; i++) {
for (size_t i = 0; i < (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])); i++) {
odom.pose_covariance[i] = odom_msg.pose_covariance[i];
}
@ -1170,7 +1170,7 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
odom.yawspeed = odom_msg.yawspeed;
/* The velocity covariance URT */
for (size_t i = 0; i < 21; 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];
}
@ -1187,7 +1187,7 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
q.copyTo(odom.q);
/* The pose covariance URT */
for (size_t i = 0; i < 21; i++) {
for (size_t i = 0; i < (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])); i++) {
odom.pose_covariance[i] = ev.covariance[i];
}