Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 19:32:36 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

mavlink odometry: properly set the frame id fields

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+11 -1
View File
@@ -2548,13 +2548,23 @@ protected:
if (_mavlink->odometry_loopback_enabled()) { if (_mavlink->odometry_loopback_enabled()) {
odom_updated = _vodom_sub.update(&odom); 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 // source: external vision system
msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION; msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION;
} else { } else {
odom_updated = _odom_sub.update(&odom); odom_updated = _odom_sub.update(&odom);
msg.frame_id = MAV_FRAME_LOCAL_NED; msg.frame_id = MAV_FRAME_LOCAL_NED;
// source: PX4 estimator // source: PX4 estimator
msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT; msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT;
} }
src/modules/mavlink/mavlink_receiver.cpp
+8 -2
View File
@@ -1309,8 +1309,6 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
q_body_to_local.normalize(); q_body_to_local.normalize();
q_body_to_local.copyTo(odometry.q); q_body_to_local.copyTo(odometry.q);
odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
// pose_covariance // pose_covariance
static constexpr size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]); 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])), 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 * 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 || 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) { if (odom.estimator_type == MAV_ESTIMATOR_TYPE_VISION || odom.estimator_type == MAV_ESTIMATOR_TYPE_VIO) {
_visual_odometry_pub.publish(odometry); _visual_odometry_pub.publish(odometry);
src/modules/simulator/simulator_mavlink.cpp
+6 -1
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]); 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);
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])), static_assert(POS_URT_SIZE == (sizeof(odom_msg.pose_covariance) / sizeof(odom_msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch"); "Odometry Pose Covariance matrix URT array size mismatch");