-
Notifications
You must be signed in to change notification settings - Fork 12
/
land2.py
381 lines (333 loc) · 17 KB
/
land2.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
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
import math
from props import getNode
from auracore import wgs84
import comms.events
import control.route
from mission.task.task import Task
d2r = math.pi / 180.0
r2d = 180.0 / math.pi
ft2m = 0.3048
m2ft = 1.0 / ft2m
class Land(Task):
def __init__(self, config_node):
Task.__init__(self)
self.task_node = getNode("/task", True)
self.home_node = getNode("/task/home", True)
self.land_node = getNode("/task/land", True)
self.circle_node = getNode("/task/circle", True)
self.route_node = getNode("/task/route", True)
self.ap_node = getNode("/autopilot", True)
self.nav_node = getNode("/navigation", True)
self.pos_node = getNode("/position", True)
self.vel_node = getNode("/velocity", True)
self.orient_node = getNode("/orientation", True)
self.flight_node = getNode("/controls/flight", True)
self.engine_node = getNode("/controls/engine", True)
self.imu_node = getNode("/sensors/imu", True)
self.targets_node = getNode("/autopilot/targets", True)
self.pilot_node = getNode("/sensors/pilot_input", True)
# get task configuration parameters
self.name = config_node.getString("name")
self.nickname = config_node.getString("nickname")
self.lateral_offset_m = config_node.getFloat("lateral_offset_m")
self.glideslope_deg = config_node.getFloat("glideslope_deg")
if self.glideslope_deg < 0.01:
self.glideslope_deg = 6.0
self.turn_radius_m = config_node.getFloat("turn_radius_m")
if self.turn_radius_m < 1.0:
self.turn_radius_m = 75.0
self.direction = config_node.getString("direction")
if self.direction == "":
self.direction = "left"
self.extend_final_leg_m = config_node.getFloat("extend_final_leg_m")
self.alt_bias_ft = config_node.getFloat("alt_bias_ft")
self.approach_speed_kt = config_node.getFloat("approach_speed_kt")
if self.approach_speed_kt < 0.1:
self.approach_speed_kt = 25.0
self.flare_pitch_deg = config_node.getFloat("flare_pitch_deg")
self.flare_seconds = config_node.getFloat("flare_seconds")
if self.flare_seconds < 0.1:
self.flare_seconds = 5.0
if config_node.hasChild("flaps"):
self.flaps = config_node.getFloat("flaps")
else:
self.flaps = 0.0
# copy to /task/land
self.land_node.setFloat("lateral_offset_m", self.lateral_offset_m)
self.land_node.setFloat("glideslope_deg", self.glideslope_deg)
self.land_node.setFloat("turn_radius_m", self.turn_radius_m)
self.land_node.setString("direction", self.direction)
self.land_node.setFloat("extend_final_leg_m", self.extend_final_leg_m)
self.land_node.setFloat("altitude_bias_ft", self.alt_bias_ft)
self.land_node.setFloat("approach_speed_kt", self.approach_speed_kt)
self.land_node.setFloat("flare_pitch_deg", self.flare_pitch_deg)
self.land_node.setFloat("flare_seconds", self.flare_seconds)
self.side = -1.0
self.flare = False
self.flare_start_time = 0.0
self.approach_throttle = 0.0
self.approach_pitch = 0.0
self.flare_pitch_range = 0.0
self.final_heading_deg = 0.0
self.final_leg_m = 0.0
self.dist_rem_m = 0.0
self.circle_capture = False
self.gs_capture = False
self.saved_fcs_mode = ""
self.saved_nav_mode = ""
self.saved_agl_ft = 0.0
self.saved_speed_kt = 0.0
def activate(self):
if not self.active:
# build the approach with the current property tree values
self.build_approach()
# Save existing state
self.saved_fcs_mode = self.ap_node.getString("mode")
self.saved_nav_mode = self.nav_node.getString("mode")
self.saved_agl_ft = self.targets_node.getFloat("altitude_agl_ft")
self.saved_speed_kt = self.targets_node.getFloat("airspeed_kt")
self.ap_node.setString("mode", "basic+alt+speed")
self.nav_node.setString("mode", "circle")
self.targets_node.setFloat("airspeed_kt",
self.land_node.getFloat("approach_speed_kt"))
self.flight_node.setFloat("flaps_setpoint", self.flaps)
# start at the beginning of the route (in case we inherit a
# partially flown approach from earlier in the flight)
# approach_mgr.restart() # FIXME
self.circle_capture = False
self.gs_capture = False
self.flare = False
self.active = True
comms.events.log("mission", "land")
def cart2polar(self, x, y):
# fixme: if display_on:
# printf("approach %0f %0f\n", x, y);
dist = math.sqrt(x*x + y*y)
deg = math.atan2(x, y) * r2d
return (dist, deg)
def polar2cart(self, deg, dist):
x = dist * math.sin(deg * d2r)
y = dist * math.cos(deg * d2r)
return (x, y)
def update(self, dt):
if not self.active:
return False
self.glideslope_rad = self.land_node.getFloat("glideslope_deg") * d2r
self.extend_final_leg_m = self.land_node.getFloat("extend_final_leg_m")
self.alt_bias_ft = self.land_node.getFloat("altitude_bias_ft")
# add ability for pilot to bias the glideslope altitude using
# stick/elevator (negative elevator is up.)
self.alt_bias_ft += -self.pilot_node.getFloat("elevator") * 25.0
# compute minimum 'safe' altitude
safe_dist_m = math.pi * self.turn_radius_m + self.final_leg_m
safe_alt_ft = safe_dist_m * math.tan(self.glideslope_rad) * m2ft \
+ self.alt_bias_ft
# position on circle descent
circle_pos = 0
mode = self.nav_node.getString('mode')
if mode == 'circle':
# circle descent portion of the approach
pos_lon = self.pos_node.getFloat("longitude_deg")
pos_lat = self.pos_node.getFloat("latitude_deg")
center_lon = self.circle_node.getFloat("longitude_deg")
center_lat = self.circle_node.getFloat("latitude_deg")
# compute course and distance to center of target circle
(course_deg, rev_deg, cur_dist_m) = \
wgs84.geo_inverse( center_lat, center_lon, pos_lat, pos_lon )
# test for circle capture
if not self.circle_capture:
fraction = abs(cur_dist_m / self.turn_radius_m)
#print 'heading to circle:', err, fraction
if fraction > 0.75 and fraction < 1.25:
# within 25% of target circle radius, call the
# circle capture
comms.events.log("land", "descent circle capture")
self.circle_capture = True
# compute portion of circle remaining to tangent point
current_crs = course_deg + self.side * 90
if current_crs > 360.0: current_crs -= 360.0
if current_crs < 0.0: current_crs += 360.0
circle_pos = current_crs - self.final_heading_deg
if circle_pos < -180.0: circle_pos += 360.0
if circle_pos > 180.0: circle_pos -= 360.0
# print 'circle_pos:', self.orient_node.getFloat('groundtrack_deg'), current_crs, self.final_heading_deg, circle_pos
angle_rem_rad = math.pi
if self.circle_capture and circle_pos > -10:
# circling, captured circle, and within 180 degrees
# towards tangent point (or just slightly passed)
angle_rem_rad = circle_pos * math.pi / 180.0
# distance to edge of circle + remaining circumference of
# circle + final approach leg
self.dist_rem_m = (cur_dist_m - self.turn_radius_m) \
+ angle_rem_rad * self.turn_radius_m \
+ self.final_leg_m
# print 'circle:', self.dist_rem_m, self.turn_radius_m, self.final_leg_m, cur_dist_m
if self.circle_capture and self.gs_capture:
# we are on the circle and on the glide slope, lets
# look for our lateral exit point
if abs(circle_pos) <= 10.0:
comms.events.log("land", "transition to final")
self.nav_node.setString("mode", "route")
else:
# on final approach
if control.route.dist_valid:
self.dist_rem_m = self.route_node.getFloat("dist_remaining_m")
# compute glideslope/target elevation
alt_m = self.dist_rem_m * math.tan(self.glideslope_rad)
# print ' ', mode, "dist = %.1f alt = %.1f" % (self.dist_rem_m, alt_m)
# Compute target altitude.
cur_alt = self.pos_node.getFloat("altitude_agl_ft")
cur_target_alt = self.targets_node.getFloat("altitude_agl_ft")
new_target_alt = alt_m * m2ft + self.alt_bias_ft
# prior to glide slope capture, never allow target altitude
# lower than safe altitude
if not self.gs_capture:
# print 'safe:', safe_alt_ft, 'new:', new_target_alt
if new_target_alt < safe_alt_ft:
new_target_alt = safe_alt_ft
# We want to avoid wasting energy needlessly gaining altitude.
# Once the approach has started, never raise the target
# altitude.
if new_target_alt > cur_target_alt:
new_target_alt = cur_target_alt
self.targets_node.setFloat("altitude_agl_ft", new_target_alt)
# compute error metrics relative to ideal glide slope
alt_error_ft = cur_alt - (alt_m * m2ft + self.alt_bias_ft)
gs_error = math.atan2(alt_error_ft * ft2m, self.dist_rem_m) * r2d
#print "alt_error_ft = %.1f" % alt_error_ft, "gs err = %.1f" % gs_error
if self.circle_capture and not self.gs_capture:
# on the circle, but haven't intercepted gs
#print 'waiting for gs intercept'
if gs_error <= 1.0 and circle_pos >= 0:
# 1 degree or less glide slope error and on the 2nd
# half of the circle, call the gs captured
comms.events.log("land", "glide slope capture")
self.gs_capture = True
# compute time to touchdown at current ground speed (assuming the
# navigation system has lined us up properly
ground_speed_ms = self.vel_node.getFloat("groundspeed_ms")
if ground_speed_ms > 0.01:
seconds_to_touchdown = self.dist_rem_m / ground_speed_ms
else:
seconds_to_touchdown = 1000.0 # lots
#print "dist_rem_m = %.1f gs = %.1f secs = %.1f" % \
# (self.dist_rem_m, ground_speed_ms, seconds_to_touchdown)
# approach_speed_kt = approach_speed_node.getFloat()
self.flare_pitch_deg = self.land_node.getFloat("flare_pitch_deg")
self.flare_seconds = self.land_node.getFloat("flare_seconds")
if seconds_to_touchdown <= self.flare_seconds and not self.flare:
# within x seconds of touchdown horizontally. Note these
# are padded numbers because we don't know the truth
# exactly ... we could easily be closer or lower or
# further or higher. Our flare strategy is to smoothly
# pull throttle to idle, while smoothly pitching to the
# target flare pitch (as configured in the task
# definition.)
comms.events.log("land", "start flare")
self.flare = True
self.flare_start_time = self.imu_node.getFloat("timestamp")
self.approach_throttle = self.engine_node.getFloat("throttle")
self.approach_pitch = self.targets_node.getFloat("pitch_deg")
self.flare_pitch_range = self.approach_pitch - self.flare_pitch_deg
self.ap_node.setString("mode", "basic")
if self.flare:
if self.flare_seconds > 0.01:
elapsed = self.imu_node.getFloat("timestamp") - self.flare_start_time
percent = elapsed / self.flare_seconds
if percent > 1.0:
percent = 1.0
self.targets_node.setFloat("pitch_deg",
self.approach_pitch
- percent * self.flare_pitch_range)
self.engine_node.setFloat("throttle",
self.approach_throttle * (1.0 - percent))
#printf("FLARE: elapsed=%.1f percent=%.2f speed=%.1f throttle=%.1f",
# elapsed, percent,
# approach_speed_kt - percent * self.flare_pitch_range,
# self.approach_throttle * (1.0 - percent))
else:
# printf("FLARE!!!!\n")
self.targets_node.setFloat("pitch_deg", self.flare_pitch_deg)
self.engine_node.setFloat("throttle", 0.0)
# if ( display_on ) {
# printf("land dist = %.0f target alt = %.0f\n",
# self.dist_rem_m, alt_m * SG_METER_TO_FEET + self.alt_bias_ft)
def is_complete(self):
return False
# Fixme: this would make more sense if we popped all the other tasks
# off the stack first, the pushed land on the stack
# ... otherwise we jump back to route or circle or whatever we
# were doing before when this finishes.
# Maybe request idle task? or pop all task and the push idle task?
if self.task_node.getBool("is_airborne"):
return False
else:
# FIXME: task is to land and we are on the ground, let's say we
# are all done.
return True;
def close(self):
# restore the previous state
self.ap_node.setString("mode", self.saved_fcs_mode)
self.nav_node.setString("mode", self.saved_nav_mode)
self.targets_node.setFloat("airspeed_kt", self.saved_speed_kt)
self.targets_node.setFloat("altitude_agl_ft", self.saved_agl_ft );
self.flight_node.setFloat("flaps_setpoint", 0.0)
self.active = False
return True
def build_approach(self):
# Setup a descending circle tangent to the final approach
# path. The touchdown point is 'home' and the final heading
# is home azimuth. The final approach route is simply two
# points. A circle decent if flown until the glideslope is
# captured and then at the correct exit point the task
# switches to route following mode. Altitude/decent is
# managed by this task.
# fetch parameters
self.turn_radius_m = self.land_node.getFloat("turn_radius_m")
self.extend_final_leg_m = self.land_node.getFloat("extend_final_leg_m")
self.lateral_offset_m = self.land_node.getFloat("lateral_offset_m")
self.side = -1.0
dir = self.land_node.getString("direction")
if dir == "left":
self.side = -1.0
elif dir == "right":
self.side = 1.0
self.final_heading_deg = self.home_node.getFloat("azimuth_deg")
# final leg length
self.final_leg_m = 2.0 * self.turn_radius_m + self.extend_final_leg_m
# compute center of decending circle
x = self.turn_radius_m * self.side - self.lateral_offset_m * self.side
y = 2 * self.turn_radius_m + self.extend_final_leg_m
(offset_dist, offset_deg) = self.cart2polar(x, -y)
circle_offset_deg = self.final_heading_deg + offset_deg
if circle_offset_deg < 0.0:
circle_offset_deg += 360.0
if circle_offset_deg > 360.0:
circle_offset_deg -= 360.0
#print "circle_offset_deg:", circle_offset_deg
(cc_lat, cc_lon, az2) = \
wgs84.geo_direct( self.home_node.getFloat("latitude_deg"),
self.home_node.getFloat("longitude_deg"),
circle_offset_deg, offset_dist )
# configure circle task
self.circle_node.setFloat('latitude_deg', cc_lat)
self.circle_node.setFloat('longitude_deg', cc_lon)
self.circle_node.setString('direction', dir)
self.circle_node.setFloat('radius_m', self.turn_radius_m)
# create and request approach route
# start of final leg point
(dist, deg) = self.cart2polar(-self.lateral_offset_m * self.side,
-self.final_leg_m)
route_request = "0,%.2f,%.2f,-" % (dist, deg)
# touchdown point
(dist, deg) = self.cart2polar(-self.lateral_offset_m * self.side, 0.0)
route_request += ",0,%.2f,%.2f,-" % (dist, deg)
# set route request and route modes
self.route_node.setString("route_request", route_request)
self.route_node.setString("start_mode", "first_wpt")
self.route_node.setString("follow_mode", "leader")
self.route_node.setString("completion_mode", "extend_last_leg")
# seed route dist_remaining_m value so it is not zero or left
# over from previous route.
self.route_node.setFloat("dist_remaining_m", self.final_leg_m)
return True