-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathlander.py
132 lines (100 loc) · 4.23 KB
/
lander.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
129
130
131
132
from math_utils import *
class lander():
def __init__(self, model, pos, vel, orient, ang_vel, dry_mass,
prop_mass, torque, thrust, max_thrust, min_thrust,
main_engine, mass_flow, landing_offset):
self.model = model
self.pos = pos
self.vel = vel
self.accel = [0,0,0]
self.orient = orient
self.ang_vel = ang_vel
self.dry_mass = dry_mass
self.prop_mass = prop_mass
self.torque = torque
self.thrust = thrust
self.max_thrust = max_thrust
self.min_thrust = min_thrust
self.main_engine = main_engine
self.mass_flow = mass_flow
self.landing_offset = landing_offset
def get_pos(self):
return self.pos
def get_vel(self):
return self.vel
def get_accel(self):
return self.accel
def get_orient(self):
return self.orient
def get_thrust(self):
return self.thrust
def get_max_thrust(self):
return self.max_thrust
def get_percent_thrust(self):
return (self.thrust / self.max_thrust) * 100
def get_ang_vel(self):
return self.ang_vel
def get_prop_mass(self):
return self.prop_mass
def toggle_main_engine(self):
self.main_engine = not self.main_engine
def get_main_engine(self):
return self.main_engine
def get_main_engine_str(self):
if self.main_engine:
return "Running"
else:
return "Shut down"
def get_mass(self):
return self.dry_mass + self.prop_mass
def update_ang_vel(self, rcs, dt):
self.ang_vel = [self.ang_vel[0] + int(rcs[0]) * dt * self.torque[0],
self.ang_vel[1] + int(rcs[1]) * dt * self.torque[1],
self.ang_vel[2] + int(rcs[2]) * dt * self.torque[2]]
def get_accel(self):
return self.accel
# we need to clamp thrust values between min and max thrust, obviously
def update_thrust(self, d_thrust, dt):
d_thrust = max(min(self.max_thrust * 0.15 * dt, d_thrust), -self.max_thrust * 0.15 * dt)
self.thrust = min(max(self.thrust + d_thrust, self.min_thrust), self.max_thrust)
def set_thrust(self, thrust):
self.thrust = min(max(thrust, self.min_thrust), self.max_thrust)
def set_percent_thrust(self, thrust_percent):
self.thrust = min(max(thrust_percent * self.max_thrust, self.min_thrust), self.max_thrust)
def update_orient(self, dt):
self.orient = rotate_matrix(self.orient, vector_scale(self.ang_vel, dt))
# there is only gravity and main engine
# that apply linear acceleration
def update_accel(self, dt):
self.accel = [0, -lunar_gravity, 0]
if self.main_engine and self.prop_mass:
thrust_vector = vector_scale(self.orient[1], self.thrust)
thrust_accel = vector_scale(thrust_vector, 1/(self.dry_mass + self.prop_mass))
self.accel = vector_add(self.accel, thrust_accel)
elif not self.prop_mass and self.main_engine:
self.toggle_main_engine()
def update_vel(self, dt):
accel_dt = vector_scale(self.accel, dt)
self.vel = vector_add(self.vel, accel_dt)
def update_pos(self, dt):
vel_dt = vector_scale(self.vel, dt)
self.pos = vector_add(self.pos, vel_dt)
def update_mass(self, dt):
if self.main_engine and self.prop_mass > 0:
self.prop_mass -= (self.thrust/self.max_thrust) * self.mass_flow * dt
elif self.prop_mass < 0:
self.prop_mass = 0
def update_physics(self, dt):
self.update_accel(dt)
self.update_vel(dt)
self.update_pos(dt)
self.update_orient(dt)
self.update_mass(dt)
def get_landing_tag_pos(self):
landing_tag_pos = numpy.matmul(numpy.array([0, self.landing_offset, 0]), self.orient).tolist()
landing_tag_pos = vector_add(landing_tag_pos, self.pos)
return landing_tag_pos
def get_alt(self, terrain):
return self.get_landing_tag_pos()[1] - terrain.get_height_at_pos([self.get_pos()[0], self.get_pos()[2]])
def get_alt_quick(self, terrain):
return self.get_landing_tag_pos()[1] - terrain.estimate_height_at_pos([self.get_pos()[0], self.get_pos()[2]])