-
Notifications
You must be signed in to change notification settings - Fork 4
/
airplane.py
168 lines (134 loc) · 6.54 KB
/
airplane.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
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
"""Airplane Library. Contains things that fly through the sky."""
import vector
import math
class FlyingObject(object):
def __init__(self,name,position=vector.Threevec(),velocity=vector.Threevec()):
self.name = name
self.position = position
self.velocity = velocity
def getPosition(self):
"""The position of the aircraft is returned as a vector.Threevec.
The components are all in meters. The x-direction is East, the
y-direction is North, and the z-direction is altitude from sealevel."""
return self.position.copy()
def getVelocity(self):
"""The velocity of the aircraft is returned as a vector.Threevec.
The components are all in meters per second. The x-direction is
East, the y-direction is North, and the z-direction is up."""
return self.velocity.copy()
def isControllable(self):
return False
def getName(self):
"""The unique name of the aircraft is returned by this method."""
return self.name
def executeTimestep(self,deltat):
self.position += self.velocity*deltat
class ControllableAirplane(FlyingObject):
"""An airplane which is controllable using the flight control system."""
vmin = 215.0 # meters per second
vmax = 250.0 # meters per second
vcruise = 230.0 # Meters per second
alt_min = 6000.0 # meters
alt_max = 10000.0 # meters
alt_cruise = 8000.0 # meters
turn_rate = 1.5 * math.pi/180.0 # radians per second
max_tilt = 7.5 * math.pi/180.0 # radians
def __init__(self,name,position=vector.Threevec(),velocity=vector.Threevec()):
FlyingObject.__init__(self,name,position,velocity)
heading = math.pi/2.0-velocity.phi
if heading<0.0:
heading+=2.0*math.pi
# Set the command info from current parameters
self.commandHeading = heading
self.commandSpeed = abs(velocity)
self.commandAltitude = position.z
# Set the desired info from current parameters, except speed.
self.desiredHeading = heading
self.desiredSpeed = ControllableAirplane.vcruise
self.desiredAltitude = ControllableAirplane.alt_cruise
def isControllable(self):
"""This will return True if you can send commands to the plane."""
return True
def sendHeading(self,heading):
"""Send the heading you would like the plane to turn to. The plane is
limited to turn at a rate of 1.5 degrees per second. Note that the
heading is given in radians with 0.0 being North, pi/2 is East, pi
is South, and 3pi/2 is West."""
self.commandHeading = heading
def sendAltitude(self,altitude):
"""Send the altitude you would like the airplane to reach in meters.
Note that the airplane is limited to an up or down pitch of up to 7.5
degrees, which means its maximum rate of altitude change is its speed
times the sine of 7.5 degrees. The airplane is limited to altitudes
between 6,000 and 10,000 meters. If you ask it to exceed its maximum
altitude or drop below its minimum altitude, it will stay at the
closest safe altitude."""
self.commandAltitude = altitude
def sendSpeed(self,speed):
"""Send the speed you would like the airplane to travel in meters per
second. Note that the airplane is limited to speeds between 215 and
250 meters per second. If you ask for a speed outside of this range,
the airplane will travel at the closest safe speed."""
self.commandSpeed = speed
def getDesiredHeading(self):
"""The desired heading of the aircraft is returned by this method
(in radians, with 0 being North and pi/2 being East).
The airplane should resume this heading after avoiding other
aircraft."""
return self.desiredHeading
def getDesiredSpeed(self):
"""The desired speed of the aircraft is returned by this method in
meters per second. The aircraft should resume this speed after
avoiding other aircraft."""
return self.desiredSpeed
def getDesiredAltitude(self):
"""The desired altitude of the aircraft is returned by this method in
meters. The aircraft should resume this altitude after avoiding other
aircraft."""
return self.desiredAltitude
def executeTimestep(self,deltat):
"""This method is called by the simulator to advance the aircraft
through time by an amount deltat (in seconds). The aircraft tries
to acommodate the commands given to it."""
speed = self.commandSpeed
if speed>ControllableAirplane.vmax:
speed = ControllableAirplane.vmax
elif speed<ControllableAirplane.vmin:
speed = ControllableAirplane.vmin
max_delta_altitude = speed*math.sin(ControllableAirplane.max_tilt)*deltat
altitude = self.commandAltitude
if altitude>ControllableAirplane.alt_max:
altitude = ControllableAirplane.alt_max
elif altitude<ControllableAirplane.alt_min:
altitude = ControllableAirplane.alt_min
current_heading = math.pi/2.0-self.velocity.phi
# headings are between 0 and 2pi
if current_heading < 0.0:
current_heading += 2.0*math.pi
current_altitude = self.position.z
tilt_set = 0.0
delta_altitude = altitude - current_altitude
if delta_altitude > max_delta_altitude:
tilt_set = ControllableAirplane.max_tilt
elif delta_altitude < max_delta_altitude and delta_altitude > -max_delta_altitude:
tilt_set = math.asin(delta_altitude/speed/deltat)
else:
tilt_set = -ControllableAirplane.max_tilt
heading_set = current_heading
delta_heading = self.commandHeading - current_heading
# Adjust turn so it can go through 2pi
if delta_heading > math.pi:
delta_heading -= 2.0*math.pi
elif delta_heading < -math.pi:
delta_heading += 2.0*math.pi
max_delta_heading = ControllableAirplane.turn_rate*deltat
if delta_heading > max_delta_heading:
heading_set += max_delta_heading
elif delta_heading < max_delta_heading and delta_heading > -max_delta_heading:
heading_set += delta_heading
else:
heading_set -= max_delta_heading
new_velocity = vector.sphvec(speed,math.pi/2.0-tilt_set,math.pi/2.0-heading_set)
new_position = self.position+(self.velocity/2.0+new_velocity/2.0)*deltat
self.velocity = new_velocity
self.position = new_position