From eeb04ba1b8f8c127ab5a29ae7a293f1194d20f38 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 25 Aug 2014 14:55:15 +1000
Subject: [PATCH] AP_Terrain: fetch more terrain data around waypoints

this ensures we have data for a wide region (1km) around each waypoint
---
 libraries/AP_Terrain/AP_Terrain.h       |  3 +++
 libraries/AP_Terrain/TerrainMission.cpp | 23 ++++++++++++++++++-----
 2 files changed, 21 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h
index adc63f0a73..df81003c53 100644
--- a/libraries/AP_Terrain/AP_Terrain.h
+++ b/libraries/AP_Terrain/AP_Terrain.h
@@ -385,6 +385,9 @@ private:
     // next mission command to check
     uint16_t next_mission_index;
 
+    // next mission position to check
+    uint8_t next_mission_pos;
+
     // last time the mission changed
     uint32_t last_mission_change_ms;
 
diff --git a/libraries/AP_Terrain/TerrainMission.cpp b/libraries/AP_Terrain/TerrainMission.cpp
index 45e851de3b..fbb7350c30 100644
--- a/libraries/AP_Terrain/TerrainMission.cpp
+++ b/libraries/AP_Terrain/TerrainMission.cpp
@@ -37,6 +37,7 @@ void AP_Terrain::update_mission_data(void)
         last_mission_spacing != grid_spacing) {
         // the mission has changed - start again
         next_mission_index = 1;
+        next_mission_pos = 0;
         last_mission_change_ms = mission.last_change_time_ms();
         last_mission_spacing = grid_spacing;
     }
@@ -47,7 +48,7 @@ void AP_Terrain::update_mission_data(void)
 
     uint16_t pending, loaded;
     get_statistics(pending, loaded);
-    if (pending) {
+    if (pending && ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
         // wait till we have fully filled the current set of grids
         return;
     }
@@ -72,10 +73,18 @@ void AP_Terrain::update_mission_data(void)
             if (!mission.read_cmd_from_storage(next_mission_index, cmd)) {
                 // nothing more to do
                 next_mission_index = 0;
+                next_mission_pos = 0;
                 return;
             }
         }
 
+        // we will fetch 5 points around the waypoint. Four at 10 grid
+        // spacings away at 45, 135, 225 and 315 degrees, and the
+        // point itself
+        if (next_mission_pos != 4) {
+            location_update(cmd.content.location, 45+90*next_mission_pos, grid_spacing.get() * 10);
+        }
+
         // we have a mission command to check
         float height;
         if (!height_amsl(cmd.content.location, height)) {
@@ -84,12 +93,16 @@ void AP_Terrain::update_mission_data(void)
             return;
         }
 
+        next_mission_pos++;
+        if (next_mission_pos == 5) {
 #if TERRAIN_DEBUG
-        hal.console->printf("checked waypoint %u\n", (unsigned)next_mission_index);
+            hal.console->printf("checked waypoint %u\n", (unsigned)next_mission_index);
 #endif
 
-        // move to next waypoint
-        next_mission_index++;
+            // move to next waypoint
+            next_mission_index++;
+            next_mission_pos = 0;
+        }
     }
 }
 
@@ -112,7 +125,7 @@ void AP_Terrain::update_rally_data(void)
 
     uint16_t pending, loaded;
     get_statistics(pending, loaded);
-    if (pending) {
+    if (pending && ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
         // wait till we have fully filled the current set of grids
         return;
     }