From 7da939047f8623dc13a826737cc4d279411f9fa0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 6 Aug 2014 16:17:39 +1000
Subject: [PATCH] AP_Terrain: added automatic waypoint checking

check we have terrain data for all waypoints
---
 libraries/AP_Terrain/AP_Terrain.cpp |  6 +++++-
 libraries/AP_Terrain/AP_Terrain.h   | 21 ++++++++++++++++++++-
 libraries/AP_Terrain/TerrainGCS.cpp | 14 ++++++++++++++
 3 files changed, 39 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp
index c73c62a6e9..8576cdce1c 100644
--- a/libraries/AP_Terrain/AP_Terrain.cpp
+++ b/libraries/AP_Terrain/AP_Terrain.cpp
@@ -53,8 +53,9 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] PROGMEM = {
 };
 
 // constructor
-AP_Terrain::AP_Terrain(AP_AHRS &_ahrs) :
+AP_Terrain::AP_Terrain(AP_AHRS &_ahrs, const AP_Mission &_mission) :
     ahrs(_ahrs),
+    mission(_mission),
     disk_io_state(DiskIoIdle),
     last_request_time_ms(0),
     fd(-1),
@@ -259,6 +260,9 @@ void AP_Terrain::update(void)
         last_current_loc_height = height;
         have_current_loc_height = true;
     }
+
+    // check for pending mission data
+    update_mission_data();
 }
 
 /*
diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h
index 190bbdfc45..f9803f00cb 100644
--- a/libraries/AP_Terrain/AP_Terrain.h
+++ b/libraries/AP_Terrain/AP_Terrain.h
@@ -31,6 +31,7 @@
 
 #include <AP_Param.h>
 #include <AP_AHRS.h>
+#include <AP_Mission.h>
 
 #define TERRAIN_DEBUG 0
 
@@ -78,7 +79,7 @@
 class AP_Terrain
 {
 public:
-    AP_Terrain(AP_AHRS &_ahrs);
+    AP_Terrain(AP_AHRS &_ahrs, const AP_Mission &_mission);
 
     enum TerrainStatus {
         TerrainStatusDisabled  = 0, // not enabled
@@ -302,6 +303,11 @@ private:
     void write_block(void);
     void read_block(void);
 
+    /*
+      check for mission terrain data
+     */
+    void update_mission_data(void);
+
     // parameters
     AP_Int8  enable;
     AP_Int16 grid_spacing; // meters between grid points
@@ -310,6 +316,10 @@ private:
     // heading and speed
     AP_AHRS &ahrs;
 
+    // reference to AP_Mission, so we can ask preload terrain data for 
+    // all waypoints
+    const AP_Mission &mission;
+
     // cache of grids in memory, LRU
     struct grid_cache cache[TERRAIN_GRID_BLOCK_CACHE_SIZE];
 
@@ -354,6 +364,15 @@ private:
     // temporarily unavailable
     bool have_current_loc_height;
     float last_current_loc_height;
+
+    // next mission command to check
+    uint16_t next_mission_index;
+
+    // last time the mission changed
+    uint32_t last_mission_change_ms;
+
+    // grid spacing during mission check
+    uint16_t last_mission_spacing;
 };
 #endif // AP_TERRAIN_AVAILABLE
 #endif // __AP_TERRAIN_H__
diff --git a/libraries/AP_Terrain/TerrainGCS.cpp b/libraries/AP_Terrain/TerrainGCS.cpp
index 1c99459f6e..56403894c7 100644
--- a/libraries/AP_Terrain/TerrainGCS.cpp
+++ b/libraries/AP_Terrain/TerrainGCS.cpp
@@ -38,6 +38,11 @@ bool AP_Terrain::request_missing(mavlink_channel_t chan, struct grid_cache &gcac
 {
     struct grid_block &grid = gcache.grid;
 
+    if (grid.spacing != grid_spacing) {
+        // an invalid grid
+        return false;
+    }
+
     // see if we are waiting for disk read
     if (gcache.state == GRID_CACHE_DISKWAIT) {
         // don't request data from the GCS till we know its not on disk
@@ -157,6 +162,9 @@ void AP_Terrain::get_statistics(uint16_t &pending, uint16_t &loaded)
     pending = 0;
     loaded = 0;
     for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
+        if (cache[i].grid.spacing != grid_spacing) {
+            continue;
+        }
         if (cache[i].state == GRID_CACHE_INVALID) {
             continue;
         }
@@ -165,6 +173,11 @@ void AP_Terrain::get_statistics(uint16_t &pending, uint16_t &loaded)
             pending += maskbits;
             continue;
         }
+        if (cache[i].state == GRID_CACHE_DIRTY) {
+            // count dirty grids as a pending, so we know where there 
+            // are disk writes pending
+            pending++;
+        }
         uint8_t bitcount = bitcount64(cache[i].grid.bitmap);
         pending += maskbits - bitcount;
         loaded += bitcount;
@@ -252,6 +265,7 @@ void AP_Terrain::handle_terrain_data(mavlink_message_t *msg)
         if (cache[i].grid.lat == packet.lat && 
             cache[i].grid.lon == packet.lon && 
             cache[i].grid.spacing == packet.grid_spacing &&
+            grid_spacing == packet.grid_spacing &&
             packet.gridbit < 56) {
             break;
         }