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

Update ROS att estimator and remove unused rotation matrix

Browse Source
This commit is contained in:
Lorenz Meier
2016-10-18 09:51:10 +02:00
committed by Lorenz Meier
parent 3696e7722a
commit 21bc78dedc
1 changed files with 0 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
View File

@@ -104,21 +104,6 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg)
msg_v_att.q[2] = quat(2);
msg_v_att.q[3] = quat(3);
math::Matrix<3, 3> rot = quat.to_dcm();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
PX4_R(msg_v_att.R, i, j) = rot(i, j);
}
}
msg_v_att.R_valid = true;
math::Vector<3> euler = rot.to_euler();
msg_v_att.roll = euler(0);
msg_v_att.pitch = euler(1);
msg_v_att.yaw = euler(2);
msg_v_att.rollspeed = (float)msg->angular_velocity.x;
msg_v_att.pitchspeed = -(float)msg->angular_velocity.y;
msg_v_att.yawspeed = -(float)msg->angular_velocity.z;