diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz
index 9e8fd78..2ec6c4f 100644
--- a/buzz_scripts/include/act/states.bzz
+++ b/buzz_scripts/include/act/states.bzz
@@ -10,7 +10,7 @@ include "utils/conversions.bzz"
 TARGET_ALTITUDE = 15.0 # m.
 BVMSTATE = "TURNEDOFF"
 PICTURE_WAIT = 20 # steps
-GOTO_MAXVEL = 0.5 # m/steps
+GOTO_MAXVEL = 5.0 # m/steps
 GOTO_MAXDIST = 150 # m.
 GOTODIST_TOL = 2.0  # m.
 GOTOANG_TOL = 0.2 # rad.
@@ -89,6 +89,7 @@ function take_picture() {
 }
 
 function goto_gps(transf) {
+	log(" has to move to lat : ", rc_goto.latitude, " long: ", rc_goto.longitude, "Current lat : ", pose.position.latitude," lon ",pose.position.longitude)
 	m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
 	print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
  	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
@@ -96,8 +97,9 @@ function goto_gps(transf) {
 	else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
 		transf()
 	else {
-		#LimitSpeed(m_navigation, 1.0)
-		log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) )
+		log("Waypoint before scale : x ",m_navigation.x, " Y ",m_navigation.y )
+               	m_navigation = LimitSpeed(m_navigation, 1.0)
+                log("Waypoint after scale : x ",m_navigation.x, " Y ",m_navigation.y )
 		goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
 	}
 }
diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz
index 5f387e4..11d2984 100644
--- a/buzz_scripts/include/taskallocate/graphformGPS.bzz
+++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz
@@ -537,7 +537,7 @@ function DoJoining(){
 
   if(m_gotjoinedparent!=1)
     set_rc_goto()
-	else
+  else
     goto_gps(TransitionToJoined)
 	#pack the communication package
 	m_selfMessage.State=s2i(BVMSTATE)
diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz
index 50a58bc..92dc4e1 100644
--- a/buzz_scripts/include/utils/conversions.bzz
+++ b/buzz_scripts/include/utils/conversions.bzz
@@ -44,9 +44,9 @@ function vec_from_gps(lat, lon, home_ref) {
   }
   ned_x = d_lat/180*math.pi * 6371000.0;
   ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
-	#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
-	#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x));
-  return math.vec2.new(ned_x,ned_y)
+  m_Lgoal_range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
+  m_Lgoal_bearing = LimitAngle(math.atan(ned_y,ned_x));
+  return math.vec2.newp(m_Lgoal_range,m_Lgoal_bearing)
 }
 
 function gps_from_vec(vec) {
@@ -66,4 +66,4 @@ function gps_from_vec(vec) {
   #goal.longitude = d_lon + pose.position.longitude;
 
   return Lgoal
-}
\ No newline at end of file
+}