Browse Source

AP_Terrain: convert to use AP_Filesystem

master
Andrew Tridgell 6 years ago
parent
commit
a4860afd32
  1. 10
      libraries/AP_Terrain/AP_Terrain.cpp
  2. 3
      libraries/AP_Terrain/AP_Terrain.h
  3. 33
      libraries/AP_Terrain/TerrainIO.cpp
  4. 10
      libraries/AP_Terrain/TerrainUtil.cpp

10
libraries/AP_Terrain/AP_Terrain.cpp

@ -23,15 +23,7 @@
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
#include <assert.h> #include <AP_Filesystem/AP_Filesystem.h>
#include <stdio.h>
#if HAL_OS_POSIX_IO
#include <unistd.h>
#include <sys/stat.h>
#include <fcntl.h>
#endif
#include <sys/types.h>
#include <errno.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;

3
libraries/AP_Terrain/AP_Terrain.h

@ -17,8 +17,9 @@
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Common/Location.h> #include <AP_Common/Location.h>
#include <AP_Filesystem/AP_Filesystem.h>
#if (HAL_OS_POSIX_IO || HAL_OS_FATFS_IO) && defined(HAL_BOARD_TERRAIN_DIRECTORY) #if HAVE_FILESYSTEM_SUPPORT && defined(HAL_BOARD_TERRAIN_DIRECTORY)
#define AP_TERRAIN_AVAILABLE 1 #define AP_TERRAIN_AVAILABLE 1
#else #else
#define AP_TERRAIN_AVAILABLE 0 #define AP_TERRAIN_AVAILABLE 0

33
libraries/AP_Terrain/TerrainIO.cpp

@ -21,19 +21,12 @@
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <stdio.h>
#include "AP_Terrain.h" #include "AP_Terrain.h"
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
#include <assert.h> #include <AP_Filesystem/AP_Filesystem.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; extern const AP_HAL::HAL& hal;
@ -180,7 +173,7 @@ void AP_Terrain::open_file(void)
// create directory if need be // create directory if need be
if (!directory_created) { if (!directory_created) {
*p = 0; *p = 0;
directory_created = !mkdir(file_path, 0755); directory_created = !AP::FS().mkdir(file_path);
*p = '/'; *p = '/';
if (!directory_created) { if (!directory_created) {
@ -196,13 +189,9 @@ void AP_Terrain::open_file(void)
} }
if (fd != -1) { if (fd != -1) {
::close(fd); AP::FS().close(fd);
} }
#if HAL_OS_POSIX_IO fd = AP::FS().open(file_path, O_RDWR|O_CREAT);
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 (fd == -1) {
#if TERRAIN_DEBUG #if TERRAIN_DEBUG
hal.console->printf("Open %s failed - %s\n", hal.console->printf("Open %s failed - %s\n",
@ -236,12 +225,12 @@ void AP_Terrain::seek_offset(void)
uint32_t file_offset = (east_blocks * block.grid_idx_x + uint32_t file_offset = (east_blocks * block.grid_idx_x +
block.grid_idx_y) * sizeof(union grid_io_block); block.grid_idx_y) * sizeof(union grid_io_block);
if (::lseek(fd, file_offset, SEEK_SET) != (off_t)file_offset) { if (AP::FS().lseek(fd, file_offset, SEEK_SET) != (off_t)file_offset) {
#if TERRAIN_DEBUG #if TERRAIN_DEBUG
hal.console->printf("Seek %lu failed - %s\n", hal.console->printf("Seek %lu failed - %s\n",
(unsigned long)file_offset, strerror(errno)); (unsigned long)file_offset, strerror(errno));
#endif #endif
::close(fd); AP::FS().close(fd);
fd = -1; fd = -1;
io_failure = true; io_failure = true;
} }
@ -259,16 +248,16 @@ void AP_Terrain::write_block(void)
disk_block.block.crc = get_block_crc(disk_block.block); disk_block.block.crc = get_block_crc(disk_block.block);
ssize_t ret = ::write(fd, &disk_block, sizeof(disk_block)); ssize_t ret = AP::FS().write(fd, &disk_block, sizeof(disk_block));
if (ret != sizeof(disk_block)) { if (ret != sizeof(disk_block)) {
#if TERRAIN_DEBUG #if TERRAIN_DEBUG
hal.console->printf("write failed - %s\n", strerror(errno)); hal.console->printf("write failed - %s\n", strerror(errno));
#endif #endif
::close(fd); AP::FS().close(fd);
fd = -1; fd = -1;
io_failure = true; io_failure = true;
} else { } else {
::fsync(fd); AP::FS().fsync(fd);
#if TERRAIN_DEBUG #if TERRAIN_DEBUG
printf("wrote block at %ld %ld ret=%d mask=%07llx\n", printf("wrote block at %ld %ld ret=%d mask=%07llx\n",
(long)disk_block.block.lat, (long)disk_block.block.lat,
@ -292,7 +281,7 @@ void AP_Terrain::read_block(void)
int32_t lat = disk_block.block.lat; int32_t lat = disk_block.block.lat;
int32_t lon = disk_block.block.lon; int32_t lon = disk_block.block.lon;
ssize_t ret = ::read(fd, &disk_block, sizeof(disk_block)); ssize_t ret = AP::FS().read(fd, &disk_block, sizeof(disk_block));
if (ret != sizeof(disk_block) || if (ret != sizeof(disk_block) ||
disk_block.block.lat != lat || disk_block.block.lat != lat ||
disk_block.block.lon != lon || disk_block.block.lon != lon ||

10
libraries/AP_Terrain/TerrainUtil.cpp

@ -25,15 +25,7 @@
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
#include <assert.h> #include <AP_Filesystem/AP_Filesystem.h>
#include <stdio.h>
#if HAL_OS_POSIX_IO
#include <unistd.h>
#include <sys/stat.h>
#include <fcntl.h>
#endif
#include <sys/types.h>
#include <errno.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;

Loading…
Cancel
Save