fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-15 12:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixing and simplifying mavlink odometry handling (#12793)

Browse Source 
* Fixing and simplify mavlink odometry
This commit is contained in:
kritz
2019-08-30 06:33:13 -07:00
committed by Lorenz Meier
parent 43fdcd7876
commit 9ed2daef48
3 changed files with 23 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+4 -4
View File
@@ -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:
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);