fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 02:53:34 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
+6 -5
View File
@@ -46,7 +46,7 @@
AttitudeEstimator::AttitudeEstimator() : AttitudeEstimator::AttitudeEstimator() :
_n(), _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)) _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 */ /* Convert quaternion to rotation matrix */
math::Quaternion quat; math::Quaternion quat;
quat(0) = (float)msg->pose[0].orientation.w; //XXX: search for vtol or other (other than 'plane') vehicle here
quat(1) = (float)msg->pose[0].orientation.x; quat(0) = (float)msg->pose[1].orientation.w;
quat(2) = (float)msg->pose[0].orientation.y; quat(1) = (float)msg->pose[1].orientation.x;
quat(3) = (float)msg->pose[0].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[0] = quat(0);
msg_v_att.q[1] = quat(1); msg_v_att.q[1] = quat(1);