fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:38:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink_receiver: use typedef dcmf for DCM matrices

Browse Source
This commit is contained in:
TSC21
2018-09-18 09:52:33 +02:00
committed by Lorenz Meier
parent 1d10c5f740
commit 1cfaccfd7b
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+4 -4
View File
@@ -1204,7 +1204,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
struct vehicle_odometry_s odometry = {}; struct vehicle_odometry_s odometry = {};
/* Dcm rotation matrix from body frame to local NED frame */ /* Dcm rotation matrix from body frame to local NED frame */
matrix::Dcm<float> Rbl; matrix::Dcmf Rbl;
odometry.timestamp = _mavlink_timesync.sync_stamp(odom.time_usec); odometry.timestamp = _mavlink_timesync.sync_stamp(odom.time_usec);
/* The position is in the local NED frame */ /* The position is in the local NED frame */
@@ -1233,7 +1233,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
if (odom.child_frame_id == MAV_FRAME_BODY_FRD) { /* WRT to estimated vehicle body-fixed frame */ 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 */ /* get quaternion from the msg quaternion itself and build DCM matrix from it */
Rbl = matrix::Dcm<float>(matrix::Quatf(odometry.q)).I(); Rbl = matrix::Dcmf(matrix::Quatf(odometry.q)).I();
/* the linear velocities needs to be transformed to the local NED frame */ /* 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)); matrix::Vector3<float> linvel_local(Rbl * matrix::Vector3<float>(odom.vx, odom.vy, odom.vz));
@@ -1255,7 +1255,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att); orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att);
/* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */ /* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */
Rbl = matrix::Dcm<float>(matrix::Quatf(_att.q)).I(); Rbl = matrix::Dcmf(matrix::Quatf(_att.q)).I();
/* the linear velocities needs to be transformed to the local NED frame */ /* 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)); matrix::Vector3<float> linvel_local(Rbl * matrix::Vector3<float>(odom.vx, odom.vy, odom.vz));
@@ -1280,7 +1280,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att); orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att);
/* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */ /* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */
matrix::Dcm<float> Rlb = matrix::Quatf(_att.q); matrix::Dcmf Rlb = matrix::Quatf(_att.q);
odometry.vx = odom.vx; odometry.vx = odom.vx;
odometry.vy = odom.vy; odometry.vy = odom.vy;