fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 12:57:35 +08: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
src/modules/simulator/simulator.h
+1
View File
@@ -176,6 +176,7 @@ private:
uORB::PublicationMulti<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
uORB::Publication<irlock_report_s> _irlock_report_pub{ORB_ID(irlock_report)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
uORB::PublicationMulti<distance_sensor_s> *_dist_pubs[RANGE_FINDER_MAX_SENSORS] {};
uint8_t _dist_sensor_ids[RANGE_FINDER_MAX_SENSORS] {};
src/modules/simulator/simulator_mavlink.cpp
+13 -3
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;
}