diff --git a/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c
index 3d0b47623bb..547626af8cf 100644
--- a/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c
+++ b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.c
@@ -392,7 +392,8 @@ bool rover_guidance_bearing_static_ctrl(void)
   guidance_control.cmd.speed = 0.0;
   guidance_control.cmd.delta  = 0.0;
   //guidance_control.throttle = BoundThrottle(0.0);
-  
+  /* Obtain current cpu time when in static control */
+  gvf_c_t0 = get_sys_time_msec();
   return true;
 }
 
diff --git a/sw/airborne/modules/guidance/gvf_common.c b/sw/airborne/modules/guidance/gvf_common.c
index a6a5df80578..258219a151b 100644
--- a/sw/airborne/modules/guidance/gvf_common.c
+++ b/sw/airborne/modules/guidance/gvf_common.c
@@ -23,3 +23,4 @@
 gvf_common_omega  gvf_c_omega;
 gvf_common_params gvf_c_info;
 gvf_common_stop_at_wp gvf_c_stopwp;
+uint32_t gvf_c_t0;
diff --git a/sw/airborne/modules/guidance/gvf_common.h b/sw/airborne/modules/guidance/gvf_common.h
index cac98d3799a..00141e830f3 100644
--- a/sw/airborne/modules/guidance/gvf_common.h
+++ b/sw/airborne/modules/guidance/gvf_common.h
@@ -39,6 +39,7 @@ extern gvf_common_omega gvf_c_omega;
 * @brief Different parameters obtained from gvfs. dot means d/dt
 * @param kappa is the curve's curvature
 * @param ori_err is the orientation error
+* #param ori_err_dot is the derivative of the orientation error
 */
 typedef struct{	
 	float kappa;
@@ -59,6 +60,8 @@ typedef struct{
  
 extern gvf_common_params gvf_c_info;
 extern gvf_common_stop_at_wp gvf_c_stopwp;
+/* @ brief Variable to keep the current cpu time in gvf calls */
+extern uint32_t gvf_c_t0;
  
  #endif // GVF_COMMON_H
 
diff --git a/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c b/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c
index cc3fdab2c42..32d73279531 100644
--- a/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c
+++ b/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.c
@@ -34,7 +34,6 @@
 
 
 // Control
-uint32_t gvf_parametric_bare_t0 = 0; // We need it for calculting the time lapse delta_T
 uint32_t gvf_parametric_bare_splines_ctr = 0; // We need it for Bézier curves splines Telemetry
 gvf_parametric_bare_con gvf_parametric_bare_control;
 
@@ -58,7 +57,7 @@ static void send_gvf_parametric_bare(struct transport_tx *trans, struct link_dev
   uint8_t traj_type = (uint8_t)gvf_parametric_bare_trajectory.type;
 
   uint32_t now = get_sys_time_msec();
-  uint32_t delta_T = now - gvf_parametric_bare_t0;
+  uint32_t delta_T = now - gvf_c_t0;
 
   float wb = gvf_parametric_bare_control.w * gvf_parametric_bare_control.beta;
 
@@ -73,7 +72,7 @@ static void send_gvf_parametric_bare(struct transport_tx *trans, struct link_dev
 static void send_circle_parametric(struct transport_tx *trans, struct link_device *dev)
 {
   uint32_t now = get_sys_time_msec();
-  uint32_t delta_T = now - gvf_parametric_bare_t0;
+  uint32_t delta_T = now - gvf_c_t0;
 
 	/*
   if (delta_T < 200)
@@ -115,11 +114,14 @@ void gvf_parametric_bare_control_2D(float kx, float ky, float f1, float f2, floa
 {
 
   uint32_t now = get_sys_time_msec();
-  gvf_parametric_bare_control.delta_T = now - gvf_parametric_bare_t0;
-  gvf_parametric_bare_t0 = now;
-
-  if (gvf_parametric_bare_control.delta_T > 300) { // We need at least two iterations for Delta_T
-    gvf_parametric_bare_control.w = 0; // Reset w since we assume the algorithm starts
+  gvf_parametric_bare_control.delta_T = now - gvf_c_t0;
+  gvf_c_t0 = now;
+
+  // We need at least two iterations for Delta_T
+  if (gvf_parametric_bare_control.delta_T > 300)
+  {
+    /* Reset w since we assume the algorithm starts */
+    gvf_parametric_bare_control.w = 0;
     return;
   }
 
diff --git a/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.h b/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.h
index 60bbb1166fc..395d6b5a6b1 100644
--- a/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.h
+++ b/sw/airborne/modules/guidance/gvf_parametric_bare/gvf_parametric_bare.h
@@ -69,6 +69,8 @@
 * @param s Defines the direction to be tracked. It takes the values -1 or 1.
 * @param k_roll Gain for tuning the coordinated turn.
 * @param k_climb Gain for tuning the climbing setting point.
+* @param k_psi Gain for tuning the heading setting point
+* @param beta Variable to scale down w
 */
 typedef struct {
   float w;