Browse Source

Remove non-quaternion handling for ROS attitude

sbg
Lorenz Meier 8 years ago
parent
commit
b035b6a112
  1. 15
      src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp

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

@ -75,21 +75,6 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP @@ -75,21 +75,6 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
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);
//XXX this is in inertial frame
// msg_v_att.rollspeed = (float)msg->twist[index].angular.x;
// msg_v_att.pitchspeed = -(float)msg->twist[index].angular.y;

Loading…
Cancel
Save