diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz
index 78e9285..7b20410 100644
--- a/buzz_scripts/include/act/states.bzz
+++ b/buzz_scripts/include/act/states.bzz
@@ -77,7 +77,7 @@ function stop() {
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
 		neighbors.broadcast("cmd", 21)
 		uav_land()
-		if(pose.position.altitude <= 0.3) {
+		if(pose.position.altitude <= 0.2) {
 			BVMSTATE = "TURNEDOFF"
 			#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
 			#barrier_ready(21)
diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz
index fe36fc3..427e61a 100644
--- a/buzz_scripts/main.bzz
+++ b/buzz_scripts/main.bzz
@@ -32,11 +32,11 @@ goal_list = {
 function init() {
   	init_stig()
 	init_swarm()
-
+	
 #	initGraph()
 
 	TARGET_ALTITUDE = takeoff_heights[id]
-
+	log("----------------------->alt",TARGET_ALTITUDE)
 	# start the swarm command listener
 	nei_cmd_listen()
 
diff --git a/include/roscontroller.h b/include/roscontroller.h
index e79c589..74f12ef 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -98,6 +98,7 @@ private:
   typedef struct POSE ros_pose;
 
   ros_pose target, home, cur_pos;
+  double* goto_pos;
 
   struct MsgData
   {
@@ -141,6 +142,7 @@ private:
   uint8_t old_val = 0;
   bool debug = false;
   bool setmode = false;
+  bool BClpose = false;
   std::string bzzfile_name;
   std::string bcfname, dbgfname;
   std::string stand_by;
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 23fc145..fa0ac58 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -39,6 +39,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
   cur_pos.longitude = 0;
   cur_pos.latitude = 0;
   cur_pos.altitude = 0;
+  goto_pos = buzzuav_closures::getgoto();
 
   // set stream rate - wait for the FC to be started
   SetStreamRate(0, 10, 1);
@@ -162,6 +163,9 @@ void roscontroller::RosControllerRun()
       prepare_msg_and_publish();
       // Call the flight controler service
       flight_controller_service_call();
+      // Broadcast local position to FCU
+      if(BClpose)
+	SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
       //  Set ROBOTS variable (swarm size)
       get_number_of_robots();
       buzz_utility::set_robot_var(no_of_robots);
@@ -737,12 +741,13 @@ script
 /-------------------------------------------------------------------------------*/
 {
   //  flight controller client call if requested from Buzz
-  double* goto_pos;
   float* gimbal;
   switch (buzzuav_closures::bzz_cmd())
   {
     case NAV_TAKEOFF:
       goto_pos = buzzuav_closures::getgoto();
+      goto_pos[0] = cur_pos.x;
+      goto_pos[1] = cur_pos.y;
       cmd_srv.request.param5 = cur_pos.latitude;
       cmd_srv.request.param6 = cur_pos.longitude;
       cmd_srv.request.param7 = goto_pos[2];
@@ -756,15 +761,19 @@ script
         ros::Duration(0.5).sleep();
         // Registering HOME POINT.
         home = cur_pos;
+	BClpose = true;
       }
       if (current_mode != "GUIDED" && setmode)
         SetMode("GUIDED", 2000);  // added for compatibility with 3DR Solo
-      if (mav_client.call(cmd_srv))
+      if(setmode)
       {
-        ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
+        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");
       }
-      else
-        ROS_ERROR("Failed to call service from flight controller");
       break;
 
     case NAV_LAND:
@@ -778,10 +787,11 @@ script
         armstate = 0;
         Arm();
       }
-      else if(cur_pos.altitude < 0.3) //disarm only when close to ground
+      else if(cur_pos.altitude < 0.4) //disarm only when close to ground
       {
         armstate = 0;
         Arm();
+	BClpose = false;
       }
       if (mav_client.call(cmd_srv))
       {
@@ -829,7 +839,6 @@ script
 
     case NAV_SPLINE_WAYPOINT:
       goto_pos = buzzuav_closures::getgoto();
-      roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
       break;
 
     case DO_MOUNT_CONTROL:
@@ -1005,6 +1014,14 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt
   cur_pos.x = msg->pose.position.x;
   cur_pos.y = msg->pose.position.y;
 
+  if(!BClpose)
+  {
+  goto_pos[0]=cur_pos.x;
+  goto_pos[1]=cur_pos.y;
+  goto_pos[2]=0.0;
+  BClpose = true;
+  }
+
   buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x);
   //  cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
   tf::Quaternion q(