def getwpidx(self, txt, reflat=999999., reflon=999999):
"""Get waypoint index to access data"""
name = txt.upper()
try:
i = self.wpid.index(name)
except:
return -1
# if no pos is specified, get first occurence
if not reflat < 99999.:
return i
# If pos is specified check for more and return closest
else:
idx = []
idx.append(i)
found = True
while i < len(self.wpid) - 1 and found:
try:
i = self.wpid.index(name, i + 1)
idx.append(i)
except:
found = False
if len(idx) == 1:
return idx[0]
else:
imin = idx[0]
dmin = geo.kwikdist(reflat, reflon, self.wplat[imin], self.wplon[imin])
for i in idx[1:]:
d = geo.kwikdist(reflat, reflon, self.wplat[i], self.wplon[i])
if d < dmin:
imin = i
dmin = d
return imin
def listconnections(self, wpid,wplat,wplon):
# Return list of connecting airway legs
connect = []
# Check from-list first
if wpid in self.awfromwpid:
idx = findall(self.awfromwpid,wpid)
for i in idx:
newitem = [self.awid[i],self.awtowpid[i]]
if (newitem not in connect) and \
geo.kwikdist(self.awfromlat[i],self.awfromlon[i],
wplat,wplon) < 10.:
connect.append(newitem)
# Check to-list nextt
if wpid in self.awtowpid:
idx = findall(self.awtowpid,wpid)
for i in idx:
newitem = [self.awid[i],self.awfromwpid[i]]
if (newitem not in connect) and \
geo.kwikdist(self.awtolat[i],self.awtolon[i],
wplat,wplon) < 10.:
connect.append(newitem)
return connect # return list of [awid,wpid]
def getwpidx(self, txt, reflat=999999., reflon=999999):
"""Get waypoint index to access data"""
name = txt.upper()
try:
i = self.wpid.index(name)
except:
return -1
# if no pos is specified, get first occurence
if not reflat < 99999.:
return i
# If pos is specified check for more and return closest
else:
idx = []
idx.append(i)
found = True
while i < len(self.wpid) - 1 and found:
try:
i = self.wpid.index(name, i + 1)
idx.append(i)
except:
found = False
if len(idx) == 1:
return idx[0]
else:
imin = idx[0]
dmin = geo.kwikdist(reflat, reflon, self.wplat[imin], self.wplon[imin])
for i in idx[1:]:
d = geo.kwikdist(reflat, reflon, self.wplat[i], self.wplon[i])
if d < dmin:
imin = i
dmin = d
return imin
def listconnections(self, wpid, wplat, wplon):
# Return list of connecting airway legs
connect = []
# Check from-list first
if wpid in self.awfromwpid:
idx = findall(self.awfromwpid, wpid)
for i in idx:
newitem = [self.awid[i], self.awtowpid[i]]
if (newitem not in connect) and \
geo.kwikdist(self.awfromlat[i],self.awfromlon[i],
wplat,wplon) < 10.:
connect.append(newitem)
# Check to-list nextt
if wpid in self.awtowpid:
idx = findall(self.awtowpid, wpid)
for i in idx:
newitem = [self.awid[i], self.awfromwpid[i]]
if (newitem not in connect) and \
geo.kwikdist(self.awtolat[i],self.awtolon[i],
wplat,wplon) < 10.:
connect.append(newitem)
return connect # return list of [awid,wpid]
def getwpindices(self, txt, reflat=999999., reflon=999999, crit=1852.0):
"""Get waypoint index to access data"""
name = txt.upper()
try:
i = self.wpid.index(name)
except:
return [-1]
# if no pos is specified, get first occurence
if not reflat < 99999.:
return [i]
# If pos is specified check for more and return closest
else:
idx = findall(self.wpid, name) # find indices of al occurences
if len(idx) == 1:
return [idx[0]]
else:
imin = idx[0]
dmin = geo.kwikdist(reflat, reflon, self.wplat[imin],
self.wplon[imin])
for i in idx[1:]:
d = geo.kwikdist(reflat, reflon, self.wplat[i],
self.wplon[i])
if d < dmin:
imin = i
dmin = d
# Find co-located
indices = [imin]
for i in idx:
if i != imin:
dist = nm*geo.kwikdist(self.wplat[i], self.wplon[i], \
self.wplat[imin], self.wplon[imin])
if dist <= crit:
indices.append(i)
return indices
def getwpindices(self, txt, reflat=999999., reflon=999999,crit=1852.0):
"""Get waypoint index to access data"""
name = txt.upper()
try:
i = self.wpid.index(name)
except:
return [-1]
# if no pos is specified, get first occurence
if not reflat < 99999.:
return [i]
# If pos is specified check for more and return closest
else:
idx = findall(self.wpid,name) # find indices of al occurences
if len(idx) == 1:
return [idx[0]]
else:
imin = idx[0]
dmin = geo.kwikdist(reflat, reflon, self.wplat[imin], self.wplon[imin])
for i in idx[1:]:
d = geo.kwikdist(reflat, reflon, self.wplat[i], self.wplon[i])
if d < dmin:
imin = i
dmin = d
# Find co-located
indices = [imin]
for i in idx:
if i!=imin:
dist = nm*geo.kwikdist(self.wplat[i], self.wplon[i], \
self.wplat[imin], self.wplon[imin])
if dist<=crit:
indices.append(i)
return indices
def update_reslos(self):
# Update timer
self.duration += settings.simdt
# Retrieve traf idx
idx1 = traf.id2idx(self.ac1)
idx2 = traf.id2idx(self.ac2)
# Retrieve lat, lon coordinates of traffic
lat1, lon1 = traf.lat[idx1], traf.lon[idx1]
lat2, lon2 = traf.lat[idx2], traf.lon[idx2]
# Compute current distance between aircraft and save
dist = kwikdist(lat1, lon1, lat2, lon2) * nm #distance in meters
self.distance = np.append(self.distance, dist)
def update():
global first_time, done, delete_dict, done_count
if first_time:
done = dict.fromkeys(traf.id)
delete_dict = dict.fromkeys(traf.id, False)
first_time = False
for id in traf.id:
if id not in done:
done[id] = False
if id not in delete_dict:
delete_dict[id] = False
dest_lat_lon = [(52.6, 4.73), (52.3, 4.36), (52.33, 5.19), (51.52, 5.33),
(51.8, 5.06), (51.82, 5.75), (52.30, 6.0)]
# dest_lat_lon[-dest]
if done.keys():
for agent_id in done.keys():
# Initialize reward to 0.
done[agent_id] = False
# First check if goal area is reached
idx = traf.id2idx(agent_id)
dest = traf.dest_temp[idx]
dest_lat, dest_lon = dest_lat_lon[-dest]
dist = geo.kwikdist(traf.lat[idx], traf.lon[idx], dest_lat,
dest_lon)
if dist <= 5:
done[agent_id] = True
done_count += 1
for agent_id in done.keys():
if agent_id in delete_dict:
if done[agent_id] and not delete_dict[agent_id]:
traf.delete(traf.id2idx(agent_id))
print('Deleted ', agent_id)
delete_dict[agent_id] = True
save_metrics()
if done_count >= 125:
sim.reset()
return
def checkInside(self, lat, lon, alt):
distance = kwikdist(self.clat, self.clon, lat, lon) # [NM]
inside = (distance <= self.r) & (self.bottom <= alt) & (alt <=
self.top)
return inside
def update_fms(self, qdr, dist):
# Shift waypoints for aircraft i where necessary
for i in bs.traf.actwp.Reached(qdr, dist, bs.traf.actwp.flyby,
bs.traf.actwp.flyturn,bs.traf.actwp.turnrad):
# Save current wp speed for use on next leg when we pass this waypoint
# VNAV speeds are always FROM-speed, so we accelerate/decellerate at the waypoint
# where this speed is specified, so we need to save it for use now
# before getting the new data for the next waypoint
# Get speed for next leg from the waypoint we pass now
bs.traf.actwp.spd[i] = bs.traf.actwp.nextspd[i]
bs.traf.actwp.spdcon[i] = bs.traf.actwp.nextspd[i]
# Use turnradius of passing wp for bank angle
if bs.traf.actwp.flyturn[i] and bs.traf.actwp.turnrad[i]>0.:
if bs.traf.actwp.turnspd[i]>0.:
turnspd = bs.traf.actwp.turnspd[i]
else:
turnspd = bs.traf.tas[i]
bs.traf.aphi[i] = atan(turnspd*turnspd/(bs.traf.actwp.turnrad[i]*nm*g0)) # [rad]
else:
bs.traf.aphi[i] = 0.0 #[rad] or leave untouched???
# Get next wp (lnavon = False if no more waypoints)
lat, lon, alt, bs.traf.actwp.nextspd[i], bs.traf.actwp.xtoalt[i], toalt, \
bs.traf.actwp.xtorta[i], bs.traf.actwp.torta[i], \
lnavon, flyby, flyturn, turnrad, turnspd,\
bs.traf.actwp.next_qdr[i] = \
self.route[i].getnextwp() # note: xtoalt,toalt in [m]
# End of route/no more waypoints: switch off LNAV using the lnavon
# switch returned by getnextwp
if not lnavon and bs.traf.swlnav[i]:
bs.traf.swlnav[i] = False
# Last wp: copy last wp values for alt and speed in autopilot
if bs.traf.swvnavspd[i] and bs.traf.actwp.nextspd[i]>= 0.0:
bs.traf.selspd[i] = bs.traf.actwp.nextspd[i]
# In case of no LNAV, do not allow VNAV mode on its own
bs.traf.swvnav[i] = bs.traf.swvnav[i] and bs.traf.swlnav[i]
bs.traf.actwp.lat[i] = lat # [deg]
bs.traf.actwp.lon[i] = lon # [deg]
# 1.0 in case of fly by, else fly over
bs.traf.actwp.flyby[i] = int(flyby)
# User has entered an altitude for this waypoint
if alt >= -0.01:
bs.traf.actwp.nextaltco[i] = alt # [m]
if not bs.traf.swlnav[i]:
bs.traf.actwp.spd[i] = -999.
# VNAV spd mode: use speed of this waypoint as commanded speed
# while passing waypoint and save next speed for passing next wp
# Speed is now from speed! Next speed is ready in wpdata
if bs.traf.swvnavspd[i] and bs.traf.actwp.spd[i]> 0.0:
bs.traf.selspd[i] = bs.traf.actwp.spd[i]
# Update qdr and turndist for this new waypoint for ComputeVNAV
qdr[i], distnmi = geo.qdrdist(bs.traf.lat[i], bs.traf.lon[i],
bs.traf.actwp.lat[i], bs.traf.actwp.lon[i])
dist[i] = distnmi*nm
# Update turndist so ComputeVNAV works, is there a next leg direction or not?
if bs.traf.actwp.next_qdr[i] < -900.:
local_next_qdr = qdr[i]
else:
local_next_qdr = bs.traf.actwp.next_qdr[i]
# Get flyturn switches and data
bs.traf.actwp.flyturn[i] = flyturn
bs.traf.actwp.turnrad[i] = turnrad
# Pass on whether currently flyturn mode:
# at beginning of leg,c copy tonextwp to lastwp
# set next turn False
bs.traf.actwp.turnfromlastwp[i] = bs.traf.actwp.turntonextwp[i]
bs.traf.actwp.turntonextwp[i] = False
# Keep both turning speeds: turn to leg and turn from leg
bs.traf.actwp.oldturnspd[i] = bs.traf.actwp.turnspd[i] # old turnspd, turning by this waypoint
if bs.traf.actwp.flyturn[i]:
bs.traf.actwp.turnspd[i] = turnspd # new turnspd, turning by next waypoint
else:
bs.traf.actwp.turnspd[i] = -990.
# Calculate turn dist (and radius which we do not use) now for scalar variable [i]
bs.traf.actwp.turndist[i], dummy = \
bs.traf.actwp.calcturn(bs.traf.tas[i], bs.traf.bank[i],
qdr[i], local_next_qdr,turnrad) # update turn distance for VNAV
# Reduce turn dist for reduced turnspd
if bs.traf.actwp.flyturn[i] and bs.traf.actwp.turnrad[i]<0.0 and bs.traf.actwp.turnspd[i]>0.:
turntas = cas2tas(bs.traf.actwp.turnspd[i], bs.traf.alt[i])
bs.traf.actwp.turndist[i] = bs.traf.actwp.turndist[i]*turntas*turntas/(bs.traf.tas[i]*bs.traf.tas[i])
# VNAV = FMS ALT/SPD mode incl. RTA
self.ComputeVNAV(i, toalt, bs.traf.actwp.xtoalt[i], bs.traf.actwp.torta[i],
bs.traf.actwp.xtorta[i])
# End of per waypoint i switching loop
# Update qdr2wp with up-to-date qdr, now that we have checked passing wp
self.qdr2wp = qdr%360.
# Continuous guidance when speed constraint on active leg is in update-method
# If still an RTA in the route and currently no speed constraint
for iac in np.where((bs.traf.actwp.torta > -99.)*(bs.traf.actwp.spdcon<0.0))[0]:
iwp = bs.traf.ap.route[iac].iactwp
if bs.traf.ap.route[iac].wprta[iwp]>-99.:
# For all a/c flying to an RTA waypoint, recalculate speed more often
dist2go4rta = geo.kwikdist(bs.traf.lat[iac],bs.traf.lon[iac], \
bs.traf.actwp.lat[iac],bs.traf.actwp.lon[iac])*nm \
+ bs.traf.ap.route[iac].wpxtorta[iwp] # last term zero for active wp rta
# Set bs.traf.actwp.spd to rta speed, if necessary
self.setspeedforRTA(iac,bs.traf.actwp.torta[iac],dist2go4rta)
# If VNAV speed is on (by default coupled to VNAV), use it for speed guidance
if bs.traf.swvnavspd[iac]:
bs.traf.selspd[iac] = bs.traf.actwp.spd[iac]
def ComputeVNAV(self, idx, toalt, xtoalt, torta, xtorta):
"""
This function to do VNAV (and RTA) calculations is only called only once per leg.
If:
- switching to next waypoint
- when VNAV is activated
- when a DIRECT is given
It prepares the profile of this leg using the the current altitude and the next altitude constraint (nextaltco).
The distance to the next altitude constraint is given by xtoalt [m] after active waypoint.
Options are (classic VNAV logic, swtoc and swtod True):
- no altitude constraint in the future, do nothing
- Top of CLimb logic (swtoc=True): if next altitude constrain is baove us, climb as soon as possible with default steepness
- Top of Descent Logic (swtod =True) Use ToD logic: descend as late aspossible, based on
steepness. Prepare a ToD somewhere on the leg if necessary based on distance to next altitude constraint.
This is done by calculating distance to next waypoint where descent should start
Alternative logic (e.g. for UAVs or GA):
- swtoc=False and next alt co is above us, climb with the angle/steepness needed to arrive at the altitude at
the waypoint with the altitude constraint (xtoalt m after active waypoint)
- swtod=False and next altco is below us, descend with the angle/steepness needed to arrive at at the altitude at
the waypoint with the altitude constraint (xtoalt m after active waypoint)
Output if this function:
self.dist2vs = distance 2 next waypoint where climb/descent needs to activated
bs.traf.actwp.vs = V/S to be used during climb/descent part, so when dist2wp<dist2vs [m] (to next waypoint)
"""
#print ("ComputeVNAV for",bs.traf.id[idx],":",toalt/ft,"ft ",xtoalt/nm,"nm")
#print("Called by",callstack()[1].function)
# Check whether active waypoint speed needs to be adjusted for RTA
# sets bs.traf.actwp.spd, if necessary
# debug print("xtorta+legdist =",(xtorta+legdist)/nm)
self.setspeedforRTA(idx, torta,
xtorta + self.dist2wp[idx]) # all scalar
# Check if there is a target altitude and VNAV is on, else return doing nothing
if toalt < 0 or not bs.traf.swvnav[idx]:
self.dist2vs[
idx] = -999999. #dist to next wp will never be less than this, so VNAV will do nothing
return
# So: somewhere there is an altitude constraint ahead
# Compute proper values for bs.traf.actwp.nextaltco, self.dist2vs, self.alt, bs.traf.actwp.vs
# Descent VNAV mode (T/D logic)
#
# xtoalt = distance to go to next altitude constraint at a waypoint in the route
# (could be beyond next waypoint) [m]
#
# toalt = altitude at next waypoint with an altitude constraint
#
# dist2vs = autopilot starts climb or descent when the remaining distance to next waypoint
# is this distance
#
#
# VNAV Guidance principle:
#
#
# T/C------X---T/D
# / . \
# / . \
# T/C----X----.-----X . .\
# / . . . \
# / . . . X---T/D
# /. . . . \
# / . . . . \
# / . . . . .\
# pos x x x x x X
#
#
# X = waypoint with alt constraint x = Wp without prescribed altitude
#
# - Ignore and look beyond waypoints without an altitude constraint
# - Climb as soon as possible after previous altitude constraint
# and climb as fast as possible, so arriving at alt earlier is ok
# - Descend at the latest when necessary for next altitude constraint
# which can be many waypoints beyond current actual waypoint
epsalt = 2. * ft # deadzone
if bs.traf.alt[idx] > toalt + epsalt:
# Stop potential current climb (e.g. due to not making it to previous altco)
# then stop immediately, as in: do not make it worse.
if bs.traf.vs[idx] > 0.0001:
self.vnavvs[idx] = 0.0
self.alt = bs.traf.alt[idx]
if bs.traf.swvnav[idx]:
bs.traf.selalt[idx] = bs.traf.alt[idx]
# Descent modes: VNAV (= swtod/Top of Descent logic) or aiming at next alt constraint
# Calculate max allowed altitude at next wp (above toalt)
bs.traf.actwp.nextaltco[idx] = toalt # [m] next alt constraint
bs.traf.actwp.xtoalt[
idx] = xtoalt # [m] distance to next alt constraint measured from next waypoint
# VNAV ToD logic
if self.swtod[idx]:
# Get distance to waypoint
self.dist2wp[idx] = nm * geo.kwikdist(
bs.traf.lat[idx], bs.traf.lon[idx], bs.traf.actwp.lat[idx],
bs.traf.actwp.lon[idx]
) # was not always up to date, so update first
# Distance to next waypoint where we need to start descent (top of descent) [m]
descdist = abs(
bs.traf.alt[idx] -
toalt) / self.steepness # [m] required length for descent
self.dist2vs[
idx] = descdist - xtoalt # [m] part of that length on this leg
#print(bs.traf.id[idx],"traf.alt =",bs.traf.alt[idx]/ft,"ft toalt = ",toalt/ft,"ft descdist =",descdist/nm,"nm")
#print ("d2wp = ",self.dist2wp[idx]/nm,"nm d2vs = ",self.dist2vs[idx]/nm,"nm")
#print("xtoalt =",xtoalt/nm,"nm descdist =",descdist/nm,"nm")
# Exceptions: Descend now? Or never on this leg?
if self.dist2wp[idx] < self.dist2vs[
idx]: # Urgent descent, we're late![m]
# Descend now using whole remaining distance on leg to reach altitude
self.alt[idx] = bs.traf.actwp.nextaltco[
idx] # dial in altitude of next waypoint as calculated
t2go = self.dist2wp[idx] / max(0.01, bs.traf.gs[idx])
bs.traf.actwp.vs[idx] = (bs.traf.alt[idx] - toalt) / max(
0.01, t2go)
elif xtoalt < descdist: # Not even descending is needed at next waypoint
# Top of decent needs to be on this leg, as next wp is in descent
bs.traf.actwp.vs[idx] = -abs(self.steepness) * (
bs.traf.gs[idx] +
(bs.traf.gs[idx] < 0.2 * bs.traf.tas[idx]) *
bs.traf.tas[idx])
else:
# else still level
bs.traf.actwp.vs[idx] = 0.0
else:
# We are higher but swtod = False, so there is no ToD descent logic, simply aim at next altco
steepness = (bs.traf.alt[idx] -
bs.traf.actwp.nextaltco[idx]) / (max(
0.01, self.dist2wp[idx] + xtoalt))
bs.traf.actwp.vs[idx] = -abs(steepness) * (
bs.traf.gs[idx] +
(bs.traf.gs[idx] < 0.2 * bs.traf.tas[idx]) *
bs.traf.tas[idx])
# VNAV climb mode: climb as soon as possible (T/C logic)
elif bs.traf.alt[idx] < toalt - 10. * ft:
# Stop potential current descent (e.g. due to not making it to previous altco)
# then stop immediately, as in: do not make it worse.
if bs.traf.vs[idx] < -0.0001:
self.vnavvs[idx] = 0.0
self.alt = bs.traf.alt[idx]
if bs.traf.swvnav[idx]:
bs.traf.selalt[idx] = bs.traf.alt[idx]
# Altitude we want to climb to: next alt constraint in our route (could be further down the route)
bs.traf.actwp.nextaltco[idx] = toalt # [m]
bs.traf.actwp.xtoalt[
idx] = xtoalt # [m] distance to next alt constraint measured from next waypoint
self.alt[idx] = bs.traf.actwp.nextaltco[
idx] # dial in altitude of next waypoint as calculated
self.dist2vs[
idx] = 99999. #[m] Forces immediate climb as current distance to next wp will be less
t2go = max(0.1, self.dist2wp[idx] + xtoalt) / max(
0.01, bs.traf.gs[idx])
if self.swtoc[idx]:
steepness = self.steepness # default steepness
else:
steepness = (bs.traf.alt[idx] -
bs.traf.actwp.nextaltco[idx]) / (max(
0.01, self.dist2wp[idx] + xtoalt))
bs.traf.actwp.vs[idx] = np.maximum(
steepness * bs.traf.gs[idx],
(bs.traf.actwp.nextaltco[idx] - bs.traf.alt[idx]) /
t2go) # [m/s]
# Level leg: never start V/S
else:
self.dist2vs[idx] = -999. # [m]
return
def radarclick(cmdline, lat, lon, acdata=None, route=None):
"""Process lat,lon as clicked in radar window"""
# Specify which argument can be clicked, and how, in this dictionary
# and when it's the last, also add ENTER
clickcmd = {"": "acid,-",
"ADDWPT": "acid,latlon,-,-,wpinroute,-",
"AFTER": "acid,wpinroute,-",
"AT": "acid,wpinroute,-",
"ALT": "acid,-",
"AREA": "latlon,-,latlon",
"ASAS": "acid,-",
"BOX": "-,latlon,-,latlon",
"CIRCLE": "-,latlon,-,dist",
"CRE": "-,-,latlon,-,hdg,-,-",
"DEFWPT": "-,latlon,-",
"DEL": "acid,-",
"DELWPT": "acid,wpinroute,-",
"DELRTE": "acid,-",
"DEST": "acid,apt",
"DIRECT": "acid,wpinroute",
"DIST": "latlon,-,latlon",
"DUMPRTE": "acid",
"ENG": "acid,-",
"HDG": "acid,hdg",
"LINE": "-,latlon,-,latlon",
"LISTRTE": "acid,-",
"LNAV": "acid,-",
"MOVE": "acid,latlon,-,-,hdg",
"NAVDISP": "acid",
"NOM": "acid",
"ND": "acid",
"ORIG": "acid,apt",
"PAN": "latlon",
"POLY": "-,latlon,...",
"POLYALT": "-,-,-,latlon,...",
"POLYGON": "-,latlon,...",
"POS": "acid",
"SSD": "acid,...",
"SPD": "acid,-",
"TRAIL":"acid,-",
"VNAV": "acid,-",
"VS": "acid,-"
}
# Default values, when nothing is found to be added based on click
todisplay = "" # Result of click is added here
tostack = "" # If it is the last argument we will pass whole line to the stack
# The pygame version has access to the complete traffic object. This gets
# passed to radarclick in the QtGL version.
if acdata is None:
acdata = bs.traf
# Split command line into command and arguments, pass traf ids to check for
# switched acid and command
cmd, args = cmdsplit(cmdline, acdata.id)
numargs = len(args)
# -------- Process click --------
# Double click on aircraft = POS command
if numargs == 0 and acdata.id.count(cmd) > 0:
todisplay = "\n" # Clear the current command
tostack = "POS " + cmd # And send a pos command to the stack
# Insert: nearest aircraft id
else:
# Check for synonyms (dictionary is imported from stack)
if cmd in cmdsynon:
cmd = cmdsynon[cmd]
# Try to find command in clickcmd dictionary
try:
lookup = clickcmd[cmd]
except KeyError:
# When command was not found in dictionary:
# do nothing, return empty strings
return "", ""
# For valid value, insert relevant dat on edit line
if lookup:
if len(cmdline) > 0 and (cmdline[-1] != " " and cmdline[-1]!=","):
todisplay = " "
# Determine argument click type
clickargs = lookup.lower().split(",")
totargs = len(clickargs)
curarg = numargs
# Exception case: if the last item of the clickargs list is "..."
# then the one-but-last can be repeatedly added
# (e.g. for the definition of a polygon)
if clickargs[-1] == "...":
totargs = 999
curarg = min(curarg, len(clickargs) - 2)
if curarg < totargs:
clicktype = clickargs[curarg]
if clicktype == "acid":
idx = findnearest(lat, lon, acdata.lat, acdata.lon)
if idx >= 0:
todisplay += acdata.id[idx] + " "
elif clicktype == "latlon":
todisplay += str(round(lat, 6)) + "," + str(round(lon, 6)) + " "
elif clicktype == "dist":
latref, lonref = float(args[1]), float(args[2])
todisplay += str(round(geo.kwikdist(latref, lonref, lat, lon), 6))
elif clicktype == "apt":
idx = findnearest(lat, lon, bs.navdb.aptlat, bs.navdb.aptlon)
if idx >= 0:
todisplay += bs.navdb.aptid[idx] + " "
elif clicktype == "wpinroute": # Find nearest waypoint in route
if acdata.id.count(args[0]) > 0:
itraf = acdata.id.index(args[0])
synerr = False
reflat = acdata.lat[itraf]
reflon = acdata.lon[itraf]
# The pygame version can get the route directly from traf
# otherwise the route is passed to this function
if route is None:
route = acdata.ap.route[itraf]
if len(route.wplat) > 0:
iwp = findnearest(lat, lon,
array(route.wplat),
array(route.wplon))
if iwp >= 0:
todisplay += route.wpname[iwp]+" "
else:
synerr = True
elif clicktype == "hdg":
# Read start position from command line
if cmd == "CRE":
try:
reflat = float(args[2])
reflon = float(args[3])
synerr = False
except:
synerr = True
elif cmd == "MOVE":
try:
reflat = float(args[1])
reflon = float(args[2])
synerr = False
except:
synerr = True
else:
if acdata.id.count(args[0]) > 0:
idx = acdata.id.index(args[0])
reflat = acdata.lat[idx]
reflon = acdata.lon[idx]
synerr = False
else:
synerr = True
if not synerr:
dy = lat - reflat
dx = (lon - reflon) * cos(radians(reflat))
hdg = degrees(atan2(dx, dy)) % 360.
todisplay += str(int(hdg)) + " "
# Is it the last argument? (then we will insert ENTER as well)
if curarg + 1 >= totargs:
tostack = cmdline + todisplay
todisplay = todisplay + '\n'
return tostack, todisplay
def update(self, gain):
# Time step update of source
# Get time step
dt = sim.simt - self.tupdate
self.tupdate = sim.simt
# Time for a new aircraft?
if dt > 0.0:
# Calculate probability of a geberate occurring with flow
chances = 1.0 - gain * self.flow * dt / 3600. #flow is in a/c per hour=360 seconds
if random.random() >= chances:
# Runways defined? => use runway lines (queues)
if len(self.runways) > 0:
# We do not yet need to create an aircraft
gennow = False
# Find shortest line and put it in
isel = random.randint(0, len(self.runways) - 1)
self.rwyline[isel] = self.rwyline[isel] + 1
else:
# Yes we do need to generate one now
gennow = True
else:
gennow = False
# Check for generating aircraft
# First check runways for a/c already in line:
txt = ""
for i in range(len(self.runways)):
# Runway vacated and there is one waiting?
if sim.simt - self.rwytotime[
i] > self.dtakeoff and self.rwyline[i] > 0:
#if self.name == "EHAM":
# print(sim.simt, self.runways[i], self.rwytotime[i])
self.rwytotime[i] = sim.simt
self.rwyline[i] = self.rwyline[i] - 1
# Choose and aicraft type, check for distance
if len(self.dest) > 0:
idest = int(random.random() * len(self.dest))
else:
idest = -1
if idest >= 0:
acid = randacname(self.name, self.dest[idest])
if self.desttype[idest] == "seg" or self.dest[
idest][:4] == "SEGM":
lat, lon, hdg = getseg(self.dest[idest])
else:
success, posobj = txt2pos(self.dest[idest], ctrlat,
ctrlon)
lat, lon = posobj.lat, posobj.lon
distroute = latlondist(self.lat, self.lon, lat,
lon) / nm
else:
acid = randacname(self.name, self.name)
if self.destactypes[idest] == []:
actype = random.choice(self.actypes)
actype = checkactype(actype, distroute, self.actypes)
else:
actype = random.choice(self.destactypes[idest])
stack.stack("CRE " + ",".join([
acid, actype,
str(self.rwylat[i]),
str(self.rwylon[i]),
str(self.rwyhdg[i]), "0.0", "0.0"
]))
#wplat,wplon = kwikpos(self.rwylat[i],self.rwylon[i],self.rwyhdg[i],5.0*nm)
#stack.stack(acid + " ADDWPT ",wplat," ",wplon)
#stack.stack(acid+"LNAV ON")
if idest >= 0:
if self.dest[idest][:4] != "SEGM":
stack.stack(acid + " DEST " + self.dest[idest])
else:
stack.stack(acid + " DEST " +
str(self.destlat[idest]) + " " +
str(self.destlon[idest]))
if self.name[:4] != "SEGM":
stack.stack(acid + " ORIG " + self.name)
else:
stack.stack(acid + " ORIG " + str(self.lat) + " " +
str(self.lon))
stack.stack(acid + " SPD 250")
stack.stack(acid + " ALT FL100")
stack.stack(acid + " HDG " + str(self.rwyhdg[i]))
stack.stack(acid + " LNAV OFF")
# Not runway, then define instantly at position with random heading or in case of segment inward heading
if gennow:
if not self.incircle:
lat, lon = kwikpos(ctrlat, ctrlon, self.segdir, radius)
hdg = self.segdir - 180
elif self.type == "seg":
lat, lon, brg = getseg(self.name)
hdg = (brg + 180) % 360
elif self.type == "rwy":
lat, lon = self.lat, self.lon
hdg = self.hdg # Runway heading
else:
hdg = random.random() * 360.
if self.startaltmin and self.startaltmax:
alt = random.randint(int(self.startaltmin),
int(self.startaltmax))
else:
alt = random.randint(200, 300) * 100 * ft
if self.startspdmin and self.startspdmax:
spd = random.randint(int(self.startspdmin),
int(self.startspdmax))
else:
spd = random.randint(250, 350)
alttxt, spdtxt = "FL" + str(int(round(alt /
(100 * ft)))), str(spd)
# Add destination
if len(self.dest) > 0:
idest = int(random.random() * len(self.dest))
acid = randacname(self.name, self.dest[idest])
else:
acid = randacname(self.name, self.name)
idest = -1
stack.stack("CRE " + ",".join([
acid,
random.choice(self.actypes),
str(self.lat),
str(self.lon),
str(int(hdg % 360)), alttxt, spdtxt
]))
if idest >= 0:
if self.dest[idest][:4] != "SEGM":
stack.stack(acid + " DEST " + self.dest[idest])
else:
stack.stack(acid + " DEST " +
str(self.destlat[idest]) + " " +
str(self.destlon[idest]))
if self.name[:4] != "SEGM":
stack.stack(acid + " ORIG " + self.name)
else:
stack.stack(acid + " ORIG " + str(self.lat) + " " +
str(self.lon))
if alttxt == "0" and spdtxt == "0":
stack.stack(acid + " SPD 250")
stack.stack(acid + " ALT FL100")
else:
if idest >= 0:
if self.desttype[idest] == "seg":
lat, lon, hdg = getseg(self.dest[idest])
brg, dist = kwikdist(self.lat, self.lon, lat, lon)
stack.stack(acid + " HDG " + str(brg))
else:
stack.stack(acid + " LNAV ON")
def update_fms(self, qdr, dist):
# Shift waypoints for aircraft i where necessary
for i in bs.traf.actwp.Reached(qdr, dist, bs.traf.actwp.flyby):
# Save current wp speed for use on next leg when we pass this waypoint
# VNAV speeds are always FROM-speed, so we accelerate/decellerate at the waypoint
# where this speed is specified, so we need to save it for use now
# before getting the new data for the next waypoint
# Save speed as specified for the waypoint we pass
oldspd = bs.traf.actwp.spd[i]
# Get next wp (lnavon = False if no more waypoints)
lat, lon, alt, spd, bs.traf.actwp.xtoalt[i], toalt, xtorta, bs.traf.actwp.torta[i],\
lnavon, flyby, bs.traf.actwp.next_qdr[i] = \
self.route[i].getnextwp() # note: xtoalt,toalt in [m]
# End of route/no more waypoints: switch off LNAV
bs.traf.swlnav[i] = bs.traf.swlnav[i] and lnavon
# In case of no LNAV, do not allow VNAV mode on its own
bs.traf.swvnav[i] = bs.traf.swvnav[i] and bs.traf.swlnav[i]
bs.traf.actwp.lat[i] = lat # [deg]
bs.traf.actwp.lon[i] = lon # [deg]
# 1.0 in case of fly by, else fly over
bs.traf.actwp.flyby[i] = int(flyby)
# User has entered an altitude for this waypoint
if alt >= -0.01:
bs.traf.actwp.nextaltco[i] = alt # [m]
if spd > -990. and bs.traf.swlnav[i] and bs.traf.swvnav[i]:
bs.traf.actwp.spd[i] = spd
else:
bs.traf.actwp.spd[i] = -999.
# VNAV spd mode: use speed of this waypoint as commanded speed
# while passing waypoint and save next speed for passing next wp
# Speed is now from speed! Next speed is ready in wpdata
if bs.traf.swvnav[i] and oldspd > 0.0:
bs.traf.selspd[i] = oldspd
# Update qdr and turndist for this new waypoint for ComputeVNAV
qdr[i], dummy = geo.qdrdist(bs.traf.lat[i], bs.traf.lon[i],
bs.traf.actwp.lat[i],
bs.traf.actwp.lon[i])
# Update turndist so ComputeVNAV works, is there a next leg direction or not?
if bs.traf.actwp.next_qdr[i] < -900.:
local_next_qdr = qdr[i]
else:
local_next_qdr = bs.traf.actwp.next_qdr[i]
# Calculate turn dist 9and radius which we do not use) now for scalar variable [i]
bs.traf.actwp.turndist[i], dummy = \
bs.traf.actwp.calcturn(bs.traf.tas[i], bs.traf.bank[i],
qdr[i], local_next_qdr) # update turn distance for VNAV
# VNAV = FMS ALT/SPD mode incl RTA
self.ComputeVNAV(i, toalt, bs.traf.actwp.xtoalt[i],
bs.traf.actwp.torta[i], xtorta)
# Check RTA guidance for all a/c flying to an RTA waypoint
#print(bs.traf.actwp.torta )
#print('bs.traf.actwp.torta=',bs.traf.actwp.torta)
for iac in np.where(bs.traf.actwp.torta > -99.)[0]:
# For all a/c flying to an RTA waypoint, recalculate speed more often
iwp = bs.traf.ap.route[iac].iactwp
dist2go4rta = geo.kwikdist(bs.traf.lat[iac],bs.traf.lon[iac], \
bs.traf.actwp.lat[iac],bs.traf.actwp.lon[iac])*nm
#print("dist2go4rta=",dist2go4rta)
self.setspeedforRTA(iac, bs.traf.actwp.torta[iac], dist2go4rta)
def checkInside(self, lat, lon, alt):
distance = kwikdist(self.clat, self.clon, lat, lon) # [NM]
inside = (distance <= self.r) & (self.bottom <= alt) & (alt <= self.top)
return inside
def update_fms(self, qdr, dist):
# Shift waypoints for aircraft i where necessary
for i in bs.traf.actwp.Reached(qdr, dist, bs.traf.actwp.flyby,
bs.traf.actwp.flyturn,bs.traf.actwp.turnrad):
# Save current wp speed for use on next leg when we pass this waypoint
# VNAV speeds are always FROM-speed, so we accelerate/decellerate at the waypoint
# where this speed is specified, so we need to save it for use now
# before getting the new data for the next waypoint
# Get speed for next leg from the waypoint we now
bs.traf.actwp.spd[i] = bs.traf.actwp.nextspd[i]
bs.traf.actwp.spdcon[i] = bs.traf.actwp.nextspd[i]
# Use turnradius of passing wp for bank angle
if bs.traf.actwp.flyturn[i] and bs.traf.actwp.turnrad[i]>0.:
if bs.traf.actwp.turnspd[i]>0.:
turnspd = bs.traf.actwp.turnspd[i]
else:
turnspd = bs.traf.tas[i]
bs.traf.aphi[i] = atan(turnspd*turnspd/(bs.traf.actwp.turnrad[i]*nm*g0)) # [rad]
else:
bs.traf.aphi[i] = 0.0 #[rad] or leave untouched???
# Get next wp (lnavon = False if no more waypoints)
lat, lon, alt, bs.traf.actwp.nextspd[i], bs.traf.actwp.xtoalt[i], toalt, \
bs.traf.actwp.xtorta[i], bs.traf.actwp.torta[i], \
lnavon, flyby, flyturn, turnrad, turnspd,\
bs.traf.actwp.next_qdr[i] = \
self.route[i].getnextwp() # note: xtoalt,toalt in [m]
# End of route/no more waypoints: switch off LNAV
bs.traf.swlnav[i] = bs.traf.swlnav[i] and lnavon
# In case of no LNAV, do not allow VNAV mode on its own
bs.traf.swvnav[i] = bs.traf.swvnav[i] and bs.traf.swlnav[i]
bs.traf.actwp.lat[i] = lat # [deg]
bs.traf.actwp.lon[i] = lon # [deg]
# 1.0 in case of fly by, else fly over
bs.traf.actwp.flyby[i] = int(flyby)
# User has entered an altitude for this waypoint
if alt >= -0.01:
bs.traf.actwp.nextaltco[i] = alt # [m]
if not bs.traf.swlnav[i]:
bs.traf.actwp.spd[i] = -999.
# VNAV spd mode: use speed of this waypoint as commanded speed
# while passing waypoint and save next speed for passing next wp
# Speed is now from speed! Next speed is ready in wpdata
if bs.traf.swvnavspd[i] and bs.traf.actwp.spd[i]> 0.0:
bs.traf.selspd[i] = bs.traf.actwp.spd[i]
# Update qdr and turndist for this new waypoint for ComputeVNAV
qdr[i], dummy = geo.qdrdist(bs.traf.lat[i], bs.traf.lon[i],
bs.traf.actwp.lat[i], bs.traf.actwp.lon[i])
# Update turndist so ComputeVNAV works, is there a next leg direction or not?
if bs.traf.actwp.next_qdr[i] < -900.:
local_next_qdr = qdr[i]
else:
local_next_qdr = bs.traf.actwp.next_qdr[i]
bs.traf.actwp.flyturn[i] = flyturn
bs.traf.actwp.turnrad[i] = turnrad
bs.traf.actwp.turnspd[i] = turnspd
# Calculate turn dist (and radius which we do not use) now for scalar variable [i]
bs.traf.actwp.turndist[i], dummy = \
bs.traf.actwp.calcturn(bs.traf.tas[i], bs.traf.bank[i],
qdr[i], local_next_qdr,turnrad) # update turn distance for VNAV
# VNAV = FMS ALT/SPD mode incl. RTA
self.ComputeVNAV(i, toalt, bs.traf.actwp.xtoalt[i], bs.traf.actwp.torta[i],
bs.traf.actwp.xtorta[i])
# Check deceleration distance before next wp in case of turns
bs.traf.actwp.deceldist[i] = np.where(flyturn,
(bs.traf.tas[i] - cas2tas(turnspd, bs.traf.alt[i])) * 0.5 *
(bs.traf.tas[i] - cas2tas(turnspd, bs.traf.alt[i])) / max(0.001, bs.traf.ax[i])
, 0.0)
# Continuous guidance when speed constraint on active leg
# If still an RTA in the route and currently no speed constraint
for iac in np.where((bs.traf.actwp.torta > -99.)*(bs.traf.actwp.spdcon<0.0))[0]:
iwp = bs.traf.ap.route[iac].iactwp
if bs.traf.ap.route[iac].wprta[iwp]>-99.:
# For all a/c flying to an RTA waypoint, recalculate speed more often
dist2go4rta = geo.kwikdist(bs.traf.lat[iac],bs.traf.lon[iac], \
bs.traf.actwp.lat[iac],bs.traf.actwp.lon[iac])*nm \
+ bs.traf.ap.route[iac].wpxtorta[iwp] # last term zero for active wp rta
# Set bs.traf.actwp.spd to rta speed, if necessary
self.setspeedforRTA(iac,bs.traf.actwp.torta[iac],dist2go4rta)
# If VNAV speed is on (by default coupled to VNAV), use it for speed guidance
if bs.traf.swvnavspd[iac]:
bs.traf.selspd[iac] = bs.traf.actwp.spd[iac]
def update(self,gain):
# Time step update of source
# Get time step
dt = sim.simt - self.tupdate
self.tupdate = sim.simt
# Time for a new aircraft?
if dt>0.0:
# Calculate probability of a geberate occurring with flow
chances = 1.0-gain*self.flow*dt/3600. #flow is in a/c per hour=360 seconds
if random.random() >= chances:
# Runways defined? => use runway lines (queues)
if len(self.runways)>0:
# We do not yet need to create an aircraft
gennow = False
# Find shortest line and put it in
isel = random.randint(0,len(self.runways)-1)
self.rwyline[isel] = self.rwyline[isel] + 1
else:
# Yes we do need to generate one now
gennow = True
else:
gennow = False
# Check for generating aircraft
# First check runways for a/c already in line:
txt = ""
for i in range(len(self.runways)):
# Runway vacated and there is one waiting?
if sim.simt-self.rwytotime[i]>self.dtakeoff and self.rwyline[i]>0:
#if self.name == "EHAM":
# print(sim.simt, self.runways[i], self.rwytotime[i])
self.rwytotime[i] = sim.simt
self.rwyline[i] = self.rwyline[i]-1
# Choose and aicraft type, check for distance
if len(self.dest)>0:
idest = int(random.random() * len(self.dest))
else:
idest = -1
if idest>=0:
acid = randacname(self.name,self.dest[idest])
if self.desttype[idest]=="seg" or self.dest[idest][:4]=="SEGM":
lat,lon,hdg = getseg(self.dest[idest])
else:
success,posobj = txt2pos(self.dest[idest],ctrlat,ctrlon)
lat,lon = posobj.lat,posobj.lon
distroute = latlondist(self.lat,self.lon,lat,lon)/nm
else:
acid = randacname(self.name, self.name)
if self.destactypes[idest] == []:
actype = random.choice(self.actypes)
actype = checkactype(actype, distroute, self.actypes)
else:
actype = random.choice(self.destactypes[idest])
stack.stack("CRE "+",".join([acid, actype,
str(self.rwylat[i]),str(self.rwylon[i]),str(self.rwyhdg[i]),
"0.0","0.0"]))
stack.stack(acid + " SPD 250")
stack.stack(acid + " ALT FL100")
stack.stack(acid + " HDG " + str(self.rwyhdg[i]))
# TBD: Add waypoint for after take-off?
if idest>=0:
if self.dest[idest][:4] != "SEGM":
stack.stack(acid + " DEST " + self.dest[idest])
else:
stack.stack(acid + " DEST " + str(self.destlat[idest])
+ " " + str(self.destlon[idest]))
if self.name[:4] != "SEGM":
stack.stack(acid + " ORIG " + self.name)
else:
stack.stack(acid + " ORIG " + str(self.lat) + " " + str(self.lon))
if idest>=0 and self.desttype[idest]=="seg":
lat,lon,hdg = getseg(self.dest[idest])
brg,dist = kwikqdrdist(self.lat,self.lon,lat,lon)
#stack.stack(acid+" HDG "+str(brg))
else:
stack.stack(acid+" LNAV OFF")
#stack.stack(acid+" VNAV ON")
# Not runway, then define instantly at position with random heading or in case of segment inward heading
if gennow:
if not self.incircle:
lat,lon = kwikpos(ctrlat,ctrlon,self.segdir,radius)
hdg = self.segdir-180
elif self.type=="seg":
lat,lon,brg = getseg(self.name)
hdg = (brg+180)%360
elif self.type=="rwy":
lat,lon = self.lat,self.lon
hdg = self.hdg # Runway heading
else:
hdg = random.random()*360.
if (self.type=="apt" or self.type=="rwy") and self.incircle:
alttxt,spdtxt = str(0),str(0)
else:
alttxt,spdtxt = "FL"+str(random.randint(200,300)), str(random.randint(250,350))
# Add destination
if len(self.dest)>0:
idest = int(random.random() * len(self.dest))
acid = randacname(self.name,self.dest[idest])
else:
acid = randacname(self.name,self.name)
idest = -1
stack.stack("CRE " + ",".join([acid, random.choice(self.actypes),
str(self.lat), str(self.lon), str(int(hdg%360)),
alttxt,spdtxt]))
if idest>=0:
if self.dest[idest][:4] != "SEGM":
stack.stack(acid + " DEST " + self.dest[idest])
else:
stack.stack(acid + " DEST " + str(self.destlat[idest])+" "+str(self.destlon[idest]))
if self.name[:4] != "SEGM":
stack.stack(acid + " ORIG " + self.name)
else:
stack.stack(acid + " ORIG " + str(self.lat)+" "+str(self.lon))
if alttxt=="0" and spdtxt =="0":
stack.stack(acid+" SPD 250")
stack.stack(acid+" ALT FL100")
else:
if idest>=0:
if self.desttype[idest] == "seg":
lat, lon, hdg = getseg(self.dest[idest])
brg, dist = kwikdist(self.lat, self.lon, lat, lon)
stack.stack(acid + " HDG " + str(brg))
else:
stack.stack(acid + " LNAV ON")
