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 e36bd73c7aebab7df67e74506241cfa842d5af2a..e5858f16c3c0e2661616ba6ddff54ebf2538bf3f 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)
+
 
 
     }