zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Fixing and simplifying mavlink odometry handling (#12793) 

* Fixing and simplify mavlink odometry
sbg
kritz 6 years ago committed by Lorenz Meier
parent
43fdcd7876
commit
9ed2daef48
3 changed files with 22 additions and 95 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      msg/vehicle_attitude.msg
  2. 8
      src/modules/mavlink/mavlink_messages.cpp
  3. 107
      src/modules/mavlink/mavlink_receiver.cpp

2
msg/vehicle_attitude.msg
Unescape View File

@ -2,7 +2,7 @@ @@ -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

8
src/modules/mavlink/mavlink_messages.cpp
Unescape View File

@ -2451,7 +2451,7 @@ protected: @@ -2451,7 +2451,7 @@ protected:
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;
@ -2464,11 +2464,11 @@ protected: @@ -2464,11 +2464,11 @@ protected:
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);

107
src/modules/mavlink/mavlink_receiver.cpp
Unescape View File

@ -1029,15 +1029,16 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) @@ -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
@ -1060,41 +1061,20 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) @@ -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);
@ -1108,59 +1088,6 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) @@ -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);
}

Write Preview
Loading…
Cancel
Save