yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix rotation of odometry velocities from body frame to local frame in the mavlink receiver odometry handler

Browse Source 
Summary:
PX4 assumed that odometry orientation was a way to rotate data from parent frame to child frame, so to rotate child to parent, it was taking the inverse rotation of the odometry orientation,
but orientation rotates the parent frame to the child frame, whereas for data, it represents a rotation from child to parent; hence, the rotation inverse operations were removed.

Test Plan: Checked that vehicle_mocap_odometry and vehicle_local_position gave correct linear velocities. Also visualized px4 output twist in rviz.
This commit is contained in:
Nick Steele
2019-07-10 14:33:00 -04:00
committed by Nuno Marques
parent 77694183b2
commit b203bc15df
1 changed files with 12 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/mavlink/mavlink_receiver.cpp
View File

@@ -1160,8 +1160,10 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
}
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 */
const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(odometry.q)).I();
/* 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));
@@ -1179,8 +1181,10 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
}
} 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 */
const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(odometry.q)).I();
/* 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));
@@ -1206,8 +1210,10 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
} else if (odom.child_frame_id == MAV_FRAME_BODY_NED) { /* WRT to vehicle body-NED frame */
if (_vehicle_attitude_sub.copy(&_att)) {
/* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */
const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(_att.q)).I();
/* 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::Vector3<float> linvel_local(Rbl * matrix::Vector3<float>(odom.vx, odom.vy, odom.vz));