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