|
1
|
|
|
from math import sqrt, pi, sin, cos, log, atan, atan2 |
|
2
|
|
|
|
|
3
|
|
|
|
|
4
|
|
|
def deg2rad(deg): |
|
5
|
|
|
"""Converts degrees to radians""" |
|
6
|
|
|
return deg * pi / 180 |
|
7
|
|
|
|
|
8
|
|
|
|
|
9
|
|
|
def rad2deg(rad): |
|
10
|
|
|
"""Converts radians to degrees""" |
|
11
|
|
|
return rad * 180 / pi |
|
12
|
|
|
|
|
13
|
|
|
|
|
14
|
|
|
class WGS84: |
|
15
|
|
|
"""General parameters defined by the WGS84 system""" |
|
16
|
|
|
#Semimajor axis length (m) |
|
17
|
|
|
a = 6378137.0 |
|
18
|
|
|
#Semiminor axis length (m) |
|
19
|
|
|
b = 6356752.3142 |
|
20
|
|
|
#Ellipsoid flatness (unitless) |
|
21
|
|
|
f = (a - b) / a |
|
22
|
|
|
#Eccentricity (unitless) |
|
23
|
|
|
e = sqrt(f * (2 - f)) |
|
24
|
|
|
#Speed of light (m/s) |
|
25
|
|
|
c = 299792458. |
|
26
|
|
|
#Relativistic constant |
|
27
|
|
|
F = -4.442807633e-10 |
|
28
|
|
|
#Earth's universal gravitational constant |
|
29
|
|
|
mu = 3.986005e14 |
|
30
|
|
|
#Earth rotation rate (rad/s) |
|
31
|
|
|
omega_ie = 7.2921151467e-5 |
|
32
|
|
|
|
|
33
|
|
|
def g0(self, L): |
|
34
|
|
|
"""acceleration due to gravity at the elipsoid surface at latitude L""" |
|
35
|
|
|
return 9.7803267715 * (1 + 0.001931851353 * sin(L)**2) / \ |
|
36
|
|
|
sqrt(1 - 0.0066943800229 * sin(L)**2) |
|
37
|
|
|
|
|
38
|
|
|
|
|
39
|
|
|
class GPS: |
|
40
|
|
|
"""Working class for GPS module""" |
|
41
|
|
|
wgs84 = WGS84() |
|
42
|
|
|
fGPS = 1023 |
|
43
|
|
|
fL1 = fGPS * 1.54e6 |
|
44
|
|
|
fL2 = fGPS * 1.2e6 |
|
45
|
|
|
|
|
46
|
|
|
def lla2ecef(self, lla): |
|
47
|
|
|
"""Convert lat, lon, alt to Earth-centered, Earth-fixed coordinates. |
|
48
|
|
|
Input: lla - (lat, lon, alt) in (decimal degrees, decimal degees, m) |
|
49
|
|
|
Output: ecef - (x, y, z) in (m, m, m) |
|
50
|
|
|
""" |
|
51
|
|
|
#Decompose the input |
|
52
|
|
|
lat = deg2rad(lla[0]) |
|
53
|
|
|
lon = deg2rad(lla[1]) |
|
54
|
|
|
alt = lla[2] |
|
55
|
|
|
#Calculate length of the normal to the ellipsoid |
|
56
|
|
|
N = self.wgs84.a / sqrt(1 - (self.wgs84.e * sin(lat))**2) |
|
57
|
|
|
#Calculate ecef coordinates |
|
58
|
|
|
x = (N + alt) * cos(lat) * cos(lon) |
|
59
|
|
|
y = (N + alt) * cos(lat) * sin(lon) |
|
60
|
|
|
z = (N * (1 - self.wgs84.e**2) + alt) * sin(lat) |
|
61
|
|
|
#Return the ecef coordinates |
|
62
|
|
|
return (x, y, z) |
|
63
|
|
|
|
|
64
|
|
|
def ecef2lla(self, ecef, tolerance=1e-9): |
|
65
|
|
|
"""Convert Earth-centered, Earth-fixed coordinates to lat, lon, alt. |
|
66
|
|
|
Input: ecef - (x, y, z) in (m, m, m) |
|
67
|
|
|
Output: lla - (lat, lon, alt) in (decimal degrees, decimal degrees, m) |
|
68
|
|
|
""" |
|
69
|
|
|
#Decompose the input |
|
70
|
|
|
x = ecef[0] |
|
71
|
|
|
y = ecef[1] |
|
72
|
|
|
z = ecef[2] |
|
73
|
|
|
#Calculate lon |
|
74
|
|
|
lon = atan2(y, x) |
|
75
|
|
|
#Initialize the variables to calculate lat and alt |
|
76
|
|
|
alt = 0 |
|
77
|
|
|
N = self.wgs84.a |
|
78
|
|
|
p = sqrt(x**2 + y**2) |
|
79
|
|
|
lat = 0 |
|
80
|
|
|
previousLat = 90 |
|
81
|
|
|
#Iterate until tolerance is reached |
|
82
|
|
|
while abs(lat - previousLat) >= tolerance: |
|
83
|
|
|
previousLat = lat |
|
84
|
|
|
sinLat = z / (N * (1 - self.wgs84.e**2) + alt) |
|
85
|
|
|
lat = atan((z + self.wgs84.e**2 * N * sinLat) / p) |
|
86
|
|
|
N = self.wgs84.a / sqrt(1 - (self.wgs84.e * sinLat)**2) |
|
87
|
|
|
alt = p / cos(lat) - N |
|
88
|
|
|
#Return the lla coordinates |
|
89
|
|
|
return (rad2deg(lat), rad2deg(lon), alt) |
|
90
|
|
|
|
bartromgens /
nsmaps
