fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 11:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink odometry: properly set the frame id fields

Browse Source
This commit is contained in:
TSC21
2020-05-28 15:45:13 +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_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.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);