[nav] Set Alt and Pos here

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 */