From 9f76f0276ffc21c64758bb68f90835d87ff12479 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 22 Jul 2014 11:28:54 +1000 Subject: [PATCH] AP_Terrain: added disk IO for terrain data --- libraries/AP_Terrain/AP_Terrain.cpp | 320 +++++++++++++++++++++++++++- libraries/AP_Terrain/AP_Terrain.h | 79 ++++++- 2 files changed, 385 insertions(+), 14 deletions(-) diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index e4526463bc..2ffe5a88b5 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -22,10 +22,15 @@ #include "AP_Terrain.h" #include #include +#include +#include +#include +#include +#include #if HAVE_AP_TERRAIN -#define TERRAIN_DEBUG 0 +#define TERRAIN_DEBUG 1 extern const AP_HAL::HAL& hal; @@ -50,7 +55,14 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] PROGMEM = { // constructor AP_Terrain::AP_Terrain(AP_AHRS &_ahrs) : ahrs(_ahrs), - last_request_time_ms(0) + last_request_time_ms(0), + disk_io_state(DiskIoIdle), + fd(-1), + timer_setup(false), + file_lat_degrees(0), + file_lon_degrees(0), + io_failure(false), + directory_created(false) { AP_Param::setup_object_defaults(this, var_info); } @@ -112,8 +124,8 @@ void AP_Terrain::calculate_grid_info(const Location &loc, struct grid_info &info // find indexes into 32*28 grids for this degree reference. Note // the use of TERRAIN_GRID_BLOCK_SPACING_{X,Y} which gives a one square // overlap between grids - uint16_t grid_idx_x = idx_x / TERRAIN_GRID_BLOCK_SPACING_X; - uint16_t grid_idx_y = idx_y / TERRAIN_GRID_BLOCK_SPACING_Y; + info.grid_idx_x = idx_x / TERRAIN_GRID_BLOCK_SPACING_X; + info.grid_idx_y = idx_y / TERRAIN_GRID_BLOCK_SPACING_Y; // find the indices within the 32*28 grid info.idx_x = idx_x % TERRAIN_GRID_BLOCK_SPACING_X; @@ -125,8 +137,8 @@ void AP_Terrain::calculate_grid_info(const Location &loc, struct grid_info &info // calculate lat/lon of SW corner of 32*28 grid_block location_offset(ref, - grid_idx_x * TERRAIN_GRID_BLOCK_SPACING_X * (float)grid_spacing, - grid_idx_y * TERRAIN_GRID_BLOCK_SPACING_Y * (float)grid_spacing); + info.grid_idx_x * TERRAIN_GRID_BLOCK_SPACING_X * (float)grid_spacing, + info.grid_idx_y * TERRAIN_GRID_BLOCK_SPACING_Y * (float)grid_spacing); info.grid_lat = ref.lat; info.grid_lon = ref.lng; @@ -164,8 +176,15 @@ AP_Terrain::grid_cache &AP_Terrain::find_grid(const struct grid_info &info) grid.grid.lat = info.grid_lat; grid.grid.lon = info.grid_lon; grid.grid.spacing = grid_spacing; + grid.grid.grid_idx_x = info.grid_idx_x; + grid.grid.grid_idx_y = info.grid_idx_y; + grid.grid.lat_degrees = info.lat_degrees; + grid.grid.lon_degrees = info.lon_degrees; grid.last_access_ms = hal.scheduler->millis(); + // mark as waiting for disk read + grid.state = GRID_CACHE_DISKWAIT; + return grid; } @@ -224,7 +243,14 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height) bool AP_Terrain::request_missing(mavlink_channel_t chan, const struct grid_info &info) { // find the grid - struct grid_block &grid = find_grid(info).grid; + struct grid_cache &gcache = find_grid(info); + struct grid_block &grid = gcache.grid; + + // 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 + return false; + } // see if it is fully populated if ((grid.bitmap & bitmap_mask) == bitmap_mask) { @@ -327,6 +353,10 @@ void AP_Terrain::handle_data(mavlink_message_t *msg) } } gcache.grid.bitmap |= ((uint64_t)1) << packet.gridbit; + + // mark dirty for disk IO + gcache.state = GRID_CACHE_DIRTY; + #if TERRAIN_DEBUG hal.console->printf("Filled bit %u idx_x=%u idx_y=%u\n", (unsigned)packet.gridbit, (unsigned)idx_x, (unsigned)idx_y); @@ -347,12 +377,107 @@ void AP_Terrain::handle_data(mavlink_message_t *msg) #endif } +/* + find cache index of disk_block + */ +int16_t AP_Terrain::find_io_idx(void) +{ + for (uint16_t i=0; iregister_io_process(AP_HAL_MEMBERPROC(&AP_Terrain::io_timer)); + } + + switch (disk_io_state) { + case DiskIoIdle: + // look for a block that needs reading or writing + check_disk_read(); + if (disk_io_state == DiskIoIdle) { + // still idle, check for writes + check_disk_write(); + } + break; + + case DiskIoDoneRead: { + // a read has completed + int16_t cache_idx = find_io_idx(); + if (cache_idx != -1) { + if (disk_block.block.bitmap != 0) { + // when bitmap is zero we read an empty block + cache[cache_idx].grid = disk_block.block; + } + cache[cache_idx].state = GRID_CACHE_VALID; + cache[cache_idx].last_access_ms = hal.scheduler->millis(); + } + disk_io_state = DiskIoIdle; + break; + } + + case DiskIoDoneWrite: { + // a write has completed + int16_t cache_idx = find_io_idx(); + if (cache_idx != -1) { + if (cache[cache_idx].grid.bitmap == disk_block.block.bitmap) { + // only mark valid if more grids haven't been added + cache[cache_idx].state = GRID_CACHE_VALID; + } + } + disk_io_state = DiskIoIdle; + break; + } + + case DiskIoWaitWrite: + case DiskIoWaitRead: + // waiting for io_timer() + break; + } + Location loc; if (!ahrs.get_position(loc)) { // we don't know where we are @@ -366,4 +491,185 @@ void AP_Terrain::update(void) #endif } + +/* + open the current degree file + */ +void AP_Terrain::open_file(void) +{ + struct grid_block &block = disk_block.block; + if (fd != -1 && + block.lat_degrees == file_lat_degrees && + block.lon_degrees == file_lon_degrees) { + // already open on right file + return; + } + + // build the pathname to the degree file + char path[] = HAL_BOARD_TERRAIN_DIRECTORY "/NxxExxx.DAT"; + char *p = &path[strlen(HAL_BOARD_TERRAIN_DIRECTORY)+1]; + snprintf(p, 12, "%c%02u%c%03u.DAT", + block.lat_degrees<0?'S':'N', + abs(block.lat_degrees), + block.lon_degrees<0?'W':'E', + abs(block.lon_degrees)); + + // create directory if need be + if (!directory_created) { + mkdir(HAL_BOARD_TERRAIN_DIRECTORY, 0755); + directory_created = true; + } + + if (fd != -1) { + ::close(fd); + } + fd = ::open(path, O_RDWR|O_CREAT, 0644); + if (fd == -1) { +#if TERRAIN_DEBUG + hal.console->printf("Open %s failed - %s\n", + path, strerror(errno)); +#endif + io_failure = true; + return; + } + + file_lat_degrees = block.lat_degrees; + file_lon_degrees = block.lon_degrees; +} + +/* + 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 + Location loc1, loc2; + loc1.lat = block.lat_degrees*10*1000*1000L; + loc1.lng = block.lon_degrees*10*1000*1000L; + loc2.lat = block.lat_degrees*10*1000*1000L; + loc2.lng = (block.lon_degrees+1)*10*1000*1000L; + + // shift another two blocks east to ensure room is available + location_offset(loc2, 0, 2*grid_spacing*TERRAIN_GRID_BLOCK_SIZE_Y); + Vector2f offset = location_diff(loc1, loc2); + uint16_t east_blocks = offset.y / (grid_spacing*TERRAIN_GRID_BLOCK_SIZE_Y); + + uint32_t file_offset = (east_blocks * block.grid_idx_x + + block.grid_idx_y) * sizeof(union grid_io_block); + if (::lseek(fd, file_offset, SEEK_SET) != file_offset) { +#if TERRAIN_DEBUG + hal.console->printf("Seek %lu failed - %s\n", + (unsigned long)file_offset, strerror(errno)); +#endif + ::close(fd); + fd = -1; + io_failure = true; + } +} + +/* + write out disk_block + */ +void AP_Terrain::write_block(void) +{ + seek_offset(); + if (io_failure) { + return; + } + ssize_t ret = ::write(fd, &disk_block, sizeof(disk_block)); + if (ret != sizeof(disk_block)) { +#if TERRAIN_DEBUG + hal.console->printf("write failed - %s\n", strerror(errno)); +#endif + ::close(fd); + fd = -1; + io_failure = true; + } +#if TERRAIN_DEBUG + printf("wrote block at %ld %ld ret=%d\n", + (long)disk_block.block.lat, + (long)disk_block.block.lon, + (int)ret); +#endif + disk_io_state = DiskIoDoneWrite; +} + +/* + read in disk_block + */ +void AP_Terrain::read_block(void) +{ + seek_offset(); + if (io_failure) { + return; + } + int32_t lat = disk_block.block.lat; + int32_t lon = disk_block.block.lon; + + ssize_t ret = ::read(fd, &disk_block, sizeof(disk_block)); + if (ret != sizeof(disk_block) || + disk_block.block.lat != lat || + disk_block.block.lon != lon) { +#if TERRAIN_DEBUG + printf("read empty block at %ld %ld ret=%d\n", + (long)lat, + (long)lon, + (int)ret); +#endif + // a short read or bad data is not an IO failure, just a + // missing block on disk + memset(&disk_block, 0, sizeof(disk_block)); + disk_block.block.lat = lat; + disk_block.block.lon = lon; + disk_block.block.bitmap = 0; + } else { +#if TERRAIN_DEBUG + printf("read block at %ld %ld ret=%d\n", + (long)lat, + (long)lon, + (int)ret); +#endif + } + disk_io_state = DiskIoDoneRead; +} + +/* + timer called to do disk IO + */ +void AP_Terrain::io_timer(void) +{ + if (io_failure) { + // don't keep trying io, so we don't thrash the filesystem + // code while flying + return; + } + + switch (disk_io_state) { + case DiskIoIdle: + case DiskIoDoneRead: + case DiskIoDoneWrite: + // nothing to do + break; + + case DiskIoWaitWrite: + // need to write out the block + open_file(); + if (fd == -1) { + return; + } + write_block(); + break; + + case DiskIoWaitRead: + // need to read in the block + open_file(); + if (fd == -1) { + return; + } + read_block(); + break; + } +} + #endif // HAVE_AP_TERRAIN diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index 0b5c0979ab..52793fa6ff 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -20,7 +20,7 @@ #include #include -#if HAL_OS_POSIX_IO +#if HAL_OS_POSIX_IO && defined(HAL_BOARD_TERRAIN_DIRECTORY) #define HAVE_AP_TERRAIN 1 #else #define HAVE_AP_TERRAIN 0 @@ -61,6 +61,7 @@ array[x][y]: x is increasing north, y is increasing east array[x]: low order bits increase east first bitmap: low order bits increase east first + file: first entries increase east, then north */ class AP_Terrain @@ -97,6 +98,13 @@ private: block oriented SD cards efficient */ struct PACKED grid_block { + // bitmap of 4x4 grids filled in from GCS (56 bits are used) + uint64_t bitmap; + + // south west corner of block in degrees*10^7 + int32_t lat; + int32_t lon; + // crc of whole block, taken with crc=0 uint16_t crc; @@ -109,28 +117,39 @@ private: // heights in meters over a 32*28 grid int16_t height[TERRAIN_GRID_BLOCK_SIZE_X][TERRAIN_GRID_BLOCK_SIZE_Y]; - // south west corner of block in degrees*10^7 - int32_t lat; - int32_t lon; + // indices info 32x28 grids for this degree reference + uint16_t grid_idx_x; + uint16_t grid_idx_y; - // bitmap of 4x4 grids filled in from GCS (56 bits are used) - uint64_t bitmap; + // rounded latitude/longitude in degrees. + int16_t lon_degrees; + int8_t lat_degrees; }; /* grid_block for disk IO, aligned on 2048 byte boundaries */ union grid_io_block { - struct grid_block grid; + struct grid_block block; uint8_t buffer[2048]; }; + enum GridCacheState { + GRID_CACHE_INVALID=0, // when first initialised + GRID_CACHE_DISKWAIT=1, // when waiting for disk read + GRID_CACHE_VALID=2, // when at least partially valid + GRID_CACHE_DIRTY=3 // when updates have been made, and + // disk write needed + }; + /* a grid_block plus some meta data used for requesting new blocks */ struct grid_cache { struct grid_block grid; + volatile enum GridCacheState state; + // the last time access was requested to this block, used for LRU uint32_t last_access_ms; }; @@ -148,6 +167,10 @@ private: int32_t grid_lat; int32_t grid_lon; + // indices info 32x28 grids for this degree reference + uint16_t grid_idx_x; + uint16_t grid_idx_y; + // indexes into 32x28 grid uint8_t idx_x; // north (0..27) uint8_t idx_y; // east (0..31) @@ -155,6 +178,9 @@ private: // fraction within the grid square float frac_x; // north (0..1) float frac_y; // east (0..1) + + // file offset of this grid + uint32_t file_offset; }; // given a location, fill a grid_info structure @@ -182,6 +208,18 @@ private: */ bool request_missing(mavlink_channel_t chan, const struct grid_info &info); + /* + disk IO functions + */ + int16_t find_io_idx(void); + void check_disk_read(void); + void check_disk_write(void); + void io_timer(void); + void open_file(void); + void seek_offset(void); + void write_block(void); + void read_block(void); + // reference to AHRS, so we can ask for our position, // heading and speed AP_AHRS &ahrs; @@ -189,10 +227,37 @@ private: // cache of grids in memory, LRU struct grid_cache cache[TERRAIN_GRID_BLOCK_CACHE_SIZE]; + // a grid_cache block waiting for disk IO + enum DiskIoState { + DiskIoIdle = 0, + DiskIoWaitWrite = 1, + DiskIoWaitRead = 2, + DiskIoDoneRead = 3, + DiskIoDoneWrite = 4 + }; + volatile enum DiskIoState disk_io_state; + union grid_io_block disk_block; + // last time we asked for more grids uint32_t last_request_time_ms; static const uint64_t bitmap_mask = (((uint64_t)1U)<<(TERRAIN_GRID_BLOCK_MUL_X*TERRAIN_GRID_BLOCK_MUL_Y)) - 1; + + // open file handle on degree file + int fd; + + // has the timer been setup? + bool timer_setup; + + // degrees lat and lon of file + int8_t file_lat_degrees; + int16_t file_lon_degrees; + + // do we have an IO failure + volatile bool io_failure; + + // have we created the terrain directory? + bool directory_created; }; #endif // HAVE_AP_TERRAIN #endif // __AP_TERRAIN_H__