diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c
index 2f23e27e0a..96001f4198 100644
--- a/sw/airborne/firmwares/rotorcraft/datalink.c
+++ b/sw/airborne/firmwares/rotorcraft/datalink.c
@@ -49,7 +49,7 @@
 #include "firmwares/rotorcraft/navigation.h"
 
 #include "math/pprz_geodetic_int.h"
-#include "subsystems/ins.h"
+#include "state.h"
 
 #define IdOfMsg(x) (x[1])
 
@@ -97,19 +97,18 @@ void dl_parse_msg(void) {
     {
       uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
       if (ac_id != AC_ID) break;
-      uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
-      struct LlaCoor_i lla;
-      struct EnuCoor_i enu;
-      lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
-      lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
-      /* WP_alt is in cm, lla.alt in mm */
-      lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_impl.ltp_def.hmsl + ins_impl.ltp_def.lla.alt;
-      enu_of_lla_point_i(&enu,&ins_impl.ltp_def,&lla);
-      enu.x = POS_BFP_OF_REAL(enu.x)/100;
-      enu.y = POS_BFP_OF_REAL(enu.y)/100;
-      enu.z = POS_BFP_OF_REAL(enu.z)/100;
-      VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
-      DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
+      if (stateIsLocalCoordinateValid()) {
+        uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
+        struct LlaCoor_i lla;
+        lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
+        lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
+        /* WP_alt from message is alt above MSL in cm
+         * lla.alt is above ellipsoid in mm
+         */
+        lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - state.ned_origin_i.hmsl +
+          state.ned_origin_i.lla.alt;
+        nav_move_waypoint_lla(wp_id, &lla);
+      }
     }
     break;
 #endif /* USE_NAVIGATION */
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index 62fb47b6ab..ceef6c7ded 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -44,6 +44,10 @@
 
 #include "math/pprz_algebra_int.h"
 
+#include "subsystems/datalink/downlink.h"
+#include "messages.h"
+#include "mcu_periph/uart.h"
+
 const uint8_t nb_waypoint = NB_WAYPOINT;
 struct EnuCoor_i waypoints[NB_WAYPOINT];
 
@@ -305,13 +309,22 @@ void nav_periodic_task() {
   ground_alt = POS_BFP_OF_REAL((float)ins_impl.ltp_def.hmsl / 1000.);
 }
 
-#include "subsystems/datalink/downlink.h"
-#include "messages.h"
-#include "mcu_periph/uart.h"
+void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos) {
+  if (stateIsLocalCoordinateValid()) {
+    struct EnuCoor_i enu;
+    enu_of_lla_point_i(&enu, &state.ned_origin_i, new_lla_pos);
+    enu.x = POS_BFP_OF_REAL(enu.x)/100;
+    enu.y = POS_BFP_OF_REAL(enu.y)/100;
+    enu.z = POS_BFP_OF_REAL(enu.z)/100;
+    nav_move_waypoint(wp_id, &enu);
+  }
+}
+
 void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) {
   if (wp_id < nb_waypoint) {
     INT32_VECT3_COPY(waypoints[wp_id],(*new_pos));
-    DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), &(new_pos->y), &(new_pos->z));
+    DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x),
+                               &(new_pos->y), &(new_pos->z));
   }
 }
 
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h
index b8df4ee553..120e574de0 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.h
+++ b/sw/airborne/firmwares/rotorcraft/navigation.h
@@ -79,6 +79,7 @@ void compute_dist2_to_home(void);
 unit_t nav_reset_reference( void ) __attribute__ ((unused));
 unit_t nav_reset_alt( void ) __attribute__ ((unused));
 void nav_periodic_task(void);
+void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos);
 void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
 bool_t nav_detect_ground(void);
 bool_t nav_is_in_flight(void);
diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c
index bf0ef5562b..afb4764d7d 100644
--- a/sw/simulator/nps/nps_ivy_rotorcraft.c
+++ b/sw/simulator/nps/nps_ivy_rotorcraft.c
@@ -8,8 +8,8 @@
 #include "nps_autopilot.h"
 #include "nps_fdm.h"
 #include "nps_sensors.h"
-#include "subsystems/ins.h"
 #include "firmwares/rotorcraft/navigation.h"
+#include "state.h"
 
 #ifdef RADIO_CONTROL_TYPE_DATALINK
 #include "subsystems/radio_control.h"
@@ -31,7 +31,6 @@ void nps_ivy_init(char* ivy_bus) {
 
 }
 
-//TODO use datalink parsing from rotorcraft instead of doing it here explicitly
 
 #include "generated/settings.h"
 #include "dl_protocol.h"
@@ -43,16 +42,13 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
     uint8_t wp_id = atoi(argv[1]);
 
     struct LlaCoor_i lla;
-    struct EnuCoor_i enu;
     lla.lat = INT32_RAD_OF_DEG(atoi(argv[3]));
     lla.lon = INT32_RAD_OF_DEG(atoi(argv[4]));
-    lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
-    enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
-    enu.x = POS_BFP_OF_REAL(enu.x)/100;
-    enu.y = POS_BFP_OF_REAL(enu.y)/100;
-    enu.z = POS_BFP_OF_REAL(enu.z)/100;
-    VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
-    DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
-    printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z);
+    /* WP_alt from message is alt above MSL in cm
+     * lla.alt is above ellipsoid in mm
+     */
+    lla.alt = atoi(argv[5]) *10 - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt;
+    nav_move_waypoint_lla(wp_id, &lla);
+    printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, waypoints[wp_id].x, waypoints[wp_id].y, waypoints[wp_id].z);
   }
 }