AP_Terrain: fixed bug in disk offset calculation

this fixes a problem where two different locations could both be
mapped to the same disk block in the terrain/*.DAT files. That meant
that pre-filled terrain on the microSD card would sometimes require a
download in flight. It also means that a RTL with loss of GCS could
sometimes fly through a region with no terrain data available

Other changes in this patch:

 - allow for a 2cm discrepancy in the lat/lon of the grid
   corners. This is needed to allow for slightly different floating
   point rounding in tools that pre-generate terrain data to load on
   the microSD

 - added TERRAIN_OPTIONS parameter to allow the user to disable
   attempts to download new terrain data. This is mostly useful for
   testing to validate a terrain generator

libraries/AP_Terrain/AP_Terrain.cpp

@@ -47,6 +47,13 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("SPACING",   1, AP_Terrain, grid_spacing, 100),
 
+    // @Param: OPTIONS
+    // @DisplayName: Terrain options
+    // @Description: Options to change behaviour of terrain system
+    // @Bitmask: 0:Disable Download
+    // @User: Advanced
+    AP_GROUPINFO("OPTIONS",   2, AP_Terrain, options, 0),
+    
     AP_GROUPEND
 };
 

libraries/AP_Terrain/AP_Terrain.h

@@ -57,7 +57,13 @@
 // format of grid on disk
 #define TERRAIN_GRID_FORMAT_VERSION 1
 
+// we allow for a 2cm discrepancy in the grid corners. This is to
+// account for different rounding in terrain DAT file generators using
+// different programming languages
+#define TERRAIN_LATLON_EQUAL(v1, v2) (labs((v1)-(v2)) <= 2)
+
 #if TERRAIN_DEBUG
+#include <assert.h>
 #define ASSERT_RANGE(v,minv,maxv) assert((v)<=(maxv)&&(v)>=(minv))
 #else
 #define ASSERT_RANGE(v,minv,maxv)
@@ -325,6 +331,7 @@ private:
     void io_timer(void);
     void open_file(void);
     void seek_offset(void);
+    uint32_t east_blocks(struct grid_block &block) const;
     void write_block(void);
     void read_block(void);
 
@@ -342,6 +349,11 @@ private:
     // parameters
     AP_Int8  enable;
     AP_Int16 grid_spacing; // meters between grid points
+    AP_Int16 options; // option bits
+
+    enum class Options {
+        DisableDownload = (1U<<0),
+    };
 
     // reference to AP_Mission, so we can ask preload terrain data for 
     // all waypoints

libraries/AP_Terrain/TerrainGCS.cpp

@@ -39,6 +39,10 @@ bool AP_Terrain::request_missing(mavlink_channel_t chan, struct grid_cache &gcac
 {
     struct grid_block &grid = gcache.grid;
 
+    if (options.get() & uint16_t(Options::DisableDownload)) {
+        return false;
+    }
+
     if (grid.spacing != grid_spacing) {
         // an invalid grid
         return false;
@@ -263,8 +267,8 @@ void AP_Terrain::handle_terrain_data(const mavlink_message_t &msg)
 
     uint16_t i;
     for (i=0; i<cache_size; i++) {
-        if (cache[i].grid.lat == packet.lat && 
-            cache[i].grid.lon == packet.lon && 
+        if (TERRAIN_LATLON_EQUAL(cache[i].grid.lat,packet.lat) &&
+            TERRAIN_LATLON_EQUAL(cache[i].grid.lon,packet.lon) &&
             cache[i].grid.spacing == packet.grid_spacing &&
             grid_spacing == packet.grid_spacing &&
             packet.gridbit < 56) {

libraries/AP_Terrain/TerrainIO.cpp

@@ -206,12 +206,10 @@ void AP_Terrain::open_file(void)
 }
 
 /*
-  seek to the right offset for disk_block
+  work out how many blocks needed in a stride for a given location
  */
-void AP_Terrain::seek_offset(void)
+uint32_t AP_Terrain::east_blocks(struct grid_block &block) const
 {
-    struct grid_block &block = disk_block.block;
-    // work out how many longitude blocks there are at this latitude
     Location loc1, loc2;
     loc1.lat = block.lat_degrees*10*1000*1000L;
     loc1.lng = block.lon_degrees*10*1000*1000L;
@@ -221,10 +219,18 @@ void AP_Terrain::seek_offset(void)
     // shift another two blocks east to ensure room is available
     loc2.offset(0, 2*grid_spacing*TERRAIN_GRID_BLOCK_SIZE_Y);
     const Vector2f offset = loc1.get_distance_NE(loc2);
-    uint16_t east_blocks = offset.y / (grid_spacing*TERRAIN_GRID_BLOCK_SIZE_Y);
+    return offset.y / (grid_spacing*TERRAIN_GRID_BLOCK_SPACING_Y);
+}
 
-    uint32_t file_offset = (east_blocks * block.grid_idx_x + 
-                            block.grid_idx_y) * sizeof(union grid_io_block);
+/*
+  seek to the right offset for disk_block
+ */
+void AP_Terrain::seek_offset(void)
+{
+    struct grid_block &block = disk_block.block;
+    // work out how many longitude blocks there are at this latitude
+    uint32_t blocknum = east_blocks(block) * block.grid_idx_x + block.grid_idx_y;
+    uint32_t file_offset = blocknum * sizeof(union grid_io_block);
     if (AP::FS().lseek(fd, file_offset, SEEK_SET) != (off_t)file_offset) {
 #if TERRAIN_DEBUG
         hal.console->printf("Seek %lu failed - %s\n",
@@ -283,17 +289,23 @@ void AP_Terrain::read_block(void)
 
     ssize_t ret = AP::FS().read(fd, &disk_block, sizeof(disk_block));
     if (ret != sizeof(disk_block) || 
-        disk_block.block.lat != lat || 
-        disk_block.block.lon != lon ||
+        !TERRAIN_LATLON_EQUAL(disk_block.block.lat,lat) ||
+        !TERRAIN_LATLON_EQUAL(disk_block.block.lon,lon) ||
         disk_block.block.bitmap == 0 ||
         disk_block.block.spacing != grid_spacing ||
         disk_block.block.version != TERRAIN_GRID_FORMAT_VERSION ||
         disk_block.block.crc != get_block_crc(disk_block.block)) {
 #if TERRAIN_DEBUG
-        printf("read empty block at %ld %ld ret=%d\n",
+        printf("read empty block at %ld %ld ret=%d (%ld %ld %u 0x%08lx) 0x%04x:0x%04x\n",
                (long)lat,
                (long)lon,
-               (int)ret);
+               (int)ret,
+               (long)disk_block.block.lat,
+               (long)disk_block.block.lon,
+               (unsigned)disk_block.block.spacing,
+               (unsigned long)disk_block.block.bitmap,
+               (unsigned)disk_block.block.crc,
+               (unsigned)get_block_crc(disk_block.block));
 #endif
         // a short read or bad data is not an IO failure, just a
         // missing block on disk

libraries/AP_Terrain/TerrainUtil.cpp

@@ -113,8 +113,8 @@ AP_Terrain::grid_cache &AP_Terrain::find_grid_cache(const struct grid_info &info
 
     // see if we have that grid
     for (uint16_t i=0; i<cache_size; i++) {
-        if (cache[i].grid.lat == info.grid_lat && 
-            cache[i].grid.lon == info.grid_lon &&
+        if (TERRAIN_LATLON_EQUAL(cache[i].grid.lat,info.grid_lat) &&
+            TERRAIN_LATLON_EQUAL(cache[i].grid.lon,info.grid_lon) &&
             cache[i].grid.spacing == grid_spacing) {
             cache[i].last_access_ms = AP_HAL::millis();
             return cache[i];
@@ -152,16 +152,16 @@ int16_t AP_Terrain::find_io_idx(enum GridCacheState state)
 {
     // try first with given state
     for (uint16_t i=0; i<cache_size; i++) {
-        if (disk_block.block.lat == cache[i].grid.lat &&
-            disk_block.block.lon == cache[i].grid.lon && 
+        if (TERRAIN_LATLON_EQUAL(disk_block.block.lat,cache[i].grid.lat) &&
+            TERRAIN_LATLON_EQUAL(disk_block.block.lon,cache[i].grid.lon) &&
             cache[i].state == state) {
             return i;
         }
     }    
     // then any state
     for (uint16_t i=0; i<cache_size; i++) {
-        if (disk_block.block.lat == cache[i].grid.lat &&
-            disk_block.block.lon == cache[i].grid.lon) {
+        if (TERRAIN_LATLON_EQUAL(disk_block.block.lat,cache[i].grid.lat) &&
+            TERRAIN_LATLON_EQUAL(disk_block.block.lon,cache[i].grid.lon)) {
             return i;
         }
     }