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");