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

attitude estimator: fix readin of pose

Browse Source
This commit is contained in:
Thomas Gubler 2014-12-30 15:55:39 +01:00
parent 133842e89d
commit 0ea60f56e9
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -46,7 +46,7 @@
AttitudeEstimator::AttitudeEstimator() :
_n(),
_sub_modelstates(_n.subscribe("gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)),
_sub_modelstates(_n.subscribe("/gazebo/model_states", 10, &AttitudeEstimator::ModelStatesCallback, this)),
_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 10))
{
}
@ -59,10 +59,11 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP
/* Convert quaternion to rotation matrix */
math::Quaternion quat;
quat(0) = (float)msg->pose[0].orientation.w;
quat(1) = (float)msg->pose[0].orientation.x;
quat(2) = (float)msg->pose[0].orientation.y;
quat(3) = (float)msg->pose[0].orientation.z;
//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;
msg_v_att.q[0] = quat(0);
msg_v_att.q[1] = quat(1);