diff --git a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
index 905efd9f89dfeab31009ee911c83496cae765d36..9257070f24a73379ad51df3cb8a115af5f8464a5 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -324,7 +324,7 @@ int main(int argc, char **argv) {
           /* get latest yaw*/
           robot_state_t current_state_;
           current_state_ = uavs[robotid-1]->get_state();
-          float yaw = current_state_.p(2)*M_PI/180.0;  // TODO confirm that indeed it is in degrees
+          float yaw = current_state_.q(3)*M_PI/180.0;  // TODO confirm that indeed it is in degrees
           float vxl =  vel_command_(0)*std::cos(yaw) +  vel_command_(1)*std::sin(yaw) ;
           float vyl = - vel_command_(0)*std::sin(yaw)  +  vel_command_(1)*std::cos(yaw) ;