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

attitude estimator: fix signs

Browse Source
This commit is contained in:
Thomas Gubler
2014-12-31 16:52:00 +01:00
parent 2ab6eefc29
commit a5890662c5
1 changed files with 11 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
+11 -7
View File
@@ -46,8 +46,8 @@
AttitudeEstimator::AttitudeEstimator() :
_n(),
_sub_modelstates(_n.subscribe("/gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)),
_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 10))
_sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)),
_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 1))
{
}
@@ -62,23 +62,27 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
//XXX: search for vtol or other (other than 'plane') vehicle here
quat(0) = (float)msg->pose[1].orientation.w;
quat(1) = (float)msg->pose[1].orientation.x;
quat(2) = (float)msg->pose[1].orientation.y;
quat(3) = (float)msg->pose[1].orientation.z;
quat(2) = (float)-msg->pose[1].orientation.y;
quat(3) = (float)-msg->pose[1].orientation.z;
msg_v_att.q[0] = quat(0);
msg_v_att.q[1] = quat(1);
msg_v_att.q[2] = quat(2);
msg_v_att.q[3] = quat(3);
math::Matrix<3, 3> Rot = quat.to_dcm();
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);
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);
_vehicle_attitude_pub.publish(msg_v_att);
}