From bbb3a57755cdef89a82b6ba2ad93a8d7af90c414 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 28 May 2020 11:14:52 +0100 Subject: [PATCH 1/2] mavlink: fix odometry frames of reference setup --- Tools/sitl_gazebo | 2 +- mavlink/include/mavlink/v2.0 | 2 +- src/modules/mavlink/mavlink_messages.cpp | 10 +++--- src/modules/mavlink/mavlink_receiver.cpp | 37 +++++++++++++-------- src/modules/simulator/simulator.h | 1 + src/modules/simulator/simulator_mavlink.cpp | 16 +++++++-- 6 files changed, 45 insertions(+), 23 deletions(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 2736a9134d7f..20e8dbe11502 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 2736a9134d7fed0cae7b7836d4370f9a4dfef439 +Subproject commit 20e8dbe115028721b1918069dc7afd9145c7483c diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index c6e18fb3389e..5f1d5d8338ff 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit c6e18fb3389eb29ee9993829e775a903e003d92a +Subproject commit 5f1d5d8338ff15108bd11137fc6970282f369246 diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8966e0787a9c..844d2d5c089e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 22e0d0d8b8b6..e14839b98fa0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 6f940f83de35..4c5583adf095 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -176,6 +176,7 @@ class Simulator : public ModuleParams uORB::PublicationMulti _flow_pub{ORB_ID(optical_flow)}; uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; uORB::PublicationMulti *_dist_pubs[RANGE_FINDER_MAX_SENSORS] {}; uint8_t _dist_sensor_ids[RANGE_FINDER_MAX_SENSORS] {}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index e979c150bf7a..3de39280232f 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -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; } From 0a286b08774f79d686995990df6e28cf17b19d13 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 28 May 2020 14:50:34 +0100 Subject: [PATCH 2/2] mavlink odometry: properly set the frame id fields --- src/modules/mavlink/mavlink_messages.cpp | 12 +++++++++++- src/modules/mavlink/mavlink_receiver.cpp | 10 ++++++++-- src/modules/simulator/simulator_mavlink.cpp | 7 ++++++- 3 files changed, 25 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 844d2d5c089e..0e92dc1177f0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2548,13 +2548,23 @@ class MavlinkStreamOdometry : public MavlinkStream if (_mavlink->odometry_loopback_enabled()) { odom_updated = _vodom_sub.update(&odom); - msg.frame_id = MAV_FRAME_LOCAL_NED; + + // set the frame_id according to the local frame of the data + if (odom.local_frame == vehicle_odometry_s::LOCAL_FRAME_NED) { + msg.frame_id = MAV_FRAME_LOCAL_NED; + + } else { + msg.frame_id = MAV_FRAME_LOCAL_FRD; + } + // source: external vision system msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION; } else { odom_updated = _odom_sub.update(&odom); + msg.frame_id = MAV_FRAME_LOCAL_NED; + // source: PX4 estimator msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e14839b98fa0..5e42484ec72a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 3de39280232f..47ed1f5dbcdd 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -1037,7 +1037,12 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]); q.copyTo(odom.q); - odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; + if (odom_msg.frame_id == MAV_FRAME_LOCAL_NED) { + odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED; + + } else { + odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; + } static_assert(POS_URT_SIZE == (sizeof(odom_msg.pose_covariance) / sizeof(odom_msg.pose_covariance[0])), "Odometry Pose Covariance matrix URT array size mismatch");