From ea5f1af2b19ce051889bb002a0e66e5404382f59 Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Mon, 30 May 2022 08:55:13 +0200
Subject: [PATCH] fixed bug in way
---
.../coordination_formation_control_pkg/src/drone_node.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
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 905efd9..9257070 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) ;
--
GitLab