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

Fixing and simplifying mavlink odometry handling #12793

Merged
merged 2 commits into from
Aug 30, 2019
Merged

Fixing and simplifying mavlink odometry handling #12793

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7c2bf13
Fixing and simplifying mavlink odometry handling
kamilritz Aug 23, 2019
7d9756e
Fix wrong quaternion label
kamilritz Aug 23, 2019
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 msg/vehicle_attitude.msg
View file
Open in desktop
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.
Copy link
Member

@MaEtUgR MaEtUgR Aug 31, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@kamilritz Good catch, we're consistently using hamilton quaternion convention since PX4/PX4-Matrix#34 which has the 4 properties:

  • the first element is real [w, x, y, z]
  • right-handedness i * j = k
  • passive operation, vectors stay we just change frame
  • q rotates from local to global q * [0; v_local] * q* = v_global

I always overlooked the comment that was introduced at the same time.

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
Open in desktop
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
Open in desktop
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);
TSC21 marked this conversation as resolved.
Show resolved Hide resolved
}
Expand Down