Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fixed bug in terrain disk format and added terrain generation script #14254

Merged
merged 2 commits into from
May 5, 2020
Merged

Fixed bug in terrain disk format and added terrain generation script #14254

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
649374e
AP_Terrain: fixed bug in disk offset calculation
tridge May 2, 2020
f6b3463
AP_Terrain: added script for creating terrain *.dat files
tridge May 1, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions libraries/AP_Terrain/AP_Terrain.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,13 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] = {
// @User: Advanced
AP_GROUPINFO("SPACING", 1, AP_Terrain, grid_spacing, 100),

// @Param: OPTIONS
// @DisplayName: Terrain options
// @Description: Options to change behaviour of terrain system
// @Bitmask: 0:Disable Download
// @User: Advanced
AP_GROUPINFO("OPTIONS", 2, AP_Terrain, options, 0),

AP_GROUPEND
};

Expand Down
12 changes: 12 additions & 0 deletions libraries/AP_Terrain/AP_Terrain.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,13 @@
// format of grid on disk
#define TERRAIN_GRID_FORMAT_VERSION 1

// we allow for a 2cm discrepancy in the grid corners. This is to
// account for different rounding in terrain DAT file generators using
// different programming languages
#define TERRAIN_LATLON_EQUAL(v1, v2) (labs((v1)-(v2)) <= 2)

#if TERRAIN_DEBUG
#include <assert.h>
#define ASSERT_RANGE(v,minv,maxv) assert((v)<=(maxv)&&(v)>=(minv))
#else
#define ASSERT_RANGE(v,minv,maxv)
Expand Down Expand Up @@ -325,6 +331,7 @@ class AP_Terrain {
void io_timer(void);
void open_file(void);
void seek_offset(void);
uint32_t east_blocks(struct grid_block &block) const;
void write_block(void);
void read_block(void);

Expand All @@ -342,6 +349,11 @@ class AP_Terrain {
// parameters
AP_Int8 enable;
AP_Int16 grid_spacing; // meters between grid points
AP_Int16 options; // option bits

enum class Options {
DisableDownload = (1U<<0),
};

// reference to AP_Mission, so we can ask preload terrain data for
// all waypoints
Expand Down
8 changes: 6 additions & 2 deletions libraries/AP_Terrain/TerrainGCS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,10 @@ bool AP_Terrain::request_missing(mavlink_channel_t chan, struct grid_cache &gcac
{
struct grid_block &grid = gcache.grid;

if (options.get() & uint16_t(Options::DisableDownload)) {
return false;
}

if (grid.spacing != grid_spacing) {
// an invalid grid
return false;
Expand Down Expand Up @@ -263,8 +267,8 @@ void AP_Terrain::handle_terrain_data(const mavlink_message_t &msg)

uint16_t i;
for (i=0; i<cache_size; i++) {
if (cache[i].grid.lat == packet.lat &&
cache[i].grid.lon == packet.lon &&
if (TERRAIN_LATLON_EQUAL(cache[i].grid.lat,packet.lat) &&
TERRAIN_LATLON_EQUAL(cache[i].grid.lon,packet.lon) &&
cache[i].grid.spacing == packet.grid_spacing &&
grid_spacing == packet.grid_spacing &&
packet.gridbit < 56) {
Expand Down
34 changes: 23 additions & 11 deletions libraries/AP_Terrain/TerrainIO.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -206,12 +206,10 @@ void AP_Terrain::open_file(void)
}

/*
seek to the right offset for disk_block
work out how many blocks needed in a stride for a given location
*/
void AP_Terrain::seek_offset(void)
uint32_t AP_Terrain::east_blocks(struct grid_block &block) const
{
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;
Expand All @@ -221,10 +219,18 @@ void AP_Terrain::seek_offset(void)
// shift another two blocks east to ensure room is available
loc2.offset(0, 2*grid_spacing*TERRAIN_GRID_BLOCK_SIZE_Y);
const Vector2f offset = loc1.get_distance_NE(loc2);
uint16_t east_blocks = offset.y / (grid_spacing*TERRAIN_GRID_BLOCK_SIZE_Y);
tridge marked this conversation as resolved.
Show resolved Hide resolved
return offset.y / (grid_spacing*TERRAIN_GRID_BLOCK_SPACING_Y);
}

uint32_t file_offset = (east_blocks * block.grid_idx_x +
block.grid_idx_y) * sizeof(union grid_io_block);
/*
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
uint32_t blocknum = east_blocks(block) * block.grid_idx_x + block.grid_idx_y;
uint32_t file_offset = blocknum * sizeof(union grid_io_block);
if (AP::FS().lseek(fd, file_offset, SEEK_SET) != (off_t)file_offset) {
#if TERRAIN_DEBUG
hal.console->printf("Seek %lu failed - %s\n",
Expand Down Expand Up @@ -283,17 +289,23 @@ void AP_Terrain::read_block(void)

ssize_t ret = AP::FS().read(fd, &disk_block, sizeof(disk_block));
if (ret != sizeof(disk_block) ||
disk_block.block.lat != lat ||
disk_block.block.lon != lon ||
!TERRAIN_LATLON_EQUAL(disk_block.block.lat,lat) ||
!TERRAIN_LATLON_EQUAL(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",
printf("read empty block at %ld %ld ret=%d (%ld %ld %u 0x%08lx) 0x%04x:0x%04x\n",
(long)lat,
(long)lon,
(int)ret);
(int)ret,
(long)disk_block.block.lat,
(long)disk_block.block.lon,
(unsigned)disk_block.block.spacing,
(unsigned long)disk_block.block.bitmap,
(unsigned)disk_block.block.crc,
(unsigned)get_block_crc(disk_block.block));
#endif
// a short read or bad data is not an IO failure, just a
// missing block on disk
Expand Down
12 changes: 6 additions & 6 deletions libraries/AP_Terrain/TerrainUtil.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,8 +113,8 @@ AP_Terrain::grid_cache &AP_Terrain::find_grid_cache(const struct grid_info &info

// see if we have that grid
for (uint16_t i=0; i<cache_size; i++) {
if (cache[i].grid.lat == info.grid_lat &&
cache[i].grid.lon == info.grid_lon &&
if (TERRAIN_LATLON_EQUAL(cache[i].grid.lat,info.grid_lat) &&
TERRAIN_LATLON_EQUAL(cache[i].grid.lon,info.grid_lon) &&
cache[i].grid.spacing == grid_spacing) {
cache[i].last_access_ms = AP_HAL::millis();
return cache[i];
Expand Down Expand Up @@ -152,16 +152,16 @@ int16_t AP_Terrain::find_io_idx(enum GridCacheState state)
{
// try first with given state
for (uint16_t i=0; i<cache_size; i++) {
if (disk_block.block.lat == cache[i].grid.lat &&
disk_block.block.lon == cache[i].grid.lon &&
if (TERRAIN_LATLON_EQUAL(disk_block.block.lat,cache[i].grid.lat) &&
TERRAIN_LATLON_EQUAL(disk_block.block.lon,cache[i].grid.lon) &&
cache[i].state == state) {
return i;
}
}
// then any state
for (uint16_t i=0; i<cache_size; i++) {
if (disk_block.block.lat == cache[i].grid.lat &&
disk_block.block.lon == cache[i].grid.lon) {
if (TERRAIN_LATLON_EQUAL(disk_block.block.lat,cache[i].grid.lat) &&
TERRAIN_LATLON_EQUAL(disk_block.block.lon,cache[i].grid.lon)) {
return i;
}
}
Expand Down
Loading