-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathassorted_lib.py
128 lines (112 loc) · 3.19 KB
/
assorted_lib.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#Assorted Library
import math
f = 1/298.257223563
Re = 6378137
'''
==============
MATH FUNCTIONS
==============
'''
def unwrapAngle(angle):
#see matlab code for this
out = angle;
while (out>math.pi):
out = out - 2*math.pi
while (out<-math.pi):
out = out + 2*math.pi
return out
def NED2LLA(NED, center):
#Convert from NED to LLA (assume altitude is ASL)
N = NED[0]
E = NED[1]
mu0 = math.radians(center[0])
l0 = math.radians(center[1])
Rn = Re / math.sqrt(1 - (2*f - math.pow(f,2))*math.pow(math.sin(mu0),2))
Rm = Rn * (1 - (2*f - math.pow(f,2)))/(1 - (2*f - math.pow(f,2))*math.pow(math.sin(mu0),2))
dmu = math.atan2(1,Rm) * N
mu = mu0 + dmu
dl = math.atan2(1,Rn*math.cos(mu))*E
l = l0 + dl
LLA = [math.degrees(mu),math.degrees(l),-NED[2]]
return LLA
def ENU2LLA(ENU, center):
#Convert from ENU to LLA (assume altitude is ASL)
N = ENU[1]
E = ENU[0]
mu0 = math.radians(center[0])
l0 = math.radians(center[1])
Rn = Re / math.sqrt(1 - (2*f - math.pow(f,2))*math.pow(math.sin(mu0),2))
Rm = Rn * (1 - (2*f - math.pow(f,2)))/(1 - (2*f - math.pow(f,2))*math.pow(math.sin(mu0),2))
dmu = math.atan2(1,Rm) * N
mu = mu0 + dmu
dl = math.atan2(1,Rn*math.cos(mu))*E
l = l0 + dl
LLA = [math.degrees(mu),math.degrees(l),-ENU[2]]
return LLA
def LLA2NED(LLA, center):
#convert from LLA to NED (assume altitude is ASL)
mu = math.radians(LLA[0])
l = math.radians(LLA[1])
mu0 = math.radians(center[0])
l0 = math.radians(center[1])
dmu = mu - mu0
dl = l - l0
Rn = Re / math.sqrt(1 - (2*f - math.pow(f,2))*math.pow(math.sin(mu0),2))
Rm = Rn * (1 - (2*f - math.pow(f,2)))/(1 - (2*f - math.pow(f,2))*math.pow(math.sin(mu0),2))
dN = dmu / (math.atan2(1,Rm))
dE = dl / (math.atan2(1,Rn*math.cos(mu0)))
NED = [dN, dE, -LLA[2]]
return NED
def LLA2ENU(LLA, center):
#convert from LLA to ENU (assume altitude is ASL)
mu = math.radians(LLA[0])
l = math.radians(LLA[1])
mu0 = math.radians(center[0])
l0 = math.radians(center[1])
dmu = mu - mu0
dl = l - l0
Rn = Re / math.sqrt(1 - (2*f - math.pow(f,2))*math.pow(math.sin(mu0),2))
Rm = Rn * (1 - (2*f - math.pow(f,2)))/(1 - (2*f - math.pow(f,2))*math.pow(math.sin(mu0),2))
dN = dmu / (math.atan2(1,Rm))
dE = dl / (math.atan2(1,Rn*math.cos(mu0)))
ENU = [dE, dN, LLA[2]]
return ENU
'''
===============
MISC FUNCTIONS
===============
def loadConfigFile(filename):
#path stuff here
#Read in lines, parse into dictionary
pass
def loadAircraftFile(dof,name):
filename = name + '_' + dof +'.ini'
if dof == '6DOF':
data = loadConfigFile(filename)
params = parseAircraftParameters(data,'6DOF')
elif dof == '1A':
data = loadConfigFile(filename)
params = parseAircraftParameters(data,'1A')
elif dof == 'DUBINS':
data = loadConfigFile(filename)
params = parseAircraftParameters(data,'DUBINS')
else:
#Error: Unknown Type
return params
def loadInitialConditionsFile():
pass
def parseAircraftParameters(data,type):
if type == '6DOF':
#Do stuff
pass
elif type == '1A':
#Do less stuff
pass
elif type == 'DUBINS':
#Do I even do stuff?
pass
else:
#Error: Type not implemented yet
pass
return parameters
'''