Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

mavlink: fix odometry frames of reference setup #14990

Merged
merged 2 commits into from
May 28, 2020
Merged

mavlink: fix odometry frames of reference setup #14990

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bbb3a57
mavlink: fix odometry frames of reference setup
TSC21 May 28, 2020
0a286b0
mavlink odometry: properly set the frame id fields
TSC21 May 28, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion Tools/sitl_gazebo
Open in desktop
Submodule sitl_gazebo updated from 2736a9 to 20e8db
2 changes: 1 addition & 1 deletion mavlink/include/mavlink/v2.0
Open in desktop
Submodule v2.0 updated 336 files
10 changes: 6 additions & 4 deletions src/modules/mavlink/mavlink_messages.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2548,13 +2548,15 @@ class MavlinkStreamOdometry : public MavlinkStream

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) {
Expand Down
37 changes: 23 additions & 14 deletions src/modules/mavlink/mavlink_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]);
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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);
Expand Down
1 change: 1 addition & 0 deletions src/modules/simulator/simulator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -176,6 +176,7 @@ class Simulator : public ModuleParams
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] {};
Expand Down
16 changes: 13 additions & 3 deletions src/modules/simulator/simulator_mavlink.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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;
}

Expand Down