diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index 87cc4ef0954..0a4807e6202 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -350,7 +350,10 @@ bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time) {
     time_at_wp = 0;
     wp_reached = FALSE;
   }
-  if (time_at_wp > stay_time) return TRUE;
+  if (time_at_wp > stay_time) {
+    INT_VECT3_ZERO(wp_last);
+    return TRUE;
+  }
   return FALSE;
 }