mirror of https://github.com/ArduPilot/ardupilot
366 lines
9.9 KiB
C++
366 lines
9.9 KiB
C++
/*
|
|
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/AP_HAL.h>
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
|
#include <GCS_MAVLink/GCS.h>
|
|
#include "AP_Terrain.h"
|
|
|
|
#if AP_TERRAIN_AVAILABLE
|
|
|
|
#include <assert.h>
|
|
#include <stdio.h>
|
|
#if HAL_OS_POSIX_IO
|
|
#include <unistd.h>
|
|
#include <sys/stat.h>
|
|
#include <fcntl.h>
|
|
#include <errno.h>
|
|
#endif
|
|
#include <sys/types.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<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<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 || !allocate()) {
|
|
return;
|
|
}
|
|
|
|
if (!timer_setup) {
|
|
timer_setup = true;
|
|
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Terrain::io_timer, void));
|
|
}
|
|
|
|
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 = AP_HAL::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;
|
|
}
|
|
if (file_path == nullptr) {
|
|
const char* terrain_dir = hal.util->get_custom_terrain_directory();
|
|
if (terrain_dir == nullptr) {
|
|
terrain_dir = HAL_BOARD_TERRAIN_DIRECTORY;
|
|
}
|
|
if (asprintf(&file_path, "%s/NxxExxx.DAT", terrain_dir) <= 0) {
|
|
io_failure = true;
|
|
file_path = nullptr;
|
|
return;
|
|
}
|
|
}
|
|
if (file_path == nullptr) {
|
|
io_failure = true;
|
|
return;
|
|
}
|
|
char *p = &file_path[strlen(file_path)-12];
|
|
if (*p != '/') {
|
|
io_failure = true;
|
|
return;
|
|
}
|
|
snprintf(p, 13, "/%c%02u%c%03u.DAT",
|
|
block.lat_degrees<0?'S':'N',
|
|
MIN(abs((int32_t)block.lat_degrees), 99),
|
|
block.lon_degrees<0?'W':'E',
|
|
MIN(abs((int32_t)block.lon_degrees), 999));
|
|
|
|
// create directory if need be
|
|
if (!directory_created) {
|
|
*p = 0;
|
|
directory_created = !mkdir(file_path, 0755);
|
|
*p = '/';
|
|
|
|
if (!directory_created) {
|
|
if (errno == EEXIST) {
|
|
// directory already existed
|
|
directory_created = true;
|
|
} else {
|
|
// if we didn't succeed at making the directory, then IO failed
|
|
io_failure = true;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (fd != -1) {
|
|
::close(fd);
|
|
}
|
|
#if HAL_OS_POSIX_IO
|
|
fd = ::open(file_path, O_RDWR|O_CREAT|O_CLOEXEC, 0644);
|
|
#else
|
|
fd = ::open(file_path, O_RDWR|O_CREAT|O_CLOEXEC);
|
|
#endif
|
|
if (fd == -1) {
|
|
#if TERRAIN_DEBUG
|
|
hal.console->printf("Open %s failed - %s\n",
|
|
file_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) != (off_t)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 // AP_TERRAIN_AVAILABLE
|