From 76a741f768a3ddf405995c5c57c3e033e708d54d Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Mon, 30 May 2022 08:52:56 +0200
Subject: [PATCH] fixed big bug yaw in drone

---
 .../src/drone_node.cpp                             | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

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 e36bd73..e5858f1 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) ;
           float vxl =  vel_command(0)*std::cos(yaw) +  vel_command(1)*std::sin(yaw) ;
@@ -335,7 +335,7 @@ int main(int argc, char **argv) {
           row_vector_2d_t in;
           in << vxl, vyl;
           row_vector_2d_t range;
-          range << -0.15, 0.15;
+          range << -0.2, 0.2;
           row_vector_2d_t out = saturate(in, range);
           current_command.vx = out(0);
           current_command.vy = out(1);
@@ -418,17 +418,19 @@ int main(int argc, char **argv) {
       logdata->input.y = input(1,0);
       logdata->input_formation.x = input(0,1);
       logdata->input_formation.y = input(1,1);
-      logdata->input_obstacle.x = input(0,2);
-      logdata->input_obstacle.y = input(1,2);
+      logdata->input_obstacle.x = current_command.vx; //input(0,2);
+      logdata->input_obstacle.y = current_command.vy;//input(1,2);
       logdata->input_navigation.x = input(0,3);
       logdata->input_navigation.y = input(1,3);
-      logdata->input_orientation.x = input(0,4);
-      logdata->input_orientation.y = input(1,4);
+      logdata->input_orientation.x =   uav_state_p(0, robotid-1); //input(0,4);
+      logdata->input_orientation.y = uav_state_p(1, robotid-1);// input(1,4);
       logdata->input_integration.x = input(0,5);
       logdata->input_integration.y = input(1,5);
       logdata->input_to_system.x = current_command.vx;
       logdata->input_to_system.y = current_command.vy;
 
+      // uav_state_p(0, robotid-1),uav_state_p(1, robotid-1)
+
 
 
     }
-- 
GitLab