diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz
index 2ae665a..254d84d 100644
--- a/buzz_scripts/include/uavstates.bzz
+++ b/buzz_scripts/include/uavstates.bzz
@@ -35,7 +35,7 @@ function takeoff() {
 		barrier_set(ROBOTS, action, land, -1)
 		barrier_ready()
 	} else {
-    		log("Altitude: ", TARGET_ALTITUDE)
+    		log("Altitude: ", position.altitude)
 		neighbors.broadcast("cmd", 22)
 		uav_takeoff(TARGET_ALTITUDE)
   	}
@@ -60,15 +60,15 @@ function set_goto(transf) {
     		gotoWP(transf);
   	}
 
-	#if(rc_goto.id==id){
-	#	if(s!=nil){
-      	#		if(s.in())
-        #			s.leave()      
-    	#}
-	#} else {
-	#	neighbors.broadcast("cmd", 16)
-	#	neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
-	#}
+	if(rc_goto.id==id){
+		if(s!=nil){
+      			if(s.in())
+        			s.leave()      
+    	}
+	} else {
+		neighbors.broadcast("cmd", 16)
+		neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
+	}
 }
 
 ptime=0
@@ -86,14 +86,14 @@ function picture() {
 }
 
 function gotoWP(transf) {
-	 print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
+	# print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
 	rb_from_gps(rc_goto.latitude, rc_goto.longitude)
-	print(" has to move ", goal.range)
+	print(" has to move ", goal.range, goal.bearing)
 	m_navigation = math.vec2.newp(goal.range, goal.bearing)
 
 	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
-    log("Sorry this is too far.")
-  else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
+		log("Sorry this is too far.")
+  	else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
 		m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
 		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
 	} else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination
diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz
index 892b5a6..97b92bf 100644
--- a/buzz_scripts/testaloneWP.bzz
+++ b/buzz_scripts/testaloneWP.bzz
@@ -23,7 +23,7 @@ function step() {
 
 	statef()
 
-#	log("Current state: ", UAVSTATE)
+	log("Current state: ", UAVSTATE)
 }
  
 # Executed once when the robot (or the simulator) is reset.
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index ed9ef9b..23676d2 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -321,7 +321,7 @@ namespace buzzuav_closures{
 	/ Buzz closure to take a picture here.
 	/----------------------------------------*/
 	int buzzuav_takepicture(buzzvm_t vm) {
-	  cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
+	  //cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
 	  buzz_cmd = COMMAND_PICTURE;
 	  return buzzvm_ret0(vm);
 	}
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 3383569..1041ec7 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -761,17 +761,20 @@ void roscontroller::flight_controller_service_call()
     roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
   } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/
     ROS_INFO("TAKING A PICTURE HERE!! --------------");
-    cmd_srv.request.param1 = 90;
-    cmd_srv.request.param2 = 0;
-    cmd_srv.request.param3 = 0;
-    cmd_srv.request.param4 = 0;
+    cmd_srv.request.param1 = 0.0;
+    cmd_srv.request.param2 = 0.0;
+    cmd_srv.request.param3 = -90.0;
+    cmd_srv.request.param4 = 0.0;
     cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
     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");
     mavros_msgs::CommandBool capture_command;
-    capture_srv.call(capture_command);
+    if (capture_srv.call(capture_command)) {
+      ROS_INFO("Reply: %ld", (long int)capture_command.response.success);
+    } else
+      ROS_ERROR("Failed to call service from camera streamer");
   }
 }
 /*----------------------------------------------
@@ -993,8 +996,7 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) {
   //		gps_ned_home(ned_x, ned_y);
   //         ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0],
   //         home[1]);
-  //         ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id,
-  //         local_pos[0], local_pos[1]);
+//  ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos_new[0], local_pos_new[1]);
 
   /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD
    * (then converted to NED)*/