diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index c43c6cca7e..bb7cafbee2 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -159,7 +159,8 @@ AP_Terrain::grid_cache &AP_Terrain::find_grid(const struct grid_info &info) // see if we have that grid for (uint16_t i=0; imillis(); return cache[i]; } @@ -597,6 +598,18 @@ void AP_Terrain::update(void) } +/* + get CRC for a block + */ +uint16_t AP_Terrain::get_block_crc(struct grid_block &block) +{ + uint16_t saved_crc = block.crc; + block.crc = 0; + uint16_t ret = crc16_ccitt((const uint8_t *)&block, sizeof(block), 0); + block.crc = saved_crc; + return ret; +} + /* open the current degree file */ @@ -682,6 +695,9 @@ void AP_Terrain::write_block(void) if (io_failure) { return; } + + disk_block.block.crc = get_block_crc(disk_block.block); + ssize_t ret = ::write(fd, &disk_block, sizeof(disk_block)); if (ret != sizeof(disk_block)) { #if TERRAIN_DEBUG @@ -720,7 +736,9 @@ void AP_Terrain::read_block(void) disk_block.block.lat != lat || disk_block.block.lon != lon || disk_block.block.bitmap == 0 || - disk_block.block.version != TERRAIN_GRID_FORMAT_VERSION) { + 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", (long)lat, diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index 54296fcfa9..d998f0fc4c 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -222,6 +222,7 @@ private: disk IO functions */ int16_t find_io_idx(void); + uint16_t get_block_crc(struct grid_block &block); void check_disk_read(void); void check_disk_write(void); void io_timer(void);