dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink odometry: properly set the frame id fields

This commit is contained in:
TSC21 2020-05-28 14:50:34 +01:00 committed by Nuno Marques
parent 562d57fee8
commit ad4d4287fa
3 changed files with 25 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/mavlink/mavlink_messages.cpp
View File

@ -2548,13 +2548,23 @@ protected:
if (_mavlink->odometry_loopback_enabled()) {
odom_updated = _vodom_sub.update(&odom);
msg.frame_id = MAV_FRAME_LOCAL_NED;
// set the frame_id according to the local frame of the data
if (odom.local_frame == vehicle_odometry_s::LOCAL_FRAME_NED) {
msg.frame_id = MAV_FRAME_LOCAL_NED;
} else {
msg.frame_id = MAV_FRAME_LOCAL_FRD;
}
// source: external vision system
msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION;
} else {
odom_updated = _odom_sub.update(&odom);
msg.frame_id = MAV_FRAME_LOCAL_NED;
// source: PX4 estimator
msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT;
}

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

@ -1309,8 +1309,6 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
q_body_to_local.normalize();
q_body_to_local.copyTo(odometry.q);
odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
// pose_covariance
static constexpr size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]);
static_assert(POS_URT_SIZE == (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])),
@ -1361,6 +1359,14 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
* with true North
*/
if (odom.frame_id == MAV_FRAME_LOCAL_NED || odom.frame_id == MAV_FRAME_LOCAL_FRD) {
if (odom.frame_id == MAV_FRAME_LOCAL_NED) {
odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
} else {
odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
}
if (odom.estimator_type == MAV_ESTIMATOR_TYPE_VISION || odom.estimator_type == MAV_ESTIMATOR_TYPE_VIO) {
_visual_odometry_pub.publish(odometry);

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

@ -1037,7 +1037,12 @@ int Simulator::publish_odometry_topic(const 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);
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
if (odom_msg.frame_id == MAV_FRAME_LOCAL_NED) {
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
} else {
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
}
static_assert(POS_URT_SIZE == (sizeof(odom_msg.pose_covariance) / sizeof(odom_msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");