fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/sitl_gazebo

@ -1 +1 @@
Subproject commit 2736a9134d7fed0cae7b7836d4370f9a4dfef439
Subproject commit 20e8dbe115028721b1918069dc7afd9145c7483c

2
mavlink/include/mavlink/v2.0

@ -1 +1 @@
Subproject commit c6e18fb3389eb29ee9993829e775a903e003d92a
Subproject commit 5f1d5d8338ff15108bd11137fc6970282f369246

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

@ -2548,13 +2548,15 @@ protected:
if (_mavlink->odometry_loopback_enabled()) {
odom_updated = _vodom_sub.update(&odom);
// frame matches the external vision system
msg.frame_id = MAV_FRAME_VISION_NED;
msg.frame_id = MAV_FRAME_LOCAL_NED;
// source: external vision system
msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION;
} else {
odom_updated = _odom_sub.update(&odom);
// frame matches the PX4 local NED frame
msg.frame_id = MAV_FRAME_ESTIM_NED;
msg.frame_id = MAV_FRAME_LOCAL_NED;
// source: PX4 estimator
msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT;
}
if (odom_updated) {

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

@ -1263,10 +1263,6 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw));
q.copyTo(visual_odom.q);
// TODO:
// - add a MAV_FRAME_*_OTHER to the Mavlink MAV_FRAME enum IOT define
// a frame of reference which is not aligned with NED or ENU
// - add usage on the estimator side
visual_odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
const size_t URT_SIZE = sizeof(visual_odom.pose_covariance) / sizeof(visual_odom.pose_covariance[0]);
@ -1305,15 +1301,14 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
odometry.y = odom.y;
odometry.z = odom.z;
/* The quaternion of the ODOMETRY msg represents a rotation from body frame to
* a local frame*/
/**
* The quaternion of the ODOMETRY msg represents a rotation from body frame
* to a local frame
*/
matrix::Quatf q_body_to_local(odom.q);
q_body_to_local.normalize();
q_body_to_local.copyTo(odometry.q);
// TODO:
// add usage of this information on the estimator side
// The heading of the local frame is not aligned with north
odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
// pose_covariance
@ -1331,7 +1326,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
odometry.pose_covariance[i] = odom.pose_covariance[i];
}
/*
/**
* PX4 expects the body's linear velocity in the local frame,
* the linear velocity is rotated from the odom child_frame to the
* local NED frame. The angular velocity needs to be expressed in the
@ -1356,11 +1351,25 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
PX4_ERR("Body frame %u not supported. Unable to publish velocity", odom.child_frame_id);
}
if (odom.frame_id == MAV_FRAME_LOCAL_FRD) {
_visual_odometry_pub.publish(odometry);
/**
* Supported local frame of reference is MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_FRD
* The supported sources of the data/tesimator type are MAV_ESTIMATOR_TYPE_VISION,
* MAV_ESTIMATOR_TYPE_VIO and MAV_ESTIMATOR_TYPE_MOCAP
*
* @note Regarding the local frames of reference, the appropriate EKF_AID_MASK
* should be set in order to match a frame aligned (NED) or not aligned (FRD)
* with true North
*/
if (odom.frame_id == MAV_FRAME_LOCAL_NED || odom.frame_id == MAV_FRAME_LOCAL_FRD) {
if (odom.estimator_type == MAV_ESTIMATOR_TYPE_VISION || odom.estimator_type == MAV_ESTIMATOR_TYPE_VIO) {
_visual_odometry_pub.publish(odometry);
} else if (odom.frame_id == MAV_FRAME_MOCAP_NED) {
_mocap_odometry_pub.publish(odometry);
} else if (odom.estimator_type == MAV_ESTIMATOR_TYPE_MOCAP) {
_mocap_odometry_pub.publish(odometry);
} else {
PX4_ERR("Estimator source %u not supported. Unable to publish pose and velocity", odom.estimator_type);
}
} else {
PX4_ERR("Local frame %u not supported. Unable to publish pose and velocity", odom.frame_id);

1
src/modules/simulator/simulator.h
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] {};

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;
}