jacob.eva/openCom-Companion
1
0
Fork 0
mirror of https://github.com/liberatedsystems/openCom-Companion.git synced 2024-11-25 14:50:37 +01:00
Code Issues Actions Packages Projects Releases Wiki Activity
b0d3ac763a
openCom-Companion/sbapp/sideband/geo.py
Mark Qvist a2f04e23a6 Added EGM2008 world altitude correction.
2024-03-26 14:18:42 +01:00

565 lines
20 KiB
Python
Raw Blame History

import os
import time
import mmap
import struct
import RNS
from math import pi, sin, cos, acos, asin, tan, atan, atan2
from math import radians, degrees, sqrt
# WGS84 Parameters
# a = 6378137.0,
# f = 0.0033528106647474805,
# e2 = 0.0066943799901413165,
# b = 6356752.314245179,
# Planetary metrics
equatorial_radius = 6378.137 *1e3
polar_radius = 6356.7523142 *1e3
ellipsoid_flattening = 1-(polar_radius/equatorial_radius)
eccentricity_squared = 2*ellipsoid_flattening-pow(ellipsoid_flattening,2)
###############################
mean_earth_radius = (1/3)*(2*equatorial_radius+polar_radius)
geoid_height = None
def geocentric_latitude(geodetic_latitude):
e2 = eccentricity_squared
lat = radians(geodetic_latitude)
return degrees(atan((1.0 - e2) * tan(lat)))
def geodetic_latitude(geocentric_latitude):
e2 = eccentricity_squared
lat = radians(geocentric_latitude)
return degrees(atan( (1/(1.0 - e2)) * tan(lat)))
def ellipsoid_radius_at(latitude):
lat = radians(latitude)
a = equatorial_radius; b = polar_radius;
a2 = pow(a,2); b2 = pow(b,2)
r = sqrt(
( pow(a2*cos(lat), 2) + pow(b2*sin(lat), 2) )
/
( pow(a*cos(lat), 2) + pow(b*sin(lat), 2) )
)
return r
def euclidian_point(latitude, longitude, altitude=0, ellipsoid=True):
# Convert latitude and longitude to radians
# and get ellipsoid or sphere radius
lat = radians(latitude); lon = radians(longitude)
r = ellipsoid_radius_at(latitude) if ellipsoid else mean_earth_radius
# Calculate euclidian coordinates from longitude
# and geocentric latitude.
gclat = radians(geocentric_latitude(latitude)) if ellipsoid else lat
x = cos(lon)*cos(gclat)*r
y = cos(gclat)*sin(lon)*r
z = sin(gclat)*r
# Calculate surface normal of ellipsoid at
# coordinates to add altitude to point
normal_x = cos(lat)*cos(lon)
normal_y = cos(lat)*sin(lon)
normal_z = sin(lat)
if altitude != 0:
x += altitude*normal_x
y += altitude*normal_y
z += altitude*normal_z
return (x,y,z, normal_x, normal_y, normal_z)
def distance(p1, p2):
dx = p1[0]-p2[0]
dy = p1[1]-p2[1]
dz = p1[2]-p2[2]
return sqrt(dx*dx + dy*dy + dz*dz)
def euclidian_distance(c1, c2, ellipsoid=True):
lat1 = c1[0]; lon1 = c1[1]; alt1 = c1[2]
lat2 = c2[0]; lon2 = c2[1]; alt2 = c2[2]
if len(c1) >= 2 and len(c2) >= 2:
if len(c1) == 2: c1 += (0,)
if len(c2) == 2: c2 += (0,)
return distance(
euclidian_point(lat1, lon1, alt1, ellipsoid=ellipsoid),
euclidian_point(lat2, lon2, alt2, ellipsoid=ellipsoid)
)
else:
return None
def central_angle(c1, c2):
lat1 = radians(c1[0]); lon1 = radians(c1[1])
lat2 = radians(c2[0]); lon2 = radians(c2[1])
d_lat = abs(lat1-lat2)
d_lon = abs(lon1-lon2)
ca = acos(
sin(lat1) * sin(lat2) +
cos(lat1) * cos(lat2) * cos(d_lon)
)
return ca
def arc_length(central_angle, r=mean_earth_radius):
return r*central_angle;
def spherical_distance(c1, c2, altitude=0, r=mean_earth_radius):
d = (r+altitude)*central_angle(c1, c2)
return d
def ellipsoid_distance(c1, c2):
# TODO: Update this to the method described by Karney in 2013
# instead of using Vincenty's algorithm.
try:
if c1[:2] == c2[:2]:
return 0
if c1[0] == 0.0: c1 = (1e-6, c1[1])
a = equatorial_radius
f = ellipsoid_flattening
b = (1 - f)*a # polar radius
tolerance = 1e-9 # to stop iteration
phi1, phi2 = radians(c1[0]), radians(c2[0])
U1 = atan((1-f)*tan(phi1))
U2 = atan((1-f)*tan(phi2))
L1, L2 = radians(c1[1]), radians(c2[1])
L = L2 - L1
lambda_old = L + 0
max_iterations = 10000
iteration = 0
timeout = 1.0
st = time.time()
while True:
iteration += 1
t = (cos(U2)*sin(lambda_old))**2
t += (cos(U1)*sin(U2) - sin(U1)*cos(U2)*cos(lambda_old))**2
sin_sigma = t**0.5
cos_sigma = sin(U1)*sin(U2) + cos(U1)*cos(U2)*cos(lambda_old)
sigma = atan2(sin_sigma, cos_sigma)
sin_alpha = cos(U1)*cos(U2)*sin(lambda_old) / sin_sigma
cos_sq_alpha = 1 - sin_alpha**2
cos_2sigma_m = cos_sigma - 2*sin(U1)*sin(U2)/cos_sq_alpha
C = f*cos_sq_alpha*(4 + f*(4-3*cos_sq_alpha))/16
t = sigma + C*sin_sigma*(cos_2sigma_m + C*cos_sigma*(-1 + 2*cos_2sigma_m**2))
lambda_new = L + (1 - C)*f*sin_alpha*t
if abs(lambda_new - lambda_old) <= tolerance:
break
else:
lambda_old = lambda_new
if iteration%1000 == 0:
if iteration >= max_iterations:
return None
if time.time() > st+timeout:
return None
u2 = cos_sq_alpha*((a**2 - b**2)/b**2)
A = 1 + (u2/16384)*(4096 + u2*(-768+u2*(320 - 175*u2)))
B = (u2/1024)*(256 + u2*(-128 + u2*(74 - 47*u2)))
t = cos_2sigma_m + 0.25*B*(cos_sigma*(-1 + 2*cos_2sigma_m**2))
t -= (B/6)*cos_2sigma_m*(-3 + 4*sin_sigma**2)*(-3 + 4*cos_2sigma_m**2)
delta_sigma = B * sin_sigma * t
s = b*A*(sigma - delta_sigma)
return s
except Exception as e:
return None
def azalt(c1, c2, ellipsoid=True):
c2rp = rotate_globe(c1, c2, ellipsoid=ellipsoid)
altitude = None
azimuth = None
if (c2rp[2]*c2rp[2]) + (c2rp[1]*c2rp[1]) > 1e-6:
theta = degrees(atan2(c2rp[2], c2rp[1]))
azimuth = 90 - theta
if azimuth < 0: azimuth += 360
if azimuth > 360: azimuth -= 360
azimuth = round(azimuth,4)
c1p = euclidian_point(c1[0], c1[1], c1[2], ellipsoid=ellipsoid)
c2p = euclidian_point(c2[0], c2[1], c2[2], ellipsoid=ellipsoid)
nvd = normalised_vector_diff(c2p, c1p)
if nvd != None:
cax = nvd[0]; cay = nvd[1]; caz = nvd[2]
cnx = c1p[3]; cny = c1p[4]; cnz = c1p[5]
a = acos(cax*cnx + cay*cny + caz*cnz)
altitude = round(90 - degrees(a),4)
return (azimuth, altitude,4)
def normalised_vector_diff(b, a):
dx = b[0] - a[0]
dy = b[1] - a[1]
dz = b[2] - a[2]
d_squared = dx*dx + dy*dy + dz*dz
if d_squared == 0:
return None
d = sqrt(d_squared)
return (dx/d, dy/d, dz/d)
def rotate_globe(c1, c2, ellipsoid=True):
if len(c1) >= 2 and len(c2) >= 2:
if len(c1) == 2: c1 += (0,)
if len(c2) == 2: c2 += (0,)
c2r = (c2[0], c2[1]-c1[1], c2[2])
c2rp = euclidian_point(c2r[0], c2r[1], c2r[2], ellipsoid=ellipsoid)
lat1 = -1*radians(c1[0])
if ellipsoid:
lat1 = radians(geocentric_latitude(degrees(lat1)))
lat1cos = cos(lat1)
lat1sin = sin(lat1)
c2x = (c2rp[0] * lat1cos) - (c2rp[2] * lat1sin)
c2y = c2rp[1]
c2z = (c2rp[0] * lat1sin) + (c2rp[2] * lat1cos)
return (c2x, c2y, c2z)
def orthodromic_distance(c1, c2, ellipsoid=True):
if ellipsoid:
return ellipsoid_distance(c1, c2)
else:
return spherical_distance(c1, c2)
def distance_to_horizon(c, ellipsoid=False):
if ellipsoid:
raise NotImplementedError("Distance to horizon on the ellipsoid is not yet implemented")
else:
# TODO: This is a only barely functional simplification.
# Need to calculate the geodesic distance to the horizon
# instead.
if len(c) >= 3:
r = mean_earth_radius
h = c[2]
return sqrt(pow((h+r),2) - r*r)
else:
return None
def angle_to_horizon(c, ellipsoid=False):
if ellipsoid:
raise NotImplementedError("Angle to horizon on the ellipsoid is not yet implemented")
else:
r = mean_earth_radius
h = c[2]
if h < 0: h = 0
return degrees(-acos(r/(r+h)))
def euclidian_horizon_distance(h):
r = mean_earth_radius
b = r
c = r+h
a = c**2 - b**2
return sqrt(a)
def euclidian_horizon_arc(h):
r = mean_earth_radius
d = euclidian_horizon_distance(h)
a = d; b = r; c = r+h
arc = acos( (b**2+c**2-a**2) / (2*b*c) )
return arc
def radio_horizon(h, rh=0, ellipsoid=False):
if ellipsoid:
raise NotImplementedError("Radio horizon on the ellipsoid is not yet implemented")
else:
geocentric_angle_to_horizon = euclidian_horizon_arc(h)
geodesic_distance = arc_length(geocentric_angle_to_horizon, r=mean_earth_radius)
return geodesic_distance
def shared_radio_horizon(c1, c2,):
lat1 = c1[0]; lon1 = c1[1]; h1 = c1[2]
lat2 = c2[0]; lon2 = c2[1]; h2 = c2[2]
geodesic_distance = orthodromic_distance((lat1, lon1, 0.0), (lat2, lon2, 0.0) , ellipsoid=False)
antenna_distance = euclidian_distance(c1,c2,ellipsoid=False)
rh1 = radio_horizon(h1)
rh2 = radio_horizon(h2)
rhc = rh1+rh2
return {
"horizon1":rh1, "horizon2":rh2, "shared":rhc,
"within":rhc > geodesic_distance,
"geodesic_distance": geodesic_distance,
"antenna_distance": antenna_distance
}
def geoid_offset(lat, lon):
global geoid_height
if geoid_height == None:
geoid_height = GeoidHeight()
return geoid_height.get(lat, lon)
def altitude_to_aamsl(alt, lat, lon):
if alt == None or lat == None or lon == None:
return None
else:
return alt-geoid_offset(lat, lon)
######################################################
# GeoidHeight class by Kim Vandry <vandry@TZoNE.ORG> #
# Originally ported fromGeographicLib/src/Geoid.cpp #
# LGPLv3 License #
######################################################
class GeoidHeight(object):
c0 = 240
c3 = (
( 9, -18, -88, 0, 96, 90, 0, 0, -60, -20),
( -9, 18, 8, 0, -96, 30, 0, 0, 60, -20),
( 9, -88, -18, 90, 96, 0, -20, -60, 0, 0),
(186, -42, -42, -150, -96, -150, 60, 60, 60, 60),
( 54, 162, -78, 30, -24, -90, -60, 60, -60, 60),
( -9, -32, 18, 30, 24, 0, 20, -60, 0, 0),
( -9, 8, 18, 30, -96, 0, -20, 60, 0, 0),
( 54, -78, 162, -90, -24, 30, 60, -60, 60, -60),
(-54, 78, 78, 90, 144, 90, -60, -60, -60, -60),
( 9, -8, -18, -30, -24, 0, 20, 60, 0, 0),
( -9, 18, -32, 0, 24, 30, 0, 0, -60, 20),
( 9, -18, -8, 0, -24, -30, 0, 0, 60, 20),
)
c0n = 372
c3n = (
( 0, 0, -131, 0, 138, 144, 0, 0, -102, -31),
( 0, 0, 7, 0, -138, 42, 0, 0, 102, -31),
( 62, 0, -31, 0, 0, -62, 0, 0, 0, 31),
(124, 0, -62, 0, 0, -124, 0, 0, 0, 62),
(124, 0, -62, 0, 0, -124, 0, 0, 0, 62),
( 62, 0, -31, 0, 0, -62, 0, 0, 0, 31),
( 0, 0, 45, 0, -183, -9, 0, 93, 18, 0),
( 0, 0, 216, 0, 33, 87, 0, -93, 12, -93),
( 0, 0, 156, 0, 153, 99, 0, -93, -12, -93),
( 0, 0, -45, 0, -3, 9, 0, 93, -18, 0),
( 0, 0, -55, 0, 48, 42, 0, 0, -84, 31),
( 0, 0, -7, 0, -48, -42, 0, 0, 84, 31),
)
c0s = 372
c3s = (
( 18, -36, -122, 0, 120, 135, 0, 0, -84, -31),
(-18, 36, -2, 0, -120, 51, 0, 0, 84, -31),
( 36, -165, -27, 93, 147, -9, 0, -93, 18, 0),
(210, 45, -111, -93, -57, -192, 0, 93, 12, 93),
(162, 141, -75, -93, -129, -180, 0, 93, -12, 93),
(-36, -21, 27, 93, 39, 9, 0, -93, -18, 0),
( 0, 0, 62, 0, 0, 31, 0, 0, 0, -31),
( 0, 0, 124, 0, 0, 62, 0, 0, 0, -62),
( 0, 0, 124, 0, 0, 62, 0, 0, 0, -62),
( 0, 0, 62, 0, 0, 31, 0, 0, 0, -31),
(-18, 36, -64, 0, 66, 51, 0, 0, -102, 31),
( 18, -36, 2, 0, -66, -51, 0, 0, 102, 31),
)
def __init__(self, name="egm2008-5.pgm"):
self.offset = None
self.scale = None
if "TELEMETER_GEOID_PATH" in os.environ:
geoid_dir = os.environ["TELEMETER_GEOID_PATH"]
else:
geoid_dir = "./"
pgm_path = os.path.join(geoid_dir, name)
RNS.log(f"Opening {pgm_path} as EGM for altitude correction", RNS.LOG_DEBUG)
with open(pgm_path, "rb") as f:
line = f.readline()
if line != b"P5\012" and line != b"P5\015\012":
raise Exception("No PGM header")
headerlen = len(line)
while True:
line = f.readline()
if len(line) == 0:
raise Exception("EOF before end of file header")
headerlen += len(line)
if line.startswith(b'# Offset '):
try:
self.offset = int(line[9:])
except ValueError as e:
raise Exception("Error reading offset", e)
elif line.startswith(b'# Scale '):
try:
self.scale = float(line[8:])
except ValueError as e:
raise Exception("Error reading scale", e)
elif not line.startswith(b'#'):
try:
self.width, self.height = list(map(int, line.split()))
except ValueError as e:
raise Exception("Bad PGM width&height line", e)
break
line = f.readline()
headerlen += len(line)
levels = int(line)
if levels != 65535:
raise Exception("PGM file must have 65535 gray levels")
if self.offset is None:
raise Exception("PGM file does not contain offset")
if self.scale is None:
raise Exception("PGM file does not contain scale")
if self.width < 2 or self.height < 2:
raise Exception("Raster size too small")
fd = f.fileno()
fullsize = os.fstat(fd).st_size
if fullsize - headerlen != self.width * self.height * 2:
raise Exception("File has the wrong length")
self.headerlen = headerlen
self.raw = mmap.mmap(fd, fullsize, mmap.MAP_SHARED, mmap.PROT_READ)
self.rlonres = self.width / 360.0
self.rlatres = (self.height - 1) / 180.0
self.ix = None
self.iy = None
def _rawval(self, ix, iy):
if iy < 0:
iy = -iy
ix += self.width/2
elif iy >= self.height:
iy = 2 * (self.height - 1) - iy
ix += self.width/2
if ix < 0:
ix += self.width
elif ix >= self.width:
ix -= self.width
return struct.unpack_from('>H', self.raw,
(iy * self.width + ix) * 2 + self.headerlen
)[0]
def get(self, lat, lon, cubic=True):
if lon < 0:
lon += 360
fy = (90 - lat) * self.rlatres
fx = lon * self.rlonres
iy = int(fy)
ix = int(fx)
fx -= ix
fy -= iy
if iy == self.height - 1:
iy -= 1
if ix != self.ix or iy != self.iy:
self.ix = ix
self.iy = iy
if not cubic:
self.v00 = self._rawval(ix, iy)
self.v01 = self._rawval(ix+1, iy)
self.v10 = self._rawval(ix, iy+1)
self.v11 = self._rawval(ix+1, iy+1)
else:
v = (
self._rawval(ix , iy - 1),
self._rawval(ix + 1, iy - 1),
self._rawval(ix - 1, iy ),
self._rawval(ix , iy ),
self._rawval(ix + 1, iy ),
self._rawval(ix + 2, iy ),
self._rawval(ix - 1, iy + 1),
self._rawval(ix , iy + 1),
self._rawval(ix + 1, iy + 1),
self._rawval(ix + 2, iy + 1),
self._rawval(ix , iy + 2),
self._rawval(ix + 1, iy + 2)
)
if iy == 0:
c3x = GeoidHeight.c3n
c0x = GeoidHeight.c0n
elif iy == self.height - 2:
c3x = GeoidHeight.c3s
c0x = GeoidHeight.c0s
else:
c3x = GeoidHeight.c3
c0x = GeoidHeight.c0
self.t = [
sum([ v[j] * c3x[j][i] for j in range(12) ]) / float(c0x)
for i in range(10)
]
if not cubic:
a = (1 - fx) * self.v00 + fx * self.v01
b = (1 - fx) * self.v10 + fx * self.v11
h = (1 - fy) * a + fy * b
else:
h = (
self.t[0] +
fx * (self.t[1] + fx * (self.t[3] + fx * self.t[6])) +
fy * (
self.t[2] + fx * (self.t[4] + fx * self.t[7]) +
fy * (self.t[5] + fx * self.t[8] + fy * self.t[9])
)
)
return self.offset + self.scale * h
# def tests():
# import RNS
# import numpy as np
# from geographiclib.geodesic import Geodesic
# geod = Geodesic.WGS84
# coords = [
# [(51.2308, 4.38703, 0.0), (47.699437, 9.268651, 0.0)],
# [(51.2308, 4.38703, 0.0), (47.699437, 9.268651, 30.0*1e3)],
# [(0.0, 0.0, 0.0), (0.0, 1.0/60/60, 30.0)],
# # [(51.230800, 4.38703, 0.0), (51.230801, 4.38703, 0.0)],
# # [(35.3524, 135.0302, 100), (35.3532,135.0305, 500)],
# # [(57.758793, 22.605194, 0.0), (43.048838, -9.241343, 0.0)],
# # [(0.0, 0.0, 0.0), (0.0, 0.0, 0.0)],
# # [(-90.0, 0.0, 0.0), (90.0, 0.0, 0.0)],
# # [(-90.0, 0.0, 0.0), (78.0, 0.0, 0.0)],
# # [(0.0, 0.0, 0.0), (0.5, 179.5, 0.0)],
# # [(0.7, 0.0, 0.0), (0.0, -180.0, 0.0)],
# ]
# for cs in coords:
# c1 = cs[0]; c2 = cs[1]
# print("Testing: "+str(c1)+" -> "+str(c2))
# us = time.time()
# ld = c1+c2; g = geod.Inverse(c1[0], c1[1], c2[0], c2[1])
# print("Lib computed in "+str(round((time.time()-us)*1e6, 3))+"us")
# us = time.time()
# eld = orthodromic_distance(c1,c2,ellipsoid=True)
# if eld:
# print("Own computed in "+str(round((time.time()-us)*1e6, 3))+"us")
# else:
# print("Own timed out in "+str(round((time.time()-us)*1e6, 3))+"us")
# ed_own = euclidian_distance(c1,c2,ellipsoid=True)
# sd_own = orthodromic_distance(c1,c2,ellipsoid=False)
# aa = azalt(c1,c2,ellipsoid=True)
# fac = 1
# if eld: print("LibDiff = "+RNS.prettydistance(g['s12']-eld)+f" {fac*g['s12']-fac*eld}")
# print("Spherical = "+RNS.prettydistance(sd_own)+f" {fac*sd_own}")
# # print("EllipLib = "+RNS.prettydistance(g['s12'])+f" {fac*g['s12']}")
# if eld: print("Ellipsoid = "+RNS.prettydistance(eld)+f" {fac*eld}")
# print("Euclidian = "+RNS.prettydistance(ed_own)+f" {fac*ed_own}")
# print("AzAlt = "+f" {aa[0]} / {aa[1]}")
# print("")
# def ghtest():
# import pygeodesy
# from pygeodesy.ellipsoidalKarney import LatLon
# ginterpolator = pygeodesy.GeoidKarney("./assets/geoids/egm2008-5.pgm")
# # Make an example location
# lat=51.416422
# lon=-116.217151
# if geoid_height == None:
# geoid_height = GeoidHeight()
# h2 = geoid_height.get(lat, lon)
# # Get the geoid height
# single_position=LatLon(lat, lon)
# h1 = ginterpolator(single_position)
# print(h1)
# print(h2)
Reference in New Issue View Git Blame Copy Permalink