diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index 5b512a6d0c..c9e0fc7539 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -290,23 +290,42 @@ void nav_route(uint8_t wp_start, uint8_t wp_end) {
   horizontal_mode = HORIZONTAL_MODE_ROUTE;
 }
 
-bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx) {
+bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx, int16_t approaching_time) {
   int32_t dist_to_point;
   struct Int32Vect2 diff;
-  VECT2_DIFF(diff, waypoints[wp_idx], *stateGetPositionEnu_i());
+  struct EnuCoor_i* pos = stateGetPositionEnu_i();
+
+  VECT2_DIFF(diff, waypoints[wp_idx], *pos);
   INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC);
   INT32_VECT2_NORM(dist_to_point, diff);
   //printf("dist %d | %d %d\n", dist_to_point,diff.x,diff.y);
   //fflush(stdout);
   //if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
   if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
-  if (from_idx > 0 && from_idx < NB_WAYPOINT) {
-    struct Int32Vect2 from_diff;
-    VECT2_DIFF(from_diff, waypoints[wp_idx], waypoints[from_idx]);
-    INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC);
-    return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
+
+  if (approaching_time >= 0) {
+    struct Int32Vect2 estimated_pos;
+    struct Int32Vect2 estimated_progress;
+    struct Int32Vect2 estimated_diff;
+    int32_t estimated_dist;
+    struct EnuCoor_i* speed = stateGetSpeedEnu_i();
+    VECT2_SMUL(estimated_progress, *speed, approaching_time);
+    INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC));
+    VECT2_SUM(estimated_pos, *pos, estimated_progress);
+    VECT2_DIFF(estimated_diff, waypoints[wp_idx], estimated_pos);
+
+    INT32_VECT2_RSHIFT(estimated_diff, estimated_diff, INT32_POS_FRAC);
+    INT32_VECT2_NORM(estimated_dist, estimated_diff);
+    if (estimated_dist < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
+
+    if (from_idx > 0 && from_idx < NB_WAYPOINT) {
+      struct Int32Vect2 from_diff;
+      VECT2_DIFF(from_diff, waypoints[wp_idx], waypoints[from_idx]);
+      INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC);
+      return (estimated_diff.x * from_diff.x + estimated_diff.y * from_diff.y < 0);
+    }
   }
-  else return FALSE;
+  return FALSE;
 }
 
 bool_t nav_check_wp_time(uint8_t wp_idx, uint16_t stay_time) {
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h
index ae0b868c1b..3658ad0864 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.h
+++ b/sw/airborne/firmwares/rotorcraft/navigation.h
@@ -93,6 +93,10 @@ extern bool_t nav_set_heading_deg(float deg);
 extern bool_t nav_set_heading_towards(float x, float y);
 extern bool_t nav_set_heading_towards_waypoint(uint8_t wp);
 
+/** default approaching_time for a wp */
+#ifndef CARROT
+#define CARROT 0
+#endif
 
 #define NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); } FALSE; })
 #define NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); } FALSE; })
@@ -151,9 +155,9 @@ extern void nav_route(uint8_t wp_start, uint8_t wp_end);
 }
 
 /** Proximity tests on approaching a wp */
-bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
-#define NavApproaching(wp, time) nav_approaching_from(wp, 0)
-#define NavApproachingFrom(wp, from, time) nav_approaching_from(wp, from)
+bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx, int16_t approaching_time);
+#define NavApproaching(wp, time) nav_approaching_from(wp, 0, time)
+#define NavApproachingFrom(wp, from, time) nav_approaching_from(wp, from, time)
 
 /** Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp  */
 bool_t nav_check_wp_time(uint8_t wp_idx, uint16_t stay_time);
@@ -202,6 +206,7 @@ bool_t nav_check_wp_time(uint8_t wp_idx, uint16_t stay_time);
 
 #define nav_SetNavRadius(x) {}
 
+
 #define navigation_SetFlightAltitude(x) { \
   flight_altitude = x; \
   nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude) - ground_alt; \