From 2e2193fcf97d944c06dd997e7ca4fb50cdd15fe5 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 22 Feb 2017 02:53:17 -0500 Subject: [PATCH] fix height --- src/buzzuav_closures.cpp | 3 ++- src/roscontroller.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 3c89b30..0044ce9 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -145,7 +145,7 @@ int buzzuav_moveto(buzzvm_t vm) { goto_pos[0]=hcpos3[0];goto_pos[1]=hcpos3[1];goto_pos[2]=height; } } - if(dx == 2.0){ + if(dx == 2.0){OB if((int)buzz_utility::get_robotid()==1){ goto_pos[0]=hcpos1[2];goto_pos[1]=hcpos1[3];goto_pos[2]=height; } @@ -163,6 +163,7 @@ int buzzuav_moveto(buzzvm_t vm) { } int buzzuav_goto(buzzvm_t vm) { + rc_goto_pos[2]=height; set_goto(rc_goto_pos); cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index a140e3e..389b0f8 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -272,7 +272,7 @@ namespace rosbzz_node{ /*prepare the goto publish message */ cmd_srv.request.param5 = goto_pos[0]; cmd_srv.request.param6 = goto_pos[1]; - cmd_srv.request.param7 = cur_pos[2]; //force constant altitude after takeoff, goto_pos[2]; + cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.command = buzzuav_closures::getcmd(); if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } else ROS_ERROR("Failed to call service from flight controller");