AP_Terrain: added test option to terrain creation script
This commit is contained in:
Andrew Tridgell
parent
15f9f7009c
commit
078de3205c
@ -7,6 +7,10 @@ from MAVProxy.modules.mavproxy_map import srtm
|
||||
import math, struct, os, sys
|
||||
import crc16, time, struct
|
||||
|
||||
# avoid annoying crc16 DeprecationWarning
|
||||
import warnings
|
||||
warnings.filterwarnings("ignore", category=DeprecationWarning)
|
||||
|
||||
# MAVLink sends 4x4 grids
|
||||
TERRAIN_GRID_MAVLINK_SIZE = 4
|
||||
|
||||
@ -29,6 +33,8 @@ TERRAIN_GRID_BLOCK_SIZE_Y = (TERRAIN_GRID_MAVLINK_SIZE*TERRAIN_GRID_BLOCK_MUL_Y)
|
||||
TERRAIN_GRID_FORMAT_VERSION = 1
|
||||
|
||||
IO_BLOCK_SIZE = 2048
|
||||
IO_BLOCK_DATA_SIZE = 1821
|
||||
IO_BLOCK_TRAILER_SIZE = IO_BLOCK_SIZE - IO_BLOCK_DATA_SIZE
|
||||
|
||||
GRID_SPACING = 100
|
||||
|
||||
@ -160,8 +166,25 @@ class GridBlock(object):
|
||||
stride = east_blocks(self.lat_degrees*1e7, self.lon_degrees*1e7)
|
||||
return stride * self.grid_idx_x + self.grid_idx_y
|
||||
|
||||
class TerrainError:
|
||||
'''represent errors from testing a degree file'''
|
||||
def __init__(self):
|
||||
self.missing = 0
|
||||
self.incorrect = 0
|
||||
self.errors = 0
|
||||
|
||||
def add(self, err):
|
||||
self.missing += err.missing
|
||||
self.incorrect += err.incorrect
|
||||
self.errors += err.errors
|
||||
|
||||
def __str__(self):
|
||||
if self.missing == 0 and self.incorrect == 0 and self.errors == 0:
|
||||
return "OK"
|
||||
return "Errors: %u Missing: %u Incorrect: %u" % (self.errors, self.missing, self.incorrect)
|
||||
|
||||
class DataFile(object):
|
||||
def __init__(self, lat, lon):
|
||||
def __init__(self, lat, lon, readonly=False):
|
||||
if lat < 0:
|
||||
NS = 'S'
|
||||
else:
|
||||
@ -170,13 +193,18 @@ class DataFile(object):
|
||||
EW = 'W'
|
||||
else:
|
||||
EW = 'E'
|
||||
name = "terrain/%c%02u%c%03u.DAT" % (NS, min(abs(int(lat)), 99),
|
||||
name = "%s/%c%02u%c%03u.DAT" % (args.directory,
|
||||
NS, min(abs(int(lat)), 99),
|
||||
EW, min(abs(int(lon)), 999))
|
||||
try:
|
||||
os.mkdir("terrain")
|
||||
os.mkdir(args.directory)
|
||||
except Exception:
|
||||
pass
|
||||
if not os.path.exists(name):
|
||||
self.fh = None
|
||||
if readonly:
|
||||
if os.path.exists(name):
|
||||
self.fh = open(name, 'rb')
|
||||
elif not os.path.exists(name):
|
||||
self.fh = open(name, 'w+b')
|
||||
else:
|
||||
self.fh = open(name, 'r+b')
|
||||
@ -194,6 +222,7 @@ class DataFile(object):
|
||||
for gx in range(TERRAIN_GRID_BLOCK_SIZE_X):
|
||||
buf += struct.pack("<%uh" % TERRAIN_GRID_BLOCK_SIZE_Y, *block.height[gx])
|
||||
buf += struct.pack("<HHhb", block.grid_idx_x, block.grid_idx_y, block.lon_degrees, block.lat_degrees)
|
||||
buf += struct.pack("%uB" % IO_BLOCK_TRAILER_SIZE, *[0]*IO_BLOCK_TRAILER_SIZE)
|
||||
return buf
|
||||
|
||||
def write(self, block):
|
||||
@ -201,13 +230,15 @@ class DataFile(object):
|
||||
self.seek_offset(block)
|
||||
block.crc = 0
|
||||
buf = self.pack(block)
|
||||
block.crc = crc16.crc16xmodem(buf)
|
||||
block.crc = crc16.crc16xmodem(buf[:IO_BLOCK_DATA_SIZE])
|
||||
buf = self.pack(block)
|
||||
self.fh.write(buf)
|
||||
|
||||
def check_filled(self, block):
|
||||
'''read a grid block and check if already filled'''
|
||||
self.seek_offset(block)
|
||||
if self.fh is None:
|
||||
return False
|
||||
buf = self.fh.read(IO_BLOCK_SIZE)
|
||||
if len(buf) != IO_BLOCK_SIZE:
|
||||
return False
|
||||
@ -219,11 +250,74 @@ class DataFile(object):
|
||||
bitmap != (1<<56)-1):
|
||||
return False
|
||||
buf = buf[:16] + struct.pack("<H", 0) + buf[18:]
|
||||
crc2 = crc16.crc16xmodem(buf[:1821])
|
||||
crc2 = crc16.crc16xmodem(buf[:IO_BLOCK_DATA_SIZE])
|
||||
if crc2 != crc:
|
||||
return False
|
||||
return True
|
||||
|
||||
def bitnum(self, gx, gy):
|
||||
'''get bit number for a grid index'''
|
||||
subgrid_x = gx // TERRAIN_GRID_MAVLINK_SIZE
|
||||
subgrid_y = gy // TERRAIN_GRID_MAVLINK_SIZE
|
||||
return subgrid_y + TERRAIN_GRID_BLOCK_MUL_Y*subgrid_x
|
||||
|
||||
def compare(self, block, test_threshold):
|
||||
'''test a grid block for correct values
|
||||
return missing, incorrect tuple
|
||||
'''
|
||||
err = TerrainError()
|
||||
total_values = TERRAIN_GRID_BLOCK_SIZE_X * TERRAIN_GRID_BLOCK_SIZE_Y
|
||||
if self.fh is None:
|
||||
err.errors += 1
|
||||
return err
|
||||
|
||||
self.seek_offset(block)
|
||||
buf = self.fh.read(IO_BLOCK_SIZE)
|
||||
if len(buf) == 0:
|
||||
# not filled in
|
||||
err.missing += total_values
|
||||
return err
|
||||
|
||||
if len(buf) != IO_BLOCK_SIZE:
|
||||
print("bad read %u" % len(buf))
|
||||
err.errors += 1
|
||||
return err
|
||||
|
||||
(bitmap, lat, lon, crc, version, spacing) = struct.unpack("<QiiHHH", buf[:22])
|
||||
if version == 0 and spacing == 0:
|
||||
# not filled in
|
||||
err.missing += total_values
|
||||
return err
|
||||
|
||||
if (version != TERRAIN_GRID_FORMAT_VERSION or
|
||||
abs(lat - block.lat)>2 or
|
||||
abs(lon - block.lon)>2 or
|
||||
spacing != GRID_SPACING):
|
||||
print("bad header")
|
||||
err.errors += 1
|
||||
return err
|
||||
|
||||
buf = buf[:16] + struct.pack("<H", 0) + buf[18:]
|
||||
crc2 = crc16.crc16xmodem(buf[:IO_BLOCK_DATA_SIZE])
|
||||
if crc2 != crc:
|
||||
print("bad crc")
|
||||
err.errors += 1
|
||||
return err
|
||||
|
||||
ofs = 22
|
||||
for gx in range(TERRAIN_GRID_BLOCK_SIZE_X):
|
||||
heights = struct.unpack("<%uh" % TERRAIN_GRID_BLOCK_SIZE_Y, buf[ofs:ofs+TERRAIN_GRID_BLOCK_SIZE_Y*2])
|
||||
for gy in range(TERRAIN_GRID_BLOCK_SIZE_Y):
|
||||
mask = 1 << self.bitnum(gx, gy)
|
||||
if not bitmap & mask:
|
||||
err.missing += 1
|
||||
continue
|
||||
if abs(heights[gy] - block.height[gx][gy]) > test_threshold:
|
||||
err.incorrect += 1
|
||||
ofs += TERRAIN_GRID_BLOCK_SIZE_Y*2
|
||||
|
||||
return err
|
||||
|
||||
def pos_range(filename):
|
||||
'''return min/max of lat/lon in a file'''
|
||||
fh = open(filename, 'rb')
|
||||
@ -240,7 +334,7 @@ def pos_range(filename):
|
||||
print("Bad version %u in %s" % (version, filename))
|
||||
break
|
||||
buf = buf[:16] + struct.pack("<H", 0) + buf[18:]
|
||||
crc2 = crc16.crc16xmodem(buf[:1821])
|
||||
crc2 = crc16.crc16xmodem(buf[:IO_BLOCK_DATA_SIZE])
|
||||
if crc2 != crc:
|
||||
print("Bad CRC in %s" % filename)
|
||||
break
|
||||
@ -276,7 +370,7 @@ def create_degree(lat, lon):
|
||||
while True:
|
||||
blocknum += 1
|
||||
(lat_e7, lon_e7) = pos_from_file_offset(lat_int, lon_int, blocknum * IO_BLOCK_SIZE)
|
||||
if int(lat_e7*1.0e-7) - lat_int > 1:
|
||||
if lat_e7*1.0e-7 - lat_int >= 1.0:
|
||||
break
|
||||
lat = lat_e7 * 1.0e-7
|
||||
lon = lon_e7 * 1.0e-7
|
||||
@ -306,16 +400,91 @@ def create_degree(lat, lon):
|
||||
grid.fill(gx, gy, altitude)
|
||||
dfile.write(grid)
|
||||
|
||||
def test_degree(lat, lon):
|
||||
'''test data file for one degree lat/lon. Return a TerrainError object'''
|
||||
lat_int = int(math.floor(lat))
|
||||
lon_int = int(math.floor((lon)))
|
||||
|
||||
tiles = {}
|
||||
|
||||
dfile = DataFile(lat_int, lon_int, True)
|
||||
|
||||
print("Testing %d %d" % (lat_int, lon_int))
|
||||
|
||||
blocknum = -1
|
||||
|
||||
errors = TerrainError()
|
||||
|
||||
while True:
|
||||
blocknum += 1
|
||||
(lat_e7, lon_e7) = pos_from_file_offset(lat_int, lon_int, blocknum * IO_BLOCK_SIZE)
|
||||
if lat_e7*1.0e-7 - lat_int >= 1.0:
|
||||
break
|
||||
lat = lat_e7 * 1.0e-7
|
||||
lon = lon_e7 * 1.0e-7
|
||||
grid = GridBlock(lat_int, lon_int, lat, lon)
|
||||
if grid.blocknum() != blocknum:
|
||||
continue
|
||||
for gx in range(TERRAIN_GRID_BLOCK_SIZE_X):
|
||||
for gy in range(TERRAIN_GRID_BLOCK_SIZE_Y):
|
||||
lat_e7, lon_e7 = add_offset(lat*1.0e7, lon*1.0e7, gx*GRID_SPACING, gy*GRID_SPACING)
|
||||
lat2_int = int(math.floor(lat_e7*1.0e-7))
|
||||
lon2_int = int(math.floor(lon_e7*1.0e-7))
|
||||
tile_idx = (lat2_int, lon2_int)
|
||||
while not tile_idx in tiles:
|
||||
tile = downloader.getTile(lat2_int, lon2_int)
|
||||
waited = False
|
||||
if tile == 0:
|
||||
print("waiting on download of %d,%d" % (lat2_int, lon2_int))
|
||||
time.sleep(0.3)
|
||||
waited = True
|
||||
continue
|
||||
if waited:
|
||||
print("downloaded %d,%d" % (lat2_int, lon2_int))
|
||||
tiles[tile_idx] = tile
|
||||
altitude = tiles[tile_idx].getAltitudeFromLatLon(lat_e7*1.0e-7, lon_e7*1.0e-7)
|
||||
grid.fill(gx, gy, altitude)
|
||||
err = dfile.compare(grid, args.test_threshold)
|
||||
errors.add(err)
|
||||
return errors
|
||||
|
||||
|
||||
def test_directory():
|
||||
'''test all terrain tiles in a directory'''
|
||||
err = TerrainError()
|
||||
files = sorted(os.listdir(args.directory))
|
||||
for f in files:
|
||||
if not f.endswith(".DAT"):
|
||||
continue
|
||||
if not (f.startswith("N") or f.startswith("S")):
|
||||
continue
|
||||
f = f[:-4]
|
||||
lat = int(f[1:3])
|
||||
lon = int(f[4:])
|
||||
if f[0] == 'S':
|
||||
lat = -lat
|
||||
if f[3] == 'W':
|
||||
lon = -lon
|
||||
e = test_degree(lat, lon)
|
||||
print(e)
|
||||
err.add(e)
|
||||
return err
|
||||
|
||||
|
||||
|
||||
from argparse import ArgumentParser
|
||||
parser = ArgumentParser(description='terrain data creator')
|
||||
|
||||
parser.add_argument("lat", type=float, default=-35.363261)
|
||||
parser.add_argument("lon", type=float, default=149.165230)
|
||||
parser.add_argument("--lat", type=float, default=None)
|
||||
parser.add_argument("--lon", type=float, default=None)
|
||||
parser.add_argument("--force", action='store_true', help="overwrite existing full blocks")
|
||||
parser.add_argument("--radius", type=int, default=100, help="radius in km")
|
||||
parser.add_argument("--debug", action='store_true', default=False)
|
||||
parser.add_argument("--spacing", type=int, default=100, help="grid spacing in meters")
|
||||
parser.add_argument("--pos-range", default=None, help="show position range for a file")
|
||||
parser.add_argument("--test", action='store_true', help="test altitudes instead of writing them")
|
||||
parser.add_argument("--test-threshold", default=2.0, help="test altitude threshold")
|
||||
parser.add_argument("--directory", default="terrain", help="directory to use")
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.pos_range is not None:
|
||||
@ -329,17 +498,25 @@ GRID_SPACING = args.spacing
|
||||
|
||||
done = set()
|
||||
|
||||
if args.test:
|
||||
err = test_directory()
|
||||
if err.errors:
|
||||
sys.exit(1)
|
||||
sys.exit(0)
|
||||
|
||||
if args.lat is None or args.lon is None:
|
||||
print("You must supply latitude and longitude")
|
||||
sys.exit(1)
|
||||
|
||||
for dx in range(-args.radius, args.radius):
|
||||
for dy in range(-args.radius, args.radius):
|
||||
(lat2,lon2) = add_offset(args.lat*1e7, args.lon*1e7, dx*1000.0, dy*1000.0)
|
||||
if abs(lat2) > 90e7 or abs(lon2) > 180e7:
|
||||
continue
|
||||
lat_int = int(round(lat2 * 1.0e-7))
|
||||
lon_int = int(round(lon2 * 1.0e-7))
|
||||
lat_int = int(math.floor(lat2 * 1.0e-7))
|
||||
lon_int = int(math.floor(lon2 * 1.0e-7))
|
||||
tag = (lat_int, lon_int)
|
||||
if tag in done:
|
||||
continue
|
||||
done.add(tag)
|
||||
create_degree(lat_int, lon_int)
|
||||
|
||||
create_degree(args.lat, args.lon)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user