yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: fix odometry frames of reference setup

Browse Source
This commit is contained in:
TSC21
2020-05-28 11:14:52 +01:00
committed by Nuno Marques
parent 0ec48cfef3
commit 562d57fee8
6 changed files with 45 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1068,6 +1068,17 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
odom.velocity_covariance[i] = odom_msg.velocity_covariance[i];
}
/* Publish the odometry based on the source */
if (odom_msg.estimator_type == MAV_ESTIMATOR_TYPE_VISION || odom_msg.estimator_type == MAV_ESTIMATOR_TYPE_VIO) {
_visual_odometry_pub.publish(odom);
} else if (odom_msg.estimator_type == MAV_ESTIMATOR_TYPE_MOCAP) {
_mocap_odometry_pub.publish(odom);
} else {
PX4_ERR("Estimator source %u not supported. Unable to publish pose and velocity", odom_msg.estimator_type);
}
} else if (odom_mavlink->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
mavlink_vision_position_estimate_t ev;
mavlink_msg_vision_position_estimate_decode(odom_mavlink, &ev);
@@ -1102,11 +1113,10 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
/* The velocity covariance URT - unknown */
odom.velocity_covariance[0] = NAN;
/* Publish the odometry */
_visual_odometry_pub.publish(odom);
}
/** @note: frame_id == MAV_FRAME_VISION_NED) */
_visual_odometry_pub.publish(odom);
return PX4_OK;
}