diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7f9883b019..359eda7f99 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -130,9 +130,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _att_sp_pub(nullptr), _rates_sp_pub(nullptr), _pos_sp_triplet_pub(nullptr), - _att_pos_mocap_pub(nullptr), - _vision_position_pub(nullptr), - _vision_attitude_pub(nullptr), + _mocap_odometry_pub(nullptr), + _visual_odometry_pub(nullptr), _radio_status_pub(nullptr), _ping_pub(nullptr), _rc_pub(nullptr), @@ -151,6 +150,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _command_ack_pub(nullptr), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _actuator_armed_sub(orb_subscribe(ORB_ID(actuator_armed))), + _vehicle_attitude_sub(orb_subscribe(ORB_ID(vehicle_attitude))), _global_ref_timestamp(0), _hil_frames(0), _old_timestamp(0), @@ -165,12 +165,15 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _p_bat_crit_thr(param_find("BAT_CRIT_THR")), _p_bat_low_thr(param_find("BAT_LOW_THR")) { + /* Make the attitude quaternion valid */ + _att.q[0] = 1.0f; } MavlinkReceiver::~MavlinkReceiver() { orb_unsubscribe(_control_mode_sub); orb_unsubscribe(_actuator_armed_sub); + orb_unsubscribe(_vehicle_attitude_sub); } void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result) @@ -239,6 +242,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vision_position_estimate(msg); break; + case MAVLINK_MSG_ID_ODOMETRY: + handle_message_odometry(msg); + break; + case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: handle_message_gps_global_origin(msg); break; @@ -817,29 +824,32 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) mavlink_att_pos_mocap_t mocap; mavlink_msg_att_pos_mocap_decode(msg, &mocap); - struct att_pos_mocap_s att_pos_mocap = {}; - - // Use the component ID to identify the mocap system - att_pos_mocap.id = msg->compid; + struct vehicle_odometry_s mocap_odom = {}; - att_pos_mocap.timestamp = _mavlink_timesync.sync_stamp(mocap.time_usec); - att_pos_mocap.timestamp_received = hrt_absolute_time(); + mocap_odom.timestamp = _mavlink_timesync.sync_stamp(mocap.time_usec); + mocap_odom.x = mocap.x; + mocap_odom.y = mocap.y; + mocap_odom.z = mocap.z; + mocap_odom.q[0] = mocap.q[0]; + mocap_odom.q[1] = mocap.q[1]; + mocap_odom.q[2] = mocap.q[2]; + mocap_odom.q[3] = mocap.q[3]; - att_pos_mocap.q[0] = mocap.q[0]; - att_pos_mocap.q[1] = mocap.q[1]; - att_pos_mocap.q[2] = mocap.q[2]; - att_pos_mocap.q[3] = mocap.q[3]; + for (size_t i = 0; i < URT_SIZE; i++) { + mocap_odom.pose_covariance[i] = mocap.covariance[i]; + } - att_pos_mocap.x = mocap.x; - att_pos_mocap.y = mocap.y; - att_pos_mocap.z = mocap.z; + mocap_odom.vx = NAN; + mocap_odom.vy = NAN; + mocap_odom.vz = NAN; + mocap_odom.rollspeed = NAN; + mocap_odom.pitchspeed = NAN; + mocap_odom.yawspeed = NAN; + mocap_odom.velocity_covariance[0] = NAN; - if (_att_pos_mocap_pub == nullptr) { - _att_pos_mocap_pub = orb_advertise(ORB_ID(att_pos_mocap), &att_pos_mocap); + int instance_id = 0; - } else { - orb_publish(ORB_ID(att_pos_mocap), _att_pos_mocap_pub, &att_pos_mocap); - } + orb_publish_auto(ORB_ID(vehicle_mocap_odometry), &_mocap_odometry_pub, &mocap_odom, &instance_id, ORB_PRIO_HIGH); } void @@ -1149,35 +1159,146 @@ MavlinkReceiver::handle_message_gps_global_origin(mavlink_message_t *msg) void MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) { - mavlink_vision_position_estimate_t pos; - mavlink_msg_vision_position_estimate_decode(msg, &pos); + mavlink_vision_position_estimate_t ev; + mavlink_msg_vision_position_estimate_decode(msg, &ev); + + struct vehicle_odometry_s visual_odom = {}; + + visual_odom.timestamp = _mavlink_timesync.sync_stamp(ev.usec); + visual_odom.x = ev.x; + visual_odom.y = ev.y; + visual_odom.z = ev.z; + matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw)); + q.copyTo(visual_odom.q); + + for (size_t i = 0; i < URT_SIZE; i++) { + visual_odom.pose_covariance[i] = ev.covariance[i]; + } + + visual_odom.vx = NAN; + visual_odom.vy = NAN; + visual_odom.vz = NAN; + visual_odom.rollspeed = NAN; + visual_odom.pitchspeed = NAN; + visual_odom.yawspeed = NAN; + visual_odom.velocity_covariance[0] = NAN; + + int instance_id = 0; + orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &visual_odom, &instance_id, ORB_PRIO_HIGH); +} + +void +MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) +{ + mavlink_odometry_t odom; + mavlink_msg_odometry_decode(msg, &odom); + + struct vehicle_odometry_s odometry = {}; + + /* Dcm rotation matrix from body frame to local NED frame */ + matrix::Dcm Rbl; + + odometry.timestamp = _mavlink_timesync.sync_stamp(odom.time_usec); + /* The position is in the local NED frame */ + odometry.x = odom.x; + odometry.y = odom.y; + odometry.z = odom.z; + /* The quaternion of the ODOMETRY msg represents a rotation from NED + * earth/local frame to XYZ body frame */ + matrix::Quatf q(odom.q[0], odom.q[1], odom.q[2], odom.q[3]); + q.copyTo(odometry.q); + + // create a method to simplify covariance copy + for (size_t i = 0; i < URT_SIZE; i++) { + odometry.pose_covariance[i] = odom.pose_covariance[i]; + } + + bool updated; + orb_check(_vehicle_attitude_sub, &updated); + + if (odom.child_frame_id == MAV_FRAME_BODY_FRD) { /* WRT to estimated vehicle body-fixed frame */ + /* get quaternion from the msg quaternion itself and build DCM matrix from it */ + Rbl = matrix::Dcm(matrix::Quatf(odometry.q)).I(); - struct vehicle_local_position_s vision_position = {}; + /* the linear velocities needs to be transformed to the local NED frame */ + matrix::Vector3 linvel_local(Rbl * matrix::Vector3(odom.vx, odom.vy, odom.vz)); + odometry.vx = linvel_local(0); + odometry.vy = linvel_local(1); + odometry.vz = linvel_local(2); - vision_position.timestamp = _mavlink_timesync.sync_stamp(pos.usec); - vision_position.x = pos.x; - vision_position.y = pos.y; - vision_position.z = pos.z; + odometry.rollspeed = odom.rollspeed; + odometry.pitchspeed = odom.pitchspeed; + odometry.yawspeed = odom.yawspeed; - //copy horizontal and vertical covariances - vision_position.eph = fmaxf(pos.covariance[0], pos.covariance[6]); - vision_position.epv = pos.covariance[11]; + //TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame + for (size_t i = 0; i < URT_SIZE; i++) { + odometry.velocity_covariance[i] = odom.twist_covariance[i]; + } + + } else if (odom.child_frame_id == MAV_FRAME_BODY_NED) { /* WRT to vehicle body-NED frame */ + if (updated) { + orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att); + + /* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */ + Rbl = matrix::Dcm(matrix::Quatf(_att.q)).I(); + + /* the linear velocities needs to be transformed to the local NED frame */ + matrix::Vector3 linvel_local(Rbl * matrix::Vector3(odom.vx, odom.vy, odom.vz)); + odometry.vx = linvel_local(0); + odometry.vy = linvel_local(1); + odometry.vz = linvel_local(2); + + odometry.rollspeed = odom.rollspeed; + odometry.pitchspeed = odom.pitchspeed; + odometry.yawspeed = odom.yawspeed; - vision_position.xy_valid = true; - vision_position.z_valid = true; - vision_position.v_xy_valid = true; - vision_position.v_z_valid = true; + //TODO: Apply rotation matrix to transform from body-fixed to earth-fixed NED frame + for (size_t i = 0; i < URT_SIZE; i++) { + odometry.velocity_covariance[i] = odom.twist_covariance[i]; + } + + } - struct vehicle_attitude_s vision_attitude = {}; + } else if (odom.child_frame_id == MAV_FRAME_VISION_NED || /* WRT to vehicle local NED frame */ + odom.child_frame_id == MAV_FRAME_MOCAP_NED) { + if (updated) { + orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att); + + /* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */ + matrix::Dcm Rlb = matrix::Quatf(_att.q); - vision_attitude.timestamp = _mavlink_timesync.sync_stamp(pos.usec); + odometry.vx = odom.vx; + odometry.vy = odom.vy; + odometry.vz = odom.vz; - matrix::Quatf q(matrix::Eulerf(pos.roll, pos.pitch, pos.yaw)); - q.copyTo(vision_attitude.q); + /* the angular rates need to be transformed to the body frame */ + matrix::Vector3 angvel_local(Rlb * matrix::Vector3(odom.rollspeed, odom.pitchspeed, odom.yawspeed)); + odometry.rollspeed = angvel_local(0); + odometry.pitchspeed = angvel_local(1); + odometry.yawspeed = angvel_local(2); - int inst = 0; - orb_publish_auto(ORB_ID(vehicle_vision_position), &_vision_position_pub, &vision_position, &inst, ORB_PRIO_DEFAULT); - orb_publish_auto(ORB_ID(vehicle_vision_attitude), &_vision_attitude_pub, &vision_attitude, &inst, ORB_PRIO_DEFAULT); + //TODO: Apply rotation matrix to transform from earth-fixed to body-fixed NED frame + for (size_t i = 0; i < URT_SIZE; i++) { + odometry.velocity_covariance[i] = odom.twist_covariance[i]; + } + + } + + } else { + PX4_ERR("Body frame %u not supported. Unable to publish velocity", odom.child_frame_id); + } + + int instance_id = 0; + + if (odom.frame_id == MAV_FRAME_VISION_NED) { + orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &odometry, &instance_id, ORB_PRIO_HIGH); + + } else if (odom.frame_id == MAV_FRAME_MOCAP_NED) { + orb_publish_auto(ORB_ID(vehicle_mocap_odometry), &_mocap_odometry_pub, &odometry, &instance_id, ORB_PRIO_HIGH); + + } else { + PX4_ERR("Local frame %u not supported. Unable to publish pose and velocity", odom.frame_id); + } } void diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 5c4eca2caf..eca09d73d6 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -79,6 +79,7 @@ #include #include #include +#include #include #include @@ -157,6 +158,7 @@ private: void handle_message_play_tune(mavlink_message_t *msg); void handle_message_obstacle_distance(mavlink_message_t *msg); void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg); + void handle_message_odometry(mavlink_message_t *msg); void handle_message_named_value_float(mavlink_message_t *msg); void handle_message_debug(mavlink_message_t *msg); void handle_message_debug_vect(mavlink_message_t *msg); @@ -201,6 +203,7 @@ private: MavlinkTimesync _mavlink_timesync; mavlink_status_t _status; ///< receiver status, used for mavlink_parse_char() + struct vehicle_attitude_s _att; struct vehicle_local_position_s _hil_local_pos; struct vehicle_land_detected_s _hil_land_detector; struct vehicle_control_mode_s _control_mode; @@ -224,9 +227,8 @@ private: orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; orb_advert_t _pos_sp_triplet_pub; - orb_advert_t _att_pos_mocap_pub; - orb_advert_t _vision_position_pub; - orb_advert_t _vision_attitude_pub; + orb_advert_t _mocap_odometry_pub; + orb_advert_t _visual_odometry_pub; orb_advert_t _radio_status_pub; orb_advert_t _ping_pub; orb_advert_t _rc_pub; @@ -246,6 +248,7 @@ private: orb_advert_t _command_ack_pub; int _control_mode_sub; int _actuator_armed_sub; + int _vehicle_attitude_sub; uint64_t _global_ref_timestamp; int _hil_frames; uint64_t _old_timestamp; @@ -264,6 +267,8 @@ private: param_t _p_bat_crit_thr; param_t _p_bat_low_thr; + static const size_t URT_SIZE = 21; + MavlinkReceiver(const MavlinkReceiver &) = delete; MavlinkReceiver operator=(const MavlinkReceiver &) = delete; };