diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 2736a9134d..20e8dbe115 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 c6e18fb338..5f1d5d8338 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 8966e0787a..844d2d5c08 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 22e0d0d8b8..e14839b98f 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 6f940f83de..4c5583adf0 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -176,6 +176,7 @@ private: 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 e979c150bf..3de3928023 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; }