Hello this is a similar script that saves each .dat file as a jpeg image however there is no crc handling or missing data handling.
Hope it helps those trying to write their terrain protocol handling.
import os,sys,struct
import math
import numpy as np
import sys
from PIL import Image
TERRAIN_GRID_MAVLINK_SIZE = 4
# MAVLink sends 4x4 grids
# a 2k grid_block on disk contains 8x7 of the mavlink grids. Each
# grid block overlaps by one with its neighbour. This ensures that
# the altitude at any point can be calculated from a single grid
# block
TERRAIN_GRID_BLOCK_MUL_X = 7
TERRAIN_GRID_BLOCK_MUL_Y = 8
IO_BLOCK_SIZE = 2048
TERRAIN_GRID_BLOCK_SIZE_X = (TERRAIN_GRID_MAVLINK_SIZE*TERRAIN_GRID_BLOCK_MUL_X)
TERRAIN_GRID_BLOCK_SIZE_Y = (TERRAIN_GRID_MAVLINK_SIZE*TERRAIN_GRID_BLOCK_MUL_Y)
def geo2dist(coords_1,coords_2):
R=6471 * 10**3
phi_1 = (coords_1[0]*np.pi)/180.00
phi_2 = (coords_2[0]*np.pi)/180.00
d_phi = phi_1-phi_2
d_lbd = (coords_1[1] - coords_2[1])*np.pi/180.00
a = (np.sin(d_phi/2))**2 + np.cos(phi_1) * np.cos(phi_2) * (np.sin(d_lbd/2))**2
c = 2* np.arctan((a**0.5) / ((1-a)**0.5))
dist2wp = R * c
return dist2wp
def to_float32(f):
return struct.unpack('f', struct.pack('f',f))[0]
class Terrain_Manager:
def __init__(self,filename):
self.filename = filename
self.fh = open(self.filename, 'rb')
self.fh.seek(0, os.SEEK_END)
self.file_size = self.fh.tell()
self.total_blocks = self.fh.tell()/IO_BLOCK_SIZE
print("Total blocks: ", self.total_blocks)
self.fh.seek(0, os.SEEK_SET)
self.lat_count = []
self.lon_count = []
self.RESOLUTION = 0.01
self.parsed_data = []
lat_array=[]
lon_array=np.array(0)
lat_row_array = np.array(0)
full_file_array=np.array(0)
self.fh.seek(0, os.SEEK_SET)
image_height = 7*4 - 1
image_width = 8*4 - 1
i_g = 0
prev_lat = 0
prev_lon = 0
first_block_parsed_in_row = False
for gridblockindex in range(int(self.total_blocks)):
geo_raster = np.zeros((8*4,7*4,3),dtype=np.uint8)
file_offset = gridblockindex*IO_BLOCK_SIZE
self.fh.seek(file_offset)
buf = self.fh.read(IO_BLOCK_SIZE)
(bitmap, lat, lon, crc, version, spacing) = struct.unpack("<QiiHHH", buf[:22])
if bitmap!=0:
(heights) = struct.unpack("<896h", buf[22:1814])
ht_index=0
for gy in range((7*4)):
for gx in range((8*4)):
gx_pos=gx
gy_pos = gy
R_MARKER = 256*256
G_MARKER = 256
B_MARKER = 1
R_HEIGHT = ((heights[ht_index])/self.RESOLUTION)
geo_raster[ gx_pos, gy_pos , 0] = int((R_HEIGHT - R_HEIGHT%R_MARKER)/R_MARKER)
G_HEIGHT = R_HEIGHT%R_MARKER
geo_raster[ gx_pos, gy_pos , 1] = int((G_HEIGHT - G_HEIGHT%G_MARKER)/G_MARKER)
B_HEIGHT = G_HEIGHT%G_MARKER
geo_raster[ gx_pos, gy_pos , 2] = int((B_HEIGHT - B_HEIGHT%B_MARKER)/B_MARKER)
ht_index+=1
if prev_lat!=lat and first_block_parsed_in_row:
print("New row ",lat,lon)
# lat_row_array = geo_raster
lat_array.append(lat_row_array)
lat_row_array = geo_raster
print(lat_row_array.shape)
if not first_block_parsed_in_row:
lat_row_array = geo_raster
first_block_parsed_in_row=True
print("First block parsed",lat,lon)
print(lat_row_array.shape)
if prev_lat==lat and first_block_parsed_in_row:
lat_row_array = np.concatenate((lat_row_array,geo_raster[3:,:]),axis=0)
print("concat along latitude",lat,lon)
print(lat_row_array.shape)
prev_lat=lat
fg=0
for i in (lat_array):
if fg!=0:
full_file_array =np.concatenate((full_file_array,i[:,3:]),axis=1)
else:
full_file_array = i
fg+=1
print("lat array index",i.shape,fg)
im = Image.fromarray(full_file_array).convert('RGB')
im.save(filename + "test"+"_rgb"+".jpeg")
if __name__ == '__main__':
for i in range(1,len(sys.argv)):
print(sys.argv[i])
try:
z = Terrain_Manager(sys.argv[i])
except:
pass
