diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index 89d96ce047..2c50a825eb 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -348,6 +348,13 @@ void AP_Terrain::update(void) // check for pending rally data update_rally_data(); + // update tiles surrounding our current location: + if (pos_valid) { + have_surrounding_tiles = update_surrounding_tiles(loc); + } else { + have_surrounding_tiles = false; + } + // update capabilities and status if (allocate()) { if (!pos_valid) { @@ -362,7 +369,24 @@ void AP_Terrain::update(void) } else { system_status = TerrainStatusDisabled; } +} +bool AP_Terrain::update_surrounding_tiles(const Location &loc) +{ + // also request a larger set of up to 9 grids + bool ret = true; + for (int8_t x=-1; x<=1; x++) { + for (int8_t y=-1; y<=1; y++) { + Location loc2 = loc; + loc2.offset(x*TERRAIN_GRID_BLOCK_SIZE_X*0.7f*grid_spacing, + y*TERRAIN_GRID_BLOCK_SIZE_Y*0.7f*grid_spacing); + float height; + if (!height_amsl(loc2, height)) { + ret = false; + } + } + } + return ret; } bool AP_Terrain::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) const diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index 7ae415d103..2ef374a94a 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -342,6 +342,9 @@ private: void write_block(void); void read_block(void); + // check for missing data in squares surrounding loc: + bool update_surrounding_tiles(const Location &loc); + /* check for missing mission terrain data */ @@ -426,6 +429,10 @@ private: bool have_current_loc_height; float last_current_loc_height; + // true if we have all of the data for the squares around the + // current location: + bool have_surrounding_tiles; + // next mission command to check uint16_t next_mission_index; diff --git a/libraries/AP_Terrain/TerrainGCS.cpp b/libraries/AP_Terrain/TerrainGCS.cpp index 4eed5931ab..340b2a7a5b 100644 --- a/libraries/AP_Terrain/TerrainGCS.cpp +++ b/libraries/AP_Terrain/TerrainGCS.cpp @@ -139,21 +139,10 @@ void AP_Terrain::send_request(mavlink_channel_t chan) return; } - // also request a larger set of up to 9 grids - for (int8_t x=-1; x<=1; x++) { - for (int8_t y=-1; y<=1; y++) { - Location loc2 = loc; - loc2.offset(x*TERRAIN_GRID_BLOCK_SIZE_X*0.7f*grid_spacing, - y*TERRAIN_GRID_BLOCK_SIZE_Y*0.7f*grid_spacing); - struct grid_info info2; - calculate_grid_info(loc2, info2); - if (request_missing(chan, info2)) { - return; - } - } - } - - // check cache blocks that may have been setup by a TERRAIN_CHECK + // check cache blocks that may have been setup by a TERRAIN_CHECK, + // mission items, rally items, squares surrounding our current + // location, favourite holiday destination, scripting, height + // reference location, .... if (send_cache_request(chan)) { return; }