From 10f5d8291e07a6529b7fc03415f00489865fd380 Mon Sep 17 00:00:00 2001
From: Pascal Brisset <pascal.brisset@free.fr>
Date: Thu, 13 Sep 2007 10:50:43 +0000
Subject: [PATCH] waypoints constrained close to HOME

---
 sw/airborne/datalink.c | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/sw/airborne/datalink.c b/sw/airborne/datalink.c
index 5ab348e637..1a5fe30146 100644
--- a/sw/airborne/datalink.c
+++ b/sw/airborne/datalink.c
@@ -75,8 +75,12 @@ void dl_parse_msg(void) {
     float lat = RadOfDeg(DL_MOVE_WP_lat(dl_buffer));
     float lon = RadOfDeg(DL_MOVE_WP_lon(dl_buffer));
     latlong_utm_of(lat, lon, nav_utm_zone0);
-
     MoveWaypoint(wp_id, latlong_utm_x, latlong_utm_y, a);
+
+    /* Waypoint range is limited. Computes the UTM pos back from the relative
+       coordinates */
+    latlong_utm_x = waypoints[wp_id].x + nav_utm_east0;
+    latlong_utm_y = waypoints[wp_id].y + nav_utm_north0;
     DOWNLINK_SEND_WP_MOVED(&wp_id, &latlong_utm_x, &latlong_utm_y, &a, &nav_utm_zone0);
   } else if (msg_id == DL_BLOCK) {
     nav_goto_block(DL_BLOCK_block_id(dl_buffer));