mirror of https://github.com/ArduPilot/ardupilot
AP_Terrain: split up code into separate cpp files
this should make it a bit easier to review
This commit is contained in:
parent
6b53f473d9
commit
db506b76fa
|
@ -31,8 +31,6 @@
|
|||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
|
||||
#define TERRAIN_DEBUG 0
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
// table of user settable parameters
|
||||
|
@ -68,129 +66,6 @@ AP_Terrain::AP_Terrain(AP_AHRS &_ahrs) :
|
|||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
#if TERRAIN_DEBUG
|
||||
#define ASSERT_RANGE(v,minv,maxv) assert((v)<=(maxv)&&(v)>=(minv))
|
||||
#else
|
||||
#define ASSERT_RANGE(v,minv,maxv)
|
||||
#endif
|
||||
|
||||
/*
|
||||
calculate bit number in grid_block bitmap. This corresponds to a
|
||||
bit representing a 4x4 mavlink transmitted block
|
||||
*/
|
||||
uint8_t AP_Terrain::grid_bitnum(uint8_t idx_x, uint8_t idx_y)
|
||||
{
|
||||
ASSERT_RANGE(idx_x,0,27);
|
||||
ASSERT_RANGE(idx_y,0,31);
|
||||
uint8_t subgrid_x = idx_x / TERRAIN_GRID_MAVLINK_SIZE;
|
||||
uint8_t subgrid_y = idx_y / TERRAIN_GRID_MAVLINK_SIZE;
|
||||
ASSERT_RANGE(subgrid_x,0,TERRAIN_GRID_BLOCK_MUL_X-1);
|
||||
ASSERT_RANGE(subgrid_y,0,TERRAIN_GRID_BLOCK_MUL_Y-1);
|
||||
return subgrid_y + TERRAIN_GRID_BLOCK_MUL_Y*subgrid_x;
|
||||
}
|
||||
|
||||
/*
|
||||
given a grid_info check that a given idx_x/idx_y is available (set
|
||||
in the bitmap)
|
||||
*/
|
||||
bool AP_Terrain::check_bitmap(const struct grid_block &grid, uint8_t idx_x, uint8_t idx_y)
|
||||
{
|
||||
uint8_t bitnum = grid_bitnum(idx_x, idx_y);
|
||||
return (grid.bitmap & (((uint64_t)1U)<<bitnum)) != 0;
|
||||
}
|
||||
|
||||
/*
|
||||
given a location, calculate the 32x28 grid SW corner, plus the
|
||||
grid indices
|
||||
*/
|
||||
void AP_Terrain::calculate_grid_info(const Location &loc, struct grid_info &info) const
|
||||
{
|
||||
// grids start on integer degrees. This makes storing terrain data
|
||||
// on the SD card a bit easier
|
||||
info.lat_degrees = (loc.lat<0?(loc.lat-9999999L):loc.lat) / (10*1000*1000L);
|
||||
info.lon_degrees = (loc.lng<0?(loc.lng-9999999L):loc.lng) / (10*1000*1000L);
|
||||
|
||||
// create reference position for this rounded degree position
|
||||
Location ref;
|
||||
ref.lat = info.lat_degrees*10*1000*1000L;
|
||||
ref.lng = info.lon_degrees*10*1000*1000L;
|
||||
|
||||
// find offset from reference
|
||||
Vector2f offset = location_diff(ref, loc);
|
||||
|
||||
// get indices in terms of grid_spacing elements
|
||||
uint32_t idx_x = offset.x / grid_spacing;
|
||||
uint32_t idx_y = offset.y / grid_spacing;
|
||||
|
||||
// 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
|
||||
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;
|
||||
info.idx_y = idx_y % TERRAIN_GRID_BLOCK_SPACING_Y;
|
||||
|
||||
// find the fraction (0..1) within the square
|
||||
info.frac_x = (offset.x - idx_x * grid_spacing) / grid_spacing;
|
||||
info.frac_y = (offset.y - idx_y * grid_spacing) / grid_spacing;
|
||||
|
||||
// calculate lat/lon of SW corner of 32*28 grid_block
|
||||
location_offset(ref,
|
||||
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;
|
||||
|
||||
ASSERT_RANGE(info.idx_x,0,TERRAIN_GRID_BLOCK_SPACING_X-1);
|
||||
ASSERT_RANGE(info.idx_y,0,TERRAIN_GRID_BLOCK_SPACING_Y-1);
|
||||
ASSERT_RANGE(info.frac_x,0,1);
|
||||
ASSERT_RANGE(info.frac_y,0,1);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
find a grid structure given a grid_info
|
||||
*/
|
||||
AP_Terrain::grid_cache &AP_Terrain::find_grid(const struct grid_info &info)
|
||||
{
|
||||
uint16_t oldest_i = 0;
|
||||
|
||||
// see if we have that grid
|
||||
for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
|
||||
if (cache[i].grid.lat == info.grid_lat &&
|
||||
cache[i].grid.lon == info.grid_lon &&
|
||||
cache[i].grid.spacing == grid_spacing) {
|
||||
cache[i].last_access_ms = hal.scheduler->millis();
|
||||
return cache[i];
|
||||
}
|
||||
if (cache[i].last_access_ms < cache[oldest_i].last_access_ms) {
|
||||
oldest_i = i;
|
||||
}
|
||||
}
|
||||
|
||||
// Not found. Use the oldest grid and make it this grid,
|
||||
// initially unpopulated
|
||||
struct grid_cache &grid = cache[oldest_i];
|
||||
memset(&grid, 0, sizeof(grid));
|
||||
|
||||
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.grid.version = TERRAIN_GRID_FORMAT_VERSION;
|
||||
grid.last_access_ms = hal.scheduler->millis();
|
||||
|
||||
// mark as waiting for disk read
|
||||
grid.state = GRID_CACHE_DISKWAIT;
|
||||
|
||||
return grid;
|
||||
}
|
||||
|
||||
/*
|
||||
return terrain height in meters above average sea level (WGS84) for
|
||||
a given position
|
||||
|
@ -255,355 +130,6 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
|
|||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
request any missing 4x4 grids from a block, given a grid_cache
|
||||
*/
|
||||
bool AP_Terrain::request_missing(mavlink_channel_t chan, struct grid_cache &gcache)
|
||||
{
|
||||
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) {
|
||||
// it is fully populated, nothing to do
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
ask the GCS to send a set of 4x4 grids
|
||||
*/
|
||||
mavlink_msg_terrain_request_send(chan, grid.lat, grid.lon, grid_spacing, bitmap_mask & ~grid.bitmap);
|
||||
last_request_time_ms = hal.scheduler->millis();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
request any missing 4x4 grids from a block
|
||||
*/
|
||||
bool AP_Terrain::request_missing(mavlink_channel_t chan, const struct grid_info &info)
|
||||
{
|
||||
// find the grid
|
||||
struct grid_cache &gcache = find_grid(info);
|
||||
return request_missing(chan, gcache);
|
||||
}
|
||||
|
||||
/*
|
||||
send any pending terrain request to the GCS
|
||||
*/
|
||||
void AP_Terrain::send_request(mavlink_channel_t chan)
|
||||
{
|
||||
if (enable == 0) {
|
||||
// not enabled
|
||||
return;
|
||||
}
|
||||
|
||||
// see if we need to schedule some disk IO
|
||||
schedule_disk_io();
|
||||
|
||||
// did we request recently?
|
||||
if (hal.scheduler->millis() - last_request_time_ms < 2000) {
|
||||
// too soon to request again
|
||||
return;
|
||||
}
|
||||
|
||||
Location loc;
|
||||
if (!ahrs.get_position(loc)) {
|
||||
// we don't know where we are
|
||||
return;
|
||||
}
|
||||
|
||||
// request any missing 4x4 blocks in the current grid
|
||||
struct grid_info info;
|
||||
calculate_grid_info(loc, info);
|
||||
|
||||
if (request_missing(chan, info)) {
|
||||
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;
|
||||
location_offset(loc2,
|
||||
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
|
||||
for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
|
||||
if (cache[i].state >= GRID_CACHE_VALID) {
|
||||
if (request_missing(chan, cache[i])) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// request the current loc last to ensure it has highest last
|
||||
// access time
|
||||
if (request_missing(chan, info)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// nothing to request, send a terrain report
|
||||
send_terrain_report(chan, loc);
|
||||
}
|
||||
|
||||
/*
|
||||
count bits in a uint64_t
|
||||
*/
|
||||
uint8_t AP_Terrain::bitcount64(uint64_t b)
|
||||
{
|
||||
return __builtin_popcount((unsigned)(b&0xFFFFFFFF)) + __builtin_popcount((unsigned)(b>>32));
|
||||
}
|
||||
|
||||
/*
|
||||
get some statistics for TERRAIN_REPORT
|
||||
*/
|
||||
void AP_Terrain::get_statistics(uint16_t &pending, uint16_t &loaded)
|
||||
{
|
||||
pending = 0;
|
||||
loaded = 0;
|
||||
for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
|
||||
if (cache[i].state == GRID_CACHE_INVALID) {
|
||||
continue;
|
||||
}
|
||||
uint8_t maskbits = TERRAIN_GRID_BLOCK_MUL_X*TERRAIN_GRID_BLOCK_MUL_Y;
|
||||
if (cache[i].state == GRID_CACHE_DISKWAIT) {
|
||||
pending += maskbits;
|
||||
continue;
|
||||
}
|
||||
uint8_t bitcount = bitcount64(cache[i].grid.bitmap);
|
||||
pending += maskbits - bitcount;
|
||||
loaded += bitcount;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
handle terrain messages from GCS
|
||||
*/
|
||||
void AP_Terrain::handle_data(mavlink_channel_t chan, mavlink_message_t *msg)
|
||||
{
|
||||
if (msg->msgid == MAVLINK_MSG_ID_TERRAIN_DATA) {
|
||||
handle_terrain_data(msg);
|
||||
} else if (msg->msgid == MAVLINK_MSG_ID_TERRAIN_CHECK) {
|
||||
handle_terrain_check(chan, msg);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
send a TERRAIN_REPORT for a location
|
||||
*/
|
||||
void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc)
|
||||
{
|
||||
float height = 0;
|
||||
uint16_t spacing = 0;
|
||||
if (height_amsl(loc, height)) {
|
||||
spacing = grid_spacing;
|
||||
}
|
||||
uint16_t pending, loaded;
|
||||
get_statistics(pending, loaded);
|
||||
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_TERRAIN_REPORT_LEN) {
|
||||
mavlink_msg_terrain_report_send(chan, loc.lat, loc.lng, spacing, height, pending, loaded);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
handle TERRAIN_CHECK messages from GCS
|
||||
*/
|
||||
void AP_Terrain::handle_terrain_check(mavlink_channel_t chan, mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_terrain_check_t packet;
|
||||
mavlink_msg_terrain_check_decode(msg, &packet);
|
||||
Location loc;
|
||||
loc.lat = packet.lat;
|
||||
loc.lng = packet.lon;
|
||||
send_terrain_report(chan, loc);
|
||||
}
|
||||
|
||||
/*
|
||||
handle TERRAIN_DATA messages from GCS
|
||||
*/
|
||||
void AP_Terrain::handle_terrain_data(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_terrain_data_t packet;
|
||||
mavlink_msg_terrain_data_decode(msg, &packet);
|
||||
|
||||
uint16_t i;
|
||||
for (i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
|
||||
if (cache[i].grid.lat == packet.lat &&
|
||||
cache[i].grid.lon == packet.lon &&
|
||||
cache[i].grid.spacing == packet.grid_spacing &&
|
||||
packet.gridbit < 56) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (i == TERRAIN_GRID_BLOCK_CACHE_SIZE) {
|
||||
// we don't have that grid, ignore data
|
||||
return;
|
||||
}
|
||||
struct grid_cache &gcache = cache[i];
|
||||
struct grid_block &grid = gcache.grid;
|
||||
uint8_t idx_x = (packet.gridbit / TERRAIN_GRID_BLOCK_MUL_Y) * TERRAIN_GRID_MAVLINK_SIZE;
|
||||
uint8_t idx_y = (packet.gridbit % TERRAIN_GRID_BLOCK_MUL_Y) * TERRAIN_GRID_MAVLINK_SIZE;
|
||||
ASSERT_RANGE(idx_x,0,(TERRAIN_GRID_BLOCK_MUL_X-1)*TERRAIN_GRID_MAVLINK_SIZE);
|
||||
ASSERT_RANGE(idx_y,0,(TERRAIN_GRID_BLOCK_MUL_Y-1)*TERRAIN_GRID_MAVLINK_SIZE);
|
||||
for (uint8_t x=0; x<TERRAIN_GRID_MAVLINK_SIZE; x++) {
|
||||
for (uint8_t y=0; y<TERRAIN_GRID_MAVLINK_SIZE; y++) {
|
||||
grid.height[idx_x+x][idx_y+y] = packet.data[x*TERRAIN_GRID_MAVLINK_SIZE+y];
|
||||
ASSERT_RANGE(grid.height[idx_x+x][idx_y+y], 1, 20000);
|
||||
}
|
||||
}
|
||||
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);
|
||||
if (gcache.grid.bitmap == bitmap_mask) {
|
||||
hal.console->printf("--lat=%12.7f --lon=%12.7f %u\n",
|
||||
grid.lat*1.0e-7f,
|
||||
grid.lon*1.0e-7f,
|
||||
grid.height[0][0]);
|
||||
Location loc2;
|
||||
loc2.lat = grid.lat;
|
||||
loc2.lng = grid.lon;
|
||||
location_offset(loc2, 28*grid_spacing, 32*grid_spacing);
|
||||
hal.console->printf("--lat=%12.7f --lon=%12.7f %u\n",
|
||||
loc2.lat*1.0e-7f,
|
||||
loc2.lng*1.0e-7f,
|
||||
grid.height[27][31]);
|
||||
}
|
||||
#endif
|
||||
|
||||
// see if we need to schedule some disk IO
|
||||
update();
|
||||
}
|
||||
|
||||
/*
|
||||
find cache index of disk_block
|
||||
*/
|
||||
int16_t AP_Terrain::find_io_idx(enum GridCacheState state)
|
||||
{
|
||||
// try first with given state
|
||||
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 &&
|
||||
cache[i].state == state) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
// then any state
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Check if we need to do disk IO for grids.
|
||||
*/
|
||||
void AP_Terrain::schedule_disk_io(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(GRID_CACHE_DISKWAIT);
|
||||
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(GRID_CACHE_DIRTY);
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
1Hz update function. This is here to ensure progress is made on disk
|
||||
|
@ -614,225 +140,10 @@ void AP_Terrain::update(void)
|
|||
{
|
||||
// just schedule any needed disk IO
|
||||
schedule_disk_io();
|
||||
}
|
||||
|
||||
/*
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
/********************************************************
|
||||
All the functions below this point run in the IO timer
|
||||
context, which is a separate thread. The code uses the state
|
||||
machine controlled by disk_io_state to manage who has
|
||||
access to the structures and to prevent race conditions.
|
||||
|
||||
The IO timer context owns the data when disk_io_state is
|
||||
DiskIoWaitWrite or DiskIoWaitRead. The main thread owns the
|
||||
data when disk_io_state is DiskIoIdle, DiskIoDoneWrite or
|
||||
DiskIoDoneRead
|
||||
|
||||
All file operations are done by the IO thread.
|
||||
*********************************************************/
|
||||
|
||||
|
||||
/*
|
||||
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;
|
||||
}
|
||||
|
||||
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
|
||||
hal.console->printf("write failed - %s\n", strerror(errno));
|
||||
#endif
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
io_failure = true;
|
||||
} else {
|
||||
::fsync(fd);
|
||||
#if TERRAIN_DEBUG
|
||||
printf("wrote block at %ld %ld ret=%d mask=%07llx\n",
|
||||
(long)disk_block.block.lat,
|
||||
(long)disk_block.block.lon,
|
||||
(int)ret,
|
||||
(unsigned long long)disk_block.block.bitmap);
|
||||
#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 ||
|
||||
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",
|
||||
(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 mask=%07llx\n",
|
||||
(long)lat,
|
||||
(long)lon,
|
||||
(int)ret,
|
||||
(unsigned long long)disk_block.block.bitmap);
|
||||
#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;
|
||||
}
|
||||
// try to ensure the home location is populated
|
||||
float height;
|
||||
height_amsl(ahrs.get_home(), height);
|
||||
}
|
||||
|
||||
#endif // HAVE_AP_TERRAIN
|
||||
|
|
|
@ -30,6 +30,10 @@
|
|||
|
||||
#include <AP_Param.h>
|
||||
#include <AP_AHRS.h>
|
||||
#include <AP_Baro.h>
|
||||
|
||||
#define TERRAIN_DEBUG 0
|
||||
|
||||
|
||||
// MAVLink sends 4x4 grids
|
||||
#define TERRAIN_GRID_MAVLINK_SIZE 4
|
||||
|
@ -55,6 +59,13 @@
|
|||
// format of grid on disk
|
||||
#define TERRAIN_GRID_FORMAT_VERSION 1
|
||||
|
||||
#if TERRAIN_DEBUG
|
||||
#define ASSERT_RANGE(v,minv,maxv) assert((v)<=(maxv)&&(v)>=(minv))
|
||||
#else
|
||||
#define ASSERT_RANGE(v,minv,maxv)
|
||||
#endif
|
||||
|
||||
|
||||
/*
|
||||
Data conventions in this library:
|
||||
|
||||
|
|
|
@ -0,0 +1,275 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
handle vehicle <-> GCS communications for terrain library
|
||||
*/
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <GCS_MAVLink.h>
|
||||
#include <GCS.h>
|
||||
#include "AP_Terrain.h"
|
||||
|
||||
#if HAVE_AP_TERRAIN
|
||||
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
/*
|
||||
request any missing 4x4 grids from a block, given a grid_cache
|
||||
*/
|
||||
bool AP_Terrain::request_missing(mavlink_channel_t chan, struct grid_cache &gcache)
|
||||
{
|
||||
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) {
|
||||
// it is fully populated, nothing to do
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
ask the GCS to send a set of 4x4 grids
|
||||
*/
|
||||
mavlink_msg_terrain_request_send(chan, grid.lat, grid.lon, grid_spacing, bitmap_mask & ~grid.bitmap);
|
||||
last_request_time_ms = hal.scheduler->millis();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
request any missing 4x4 grids from a block
|
||||
*/
|
||||
bool AP_Terrain::request_missing(mavlink_channel_t chan, const struct grid_info &info)
|
||||
{
|
||||
// find the grid
|
||||
struct grid_cache &gcache = find_grid(info);
|
||||
return request_missing(chan, gcache);
|
||||
}
|
||||
|
||||
/*
|
||||
send any pending terrain request to the GCS
|
||||
*/
|
||||
void AP_Terrain::send_request(mavlink_channel_t chan)
|
||||
{
|
||||
if (enable == 0) {
|
||||
// not enabled
|
||||
return;
|
||||
}
|
||||
|
||||
// see if we need to schedule some disk IO
|
||||
schedule_disk_io();
|
||||
|
||||
// did we request recently?
|
||||
if (hal.scheduler->millis() - last_request_time_ms < 2000) {
|
||||
// too soon to request again
|
||||
return;
|
||||
}
|
||||
|
||||
Location loc;
|
||||
if (!ahrs.get_position(loc)) {
|
||||
// we don't know where we are
|
||||
return;
|
||||
}
|
||||
|
||||
// request any missing 4x4 blocks in the current grid
|
||||
struct grid_info info;
|
||||
calculate_grid_info(loc, info);
|
||||
|
||||
if (request_missing(chan, info)) {
|
||||
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;
|
||||
location_offset(loc2,
|
||||
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
|
||||
for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
|
||||
if (cache[i].state >= GRID_CACHE_VALID) {
|
||||
if (request_missing(chan, cache[i])) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// request the current loc last to ensure it has highest last
|
||||
// access time
|
||||
if (request_missing(chan, info)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// nothing to request, send a terrain report
|
||||
send_terrain_report(chan, loc);
|
||||
}
|
||||
|
||||
/*
|
||||
count bits in a uint64_t
|
||||
*/
|
||||
uint8_t AP_Terrain::bitcount64(uint64_t b)
|
||||
{
|
||||
return __builtin_popcount((unsigned)(b&0xFFFFFFFF)) + __builtin_popcount((unsigned)(b>>32));
|
||||
}
|
||||
|
||||
/*
|
||||
get some statistics for TERRAIN_REPORT
|
||||
*/
|
||||
void AP_Terrain::get_statistics(uint16_t &pending, uint16_t &loaded)
|
||||
{
|
||||
pending = 0;
|
||||
loaded = 0;
|
||||
for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
|
||||
if (cache[i].state == GRID_CACHE_INVALID) {
|
||||
continue;
|
||||
}
|
||||
uint8_t maskbits = TERRAIN_GRID_BLOCK_MUL_X*TERRAIN_GRID_BLOCK_MUL_Y;
|
||||
if (cache[i].state == GRID_CACHE_DISKWAIT) {
|
||||
pending += maskbits;
|
||||
continue;
|
||||
}
|
||||
uint8_t bitcount = bitcount64(cache[i].grid.bitmap);
|
||||
pending += maskbits - bitcount;
|
||||
loaded += bitcount;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
handle terrain messages from GCS
|
||||
*/
|
||||
void AP_Terrain::handle_data(mavlink_channel_t chan, mavlink_message_t *msg)
|
||||
{
|
||||
if (msg->msgid == MAVLINK_MSG_ID_TERRAIN_DATA) {
|
||||
handle_terrain_data(msg);
|
||||
} else if (msg->msgid == MAVLINK_MSG_ID_TERRAIN_CHECK) {
|
||||
handle_terrain_check(chan, msg);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
send a TERRAIN_REPORT for a location
|
||||
*/
|
||||
void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc)
|
||||
{
|
||||
float height = 0;
|
||||
uint16_t spacing = 0;
|
||||
if (height_amsl(loc, height)) {
|
||||
spacing = grid_spacing;
|
||||
}
|
||||
uint16_t pending, loaded;
|
||||
get_statistics(pending, loaded);
|
||||
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_TERRAIN_REPORT_LEN) {
|
||||
mavlink_msg_terrain_report_send(chan, loc.lat, loc.lng, spacing, height, pending, loaded);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
handle TERRAIN_CHECK messages from GCS
|
||||
*/
|
||||
void AP_Terrain::handle_terrain_check(mavlink_channel_t chan, mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_terrain_check_t packet;
|
||||
mavlink_msg_terrain_check_decode(msg, &packet);
|
||||
Location loc;
|
||||
loc.lat = packet.lat;
|
||||
loc.lng = packet.lon;
|
||||
send_terrain_report(chan, loc);
|
||||
}
|
||||
|
||||
/*
|
||||
handle TERRAIN_DATA messages from GCS
|
||||
*/
|
||||
void AP_Terrain::handle_terrain_data(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_terrain_data_t packet;
|
||||
mavlink_msg_terrain_data_decode(msg, &packet);
|
||||
|
||||
uint16_t i;
|
||||
for (i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
|
||||
if (cache[i].grid.lat == packet.lat &&
|
||||
cache[i].grid.lon == packet.lon &&
|
||||
cache[i].grid.spacing == packet.grid_spacing &&
|
||||
packet.gridbit < 56) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (i == TERRAIN_GRID_BLOCK_CACHE_SIZE) {
|
||||
// we don't have that grid, ignore data
|
||||
return;
|
||||
}
|
||||
struct grid_cache &gcache = cache[i];
|
||||
struct grid_block &grid = gcache.grid;
|
||||
uint8_t idx_x = (packet.gridbit / TERRAIN_GRID_BLOCK_MUL_Y) * TERRAIN_GRID_MAVLINK_SIZE;
|
||||
uint8_t idx_y = (packet.gridbit % TERRAIN_GRID_BLOCK_MUL_Y) * TERRAIN_GRID_MAVLINK_SIZE;
|
||||
ASSERT_RANGE(idx_x,0,(TERRAIN_GRID_BLOCK_MUL_X-1)*TERRAIN_GRID_MAVLINK_SIZE);
|
||||
ASSERT_RANGE(idx_y,0,(TERRAIN_GRID_BLOCK_MUL_Y-1)*TERRAIN_GRID_MAVLINK_SIZE);
|
||||
for (uint8_t x=0; x<TERRAIN_GRID_MAVLINK_SIZE; x++) {
|
||||
for (uint8_t y=0; y<TERRAIN_GRID_MAVLINK_SIZE; y++) {
|
||||
grid.height[idx_x+x][idx_y+y] = packet.data[x*TERRAIN_GRID_MAVLINK_SIZE+y];
|
||||
ASSERT_RANGE(grid.height[idx_x+x][idx_y+y], 1, 20000);
|
||||
}
|
||||
}
|
||||
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);
|
||||
if (gcache.grid.bitmap == bitmap_mask) {
|
||||
hal.console->printf("--lat=%12.7f --lon=%12.7f %u\n",
|
||||
grid.lat*1.0e-7f,
|
||||
grid.lon*1.0e-7f,
|
||||
grid.height[0][0]);
|
||||
Location loc2;
|
||||
loc2.lat = grid.lat;
|
||||
loc2.lng = grid.lon;
|
||||
location_offset(loc2, 28*grid_spacing, 32*grid_spacing);
|
||||
hal.console->printf("--lat=%12.7f --lon=%12.7f %u\n",
|
||||
loc2.lat*1.0e-7f,
|
||||
loc2.lng*1.0e-7f,
|
||||
grid.height[27][31]);
|
||||
}
|
||||
#endif
|
||||
|
||||
// see if we need to schedule some disk IO
|
||||
update();
|
||||
}
|
||||
|
||||
|
||||
#endif // HAVE_AP_TERRAIN
|
|
@ -0,0 +1,332 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
handle disk IO for terrain code
|
||||
*/
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <GCS_MAVLink.h>
|
||||
#include <GCS.h>
|
||||
#include "AP_Terrain.h"
|
||||
|
||||
#if HAVE_AP_TERRAIN
|
||||
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
/*
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Check if we need to do disk IO for grids.
|
||||
*/
|
||||
void AP_Terrain::schedule_disk_io(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(GRID_CACHE_DISKWAIT);
|
||||
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(GRID_CACHE_DIRTY);
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/********************************************************
|
||||
All the functions below this point run in the IO timer context, which
|
||||
is a separate thread. The code uses the state machine controlled by
|
||||
disk_io_state to manage who has access to the structures and to
|
||||
prevent race conditions.
|
||||
|
||||
The IO timer context owns the data when disk_io_state is
|
||||
DiskIoWaitWrite or DiskIoWaitRead. The main thread owns the data when
|
||||
disk_io_state is DiskIoIdle, DiskIoDoneWrite or DiskIoDoneRead
|
||||
|
||||
All file operations are done by the IO thread.
|
||||
*********************************************************/
|
||||
|
||||
|
||||
/*
|
||||
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;
|
||||
}
|
||||
|
||||
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
|
||||
hal.console->printf("write failed - %s\n", strerror(errno));
|
||||
#endif
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
io_failure = true;
|
||||
} else {
|
||||
::fsync(fd);
|
||||
#if TERRAIN_DEBUG
|
||||
printf("wrote block at %ld %ld ret=%d mask=%07llx\n",
|
||||
(long)disk_block.block.lat,
|
||||
(long)disk_block.block.lon,
|
||||
(int)ret,
|
||||
(unsigned long long)disk_block.block.bitmap);
|
||||
#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 ||
|
||||
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",
|
||||
(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 mask=%07llx\n",
|
||||
(long)lat,
|
||||
(long)lon,
|
||||
(int)ret,
|
||||
(unsigned long long)disk_block.block.bitmap);
|
||||
#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
|
|
@ -0,0 +1,191 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
handle disk IO for terrain code
|
||||
*/
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <GCS_MAVLink.h>
|
||||
#include <GCS.h>
|
||||
#include "AP_Terrain.h"
|
||||
|
||||
#if HAVE_AP_TERRAIN
|
||||
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
/*
|
||||
calculate bit number in grid_block bitmap. This corresponds to a
|
||||
bit representing a 4x4 mavlink transmitted block
|
||||
*/
|
||||
uint8_t AP_Terrain::grid_bitnum(uint8_t idx_x, uint8_t idx_y)
|
||||
{
|
||||
ASSERT_RANGE(idx_x,0,27);
|
||||
ASSERT_RANGE(idx_y,0,31);
|
||||
uint8_t subgrid_x = idx_x / TERRAIN_GRID_MAVLINK_SIZE;
|
||||
uint8_t subgrid_y = idx_y / TERRAIN_GRID_MAVLINK_SIZE;
|
||||
ASSERT_RANGE(subgrid_x,0,TERRAIN_GRID_BLOCK_MUL_X-1);
|
||||
ASSERT_RANGE(subgrid_y,0,TERRAIN_GRID_BLOCK_MUL_Y-1);
|
||||
return subgrid_y + TERRAIN_GRID_BLOCK_MUL_Y*subgrid_x;
|
||||
}
|
||||
|
||||
/*
|
||||
given a grid_info check that a given idx_x/idx_y is available (set
|
||||
in the bitmap)
|
||||
*/
|
||||
bool AP_Terrain::check_bitmap(const struct grid_block &grid, uint8_t idx_x, uint8_t idx_y)
|
||||
{
|
||||
uint8_t bitnum = grid_bitnum(idx_x, idx_y);
|
||||
return (grid.bitmap & (((uint64_t)1U)<<bitnum)) != 0;
|
||||
}
|
||||
|
||||
/*
|
||||
given a location, calculate the 32x28 grid SW corner, plus the
|
||||
grid indices
|
||||
*/
|
||||
void AP_Terrain::calculate_grid_info(const Location &loc, struct grid_info &info) const
|
||||
{
|
||||
// grids start on integer degrees. This makes storing terrain data
|
||||
// on the SD card a bit easier
|
||||
info.lat_degrees = (loc.lat<0?(loc.lat-9999999L):loc.lat) / (10*1000*1000L);
|
||||
info.lon_degrees = (loc.lng<0?(loc.lng-9999999L):loc.lng) / (10*1000*1000L);
|
||||
|
||||
// create reference position for this rounded degree position
|
||||
Location ref;
|
||||
ref.lat = info.lat_degrees*10*1000*1000L;
|
||||
ref.lng = info.lon_degrees*10*1000*1000L;
|
||||
|
||||
// find offset from reference
|
||||
Vector2f offset = location_diff(ref, loc);
|
||||
|
||||
// get indices in terms of grid_spacing elements
|
||||
uint32_t idx_x = offset.x / grid_spacing;
|
||||
uint32_t idx_y = offset.y / grid_spacing;
|
||||
|
||||
// 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
|
||||
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;
|
||||
info.idx_y = idx_y % TERRAIN_GRID_BLOCK_SPACING_Y;
|
||||
|
||||
// find the fraction (0..1) within the square
|
||||
info.frac_x = (offset.x - idx_x * grid_spacing) / grid_spacing;
|
||||
info.frac_y = (offset.y - idx_y * grid_spacing) / grid_spacing;
|
||||
|
||||
// calculate lat/lon of SW corner of 32*28 grid_block
|
||||
location_offset(ref,
|
||||
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;
|
||||
|
||||
ASSERT_RANGE(info.idx_x,0,TERRAIN_GRID_BLOCK_SPACING_X-1);
|
||||
ASSERT_RANGE(info.idx_y,0,TERRAIN_GRID_BLOCK_SPACING_Y-1);
|
||||
ASSERT_RANGE(info.frac_x,0,1);
|
||||
ASSERT_RANGE(info.frac_y,0,1);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
find a grid structure given a grid_info
|
||||
*/
|
||||
AP_Terrain::grid_cache &AP_Terrain::find_grid(const struct grid_info &info)
|
||||
{
|
||||
uint16_t oldest_i = 0;
|
||||
|
||||
// see if we have that grid
|
||||
for (uint16_t i=0; i<TERRAIN_GRID_BLOCK_CACHE_SIZE; i++) {
|
||||
if (cache[i].grid.lat == info.grid_lat &&
|
||||
cache[i].grid.lon == info.grid_lon &&
|
||||
cache[i].grid.spacing == grid_spacing) {
|
||||
cache[i].last_access_ms = hal.scheduler->millis();
|
||||
return cache[i];
|
||||
}
|
||||
if (cache[i].last_access_ms < cache[oldest_i].last_access_ms) {
|
||||
oldest_i = i;
|
||||
}
|
||||
}
|
||||
|
||||
// Not found. Use the oldest grid and make it this grid,
|
||||
// initially unpopulated
|
||||
struct grid_cache &grid = cache[oldest_i];
|
||||
memset(&grid, 0, sizeof(grid));
|
||||
|
||||
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.grid.version = TERRAIN_GRID_FORMAT_VERSION;
|
||||
grid.last_access_ms = hal.scheduler->millis();
|
||||
|
||||
// mark as waiting for disk read
|
||||
grid.state = GRID_CACHE_DISKWAIT;
|
||||
|
||||
return grid;
|
||||
}
|
||||
|
||||
/*
|
||||
find cache index of disk_block
|
||||
*/
|
||||
int16_t AP_Terrain::find_io_idx(enum GridCacheState state)
|
||||
{
|
||||
// try first with given state
|
||||
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 &&
|
||||
cache[i].state == state) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
// then any state
|
||||
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;
|
||||
}
|
||||
|
||||
/*
|
||||
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;
|
||||
}
|
||||
|
||||
#endif // HAVE_AP_TERRAIN
|
Loading…
Reference in New Issue