Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Terrain: added disk IO for terrain data

Browse Source
This commit is contained in:
Andrew Tridgell 2014-07-22 11:28:54 +10:00
parent 63b5811a7c
commit 9f76f0276f
2 changed files with 385 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

320
libraries/AP_Terrain/AP_Terrain.cpp
View File

@ -22,10 +22,15 @@
#include "AP_Terrain.h"
#include <assert.h>
#include <stdio.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#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; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
if (disk_block.block.lat == cache[i].grid.lat &&
disk_block.block.lon == cache[i].grid.lon) {
return i;
}
}
return -1;
}
/*
check for blocks that need to be read from disk
*/
void AP_Terrain::check_disk_read(void)
{
for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
if (cache[i].state == GRID_CACHE_DISKWAIT) {
disk_block.block = cache[i].grid;
disk_io_state = DiskIoWaitRead;
return;
}
}
}
/*
check for blocks that need to be written to disk
*/
void AP_Terrain::check_disk_write(void)
{
for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
if (cache[i].state == GRID_CACHE_DIRTY) {
disk_block.block = cache[i].grid;
disk_io_state = DiskIoWaitWrite;
return;
}
}
}
/*
update terrain data. Check if we need to request more grids. This
should be called at 1Hz
*/
void AP_Terrain::update(void)
{
if (enable == 0) {
return;
}
if (!timer_setup) {
timer_setup = true;
hal.scheduler->register_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

79
libraries/AP_Terrain/AP_Terrain.h
View File

@ -20,7 +20,7 @@
#include <AP_Common.h>
#include <AP_HAL.h>
#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__