diff --git a/sw/airborne/subsystems/navigation/common_nav.h b/sw/airborne/subsystems/navigation/common_nav.h
index 43e88e1fc3..c3657846c4 100644
--- a/sw/airborne/subsystems/navigation/common_nav.h
+++ b/sw/airborne/subsystems/navigation/common_nav.h
@@ -78,4 +78,11 @@ void common_nav_periodic_task_4Hz(void);
   FALSE; \
 })
 
+#define NavSetWaypointPosAndAltHere(_wp) ({ \
+    waypoints[_wp].x = stateGetPositionEnu_f()->x; \
+    waypoints[_wp].y = stateGetPositionEnu_f()->y; \
+    waypoints[_wp].a = stateGetPositionEnu_f()->z + ground_alt; \
+    FALSE; \
+  })
+
 #endif /* COMMON_NAV_H */