Skip to content

Commit

Permalink
Fixing and simplifying mavlink odometry handling (#12793)
Browse files Browse the repository at this point in the history 
* Fixing and simplify mavlink odometry
  • Loading branch information
@kamilritz @LorenzMeier
kamilritz authored and LorenzMeier committed Aug 30, 2019
1 parent 43fdcd7 commit 9ed2dae
Show file tree
Hide file tree
Showing 3 changed files with 22 additions and 95 deletions.
2 changes: 1 addition & 1 deletion msg/vehicle_attitude.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

uint64 timestamp # time since system start (microseconds)

float32[4] q # Quaternion rotation from NED earth frame to XYZ body frame
float32[4] q # Quaternion rotation from XYZ body frame to NED earth frame.
float32[4] delta_q_reset # Amount by which quaternion has changed during last reset
uint8 quat_reset_counter # Quaternion reset counter

Expand Down
8 changes: 4 additions & 4 deletions src/modules/mavlink/mavlink_messages.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2451,7 +2451,7 @@ class MavlinkStreamOdometry : public MavlinkStream

if (odom_updated) {
msg.time_usec = odom.timestamp;
msg.child_frame_id = MAV_FRAME_BODY_NED;
msg.child_frame_id = MAV_FRAME_BODY_FRD;

// Current position
msg.x = odom.x;
Expand All @@ -2464,11 +2464,11 @@ class MavlinkStreamOdometry : public MavlinkStream
msg.q[2] = odom.q[2];
msg.q[3] = odom.q[3];

// Local NED to body-NED Dcm matrix
matrix::Dcmf Rlb(matrix::Quatf(odom.q));
// Body-FRD frame to local NED frame Dcm matrix
matrix::Dcmf R_body_to_local(matrix::Quatf(odom.q));

// Rotate linear and angular velocity from local NED to body-NED frame
matrix::Vector3f linvel_body(Rlb * matrix::Vector3f(odom.vx, odom.vy, odom.vz));
matrix::Vector3f linvel_body(R_body_to_local.transpose() * matrix::Vector3f(odom.vx, odom.vy, odom.vz));

// Current linear velocity
msg.vx = linvel_body(0);
Expand Down
107 changes: 17 additions & 90 deletions src/modules/mavlink/mavlink_receiver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1029,15 +1029,16 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)

odometry.timestamp = _mavlink_timesync.sync_stamp(odom.time_usec);

/* The position is in the local NED frame */
/* The position is in a local FRD 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 */
const matrix::Quatf q(odom.q);
q.copyTo(odometry.q);
/* 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 a MAV_FRAME_*_OTHER to the Mavlink MAV_FRAME enum IOT define
Expand All @@ -1060,41 +1061,20 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
odometry.pose_covariance[i] = odom.pose_covariance[i];
}

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
* Note that the msg quaternion represents the rotation of the msg frame to the msg child frame
* but rotates msg child frame *data* into the msg frame */
const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(odometry.q));

/* the linear velocities needs to be transformed to the local NED frame */
matrix::Vector3<float> linvel_local(Rbl * matrix::Vector3<float>(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;

//TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odometry.velocity_covariance[i] = odom.velocity_covariance[i];
}

} else if (odom.child_frame_id == MAV_FRAME_BODY_FLU) { /* WRT to estimated vehicle body-fixed frame */
/* Get quaternion from the msg quaternion itself and build DCM matrix from it
* Note that the msg quaternion represents the rotation of the msg frame to the msg child frame
* but rotates msg child frame *data* into the msg frame */
const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(odometry.q));

/* the position needs to be transformed to the local NED frame */
matrix::Vector3f pos(Rbl * matrix::Vector3<float>(odom.x, -odom.y, -odom.z));
odometry.x = pos(0);
odometry.y = pos(1);
odometry.z = pos(2);
/*
* 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
* body (fcu_frd) frame.
*/
if (odom.child_frame_id == MAV_FRAME_BODY_FRD) {
/* Linear velocity has to be rotated to the local NED frame
* Angular velocities are already in the right body frame */
const matrix::Dcmf R_body_to_local = matrix::Dcmf(q_body_to_local);

/* the linear velocities needs to be transformed to the local NED frame */
matrix::Vector3f linvel_local(Rbl * matrix::Vector3<float>(odom.vx, -odom.vy, -odom.vz));
matrix::Vector3<float> linvel_local(R_body_to_local * matrix::Vector3<float>(odom.vx, odom.vy, odom.vz));
odometry.vx = linvel_local(0);
odometry.vy = linvel_local(1);
odometry.vz = linvel_local(2);
Expand All @@ -1108,59 +1088,6 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
odometry.velocity_covariance[i] = odom.velocity_covariance[i];
}

} else if (odom.child_frame_id == MAV_FRAME_BODY_NED) { /* WRT to vehicle body-NED frame */
vehicle_attitude_s att{};

if (_vehicle_attitude_sub.copy(&att)) {

/* Get quaternion from vehicle_attitude quaternion and build DCM matrix from it
* Note that the msg quaternion represents the rotation of the msg frame to the msg child frame
* but rotates msg child frame *data* into the msg frame */
const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(att.q));

/* the linear velocities needs to be transformed to the local NED frame */
matrix::Vector3f linvel_local(Rbl * matrix::Vector3f(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;

//TODO: Apply rotation matrix to transform from body-fixed to earth-fixed NED frame
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odometry.velocity_covariance[i] = odom.velocity_covariance[i];
}

}

} else if (odom.child_frame_id == MAV_FRAME_VISION_NED || /* WRT to vehicle local NED frame */
odom.child_frame_id == MAV_FRAME_MOCAP_NED) {

vehicle_attitude_s att{};

if (_vehicle_attitude_sub.copy(&att)) {

/* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */
matrix::Dcmf Rlb = matrix::Quatf(att.q);

odometry.vx = odom.vx;
odometry.vy = odom.vy;
odometry.vz = odom.vz;

/* the angular rates need to be transformed to the body frame */
matrix::Vector3f angvel_local(Rlb * matrix::Vector3f(odom.rollspeed, odom.pitchspeed, odom.yawspeed));
odometry.rollspeed = angvel_local(0);
odometry.pitchspeed = angvel_local(1);
odometry.yawspeed = angvel_local(2);

//TODO: Apply rotation matrix to transform from earth-fixed to body-fixed NED frame
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odometry.velocity_covariance[i] = odom.velocity_covariance[i];
}
}

} else {
PX4_ERR("Body frame %u not supported. Unable to publish velocity", odom.child_frame_id);
}
Expand Down

0 comments on commit 9ed2dae

Please sign in to comment.