def act5(self):
# Compute 15 degree turn left waypoint.
# Use current speed to compute waypoint.
dqdr = 15
latA = traf.lat[self.acidx]
lonA = traf.lon[self.acidx]
turnrad = traf.tas[self.acidx]**2 / (np.maximum(0.01, np.tan(traf.bank[self.acidx])) * g0) # [m]
#Turn right so add bearing
# qdr = traf.qdr[self.acidx] + 90
latR, lonR = qdrpos(latA, lonA, traf.hdg[self.acidx] - 90, turnrad/nm) # [deg, deg]
# Rotate vector
latB, lonB = qdrpos(latR, lonR, traf.hdg[self.acidx] + 90 - dqdr, turnrad/nm) # [deg, deg]
cmd = "{} BEFORE {} ADDWPT '{},{}'".format(traf.id[self.acidx], traf.ap.route[0].wpname[-2], latB, lonB)
stack.stack(cmd)
def arguments(numargs, cmdargs):
syntaxerror = False
# tunables:
acalt = float(10000) # default
acspd = float(300) # default
actype = "B747" # default
startdistance = 1 # default
ang = float(cmdargs[1]) / 2
if numargs > 2: #process optional arguments
for i in range(
2, numargs
): # loop over arguments (TODO: put arguments in np array)
if cmdargs[i].upper().startswith("-R"): #radius
startdistance = geo.qdrpos(0, 0, 90, float(
cmdargs[i][3:]))[2] #input in nm
elif cmdargs[i].upper().startswith("-A"): #altitude
acalt = txt2alt(cmdargs[i][3:]) * ft
elif cmdargs[i].upper().startswith("-S"): #speed
acspd = txt2spd(cmdargs[i][3:], acalt)
elif cmdargs[i].upper().startswith("-T"): #ac type
actype = cmdargs[i][3:].upper()
else:
syntaxerror = True
return syntaxerror, acalt, acspd, actype, startdistance, ang
def creconfs(self,
acid,
actype,
targetidx,
dpsi,
cpa,
tlosh,
dH=None,
tlosv=None,
spd=None):
latref = self.lat[targetidx] # deg
lonref = self.lon[targetidx] # deg
altref = self.alt[targetidx] # m
trkref = radians(self.trk[targetidx])
gsref = self.gs[targetidx] # m/s
vsref = self.vs[targetidx] # m/s
cpa = cpa * nm
pzr = settings.asas_pzr * nm
pzh = settings.asas_pzh * ft
trk = trkref + radians(dpsi)
gs = gsref if spd is None else spd
if dH is None:
acalt = altref
acvs = 0.0
else:
acalt = altref + dH
tlosv = tlosh if tlosv is None else tlosv
acvs = vsref - np.sign(dH) * (abs(dH) - pzh) / tlosv
# Horizontal relative velocity vector
gsn, gse = gs * cos(trk), gs * sin(trk)
vreln, vrele = gsref * cos(trkref) - gsn, gsref * sin(trkref) - gse
# Relative velocity magnitude
vrel = sqrt(vreln * vreln + vrele * vrele)
# Relative travel distance to closest point of approach
drelcpa = tlosh * vrel + (0 if cpa > pzr else sqrt(pzr * pzr -
cpa * cpa))
# Initial intruder distance
dist = sqrt(drelcpa * drelcpa + cpa * cpa)
# Rotation matrix diagonal and cross elements for distance vector
rd = drelcpa / dist
rx = cpa / dist
# Rotate relative velocity vector to obtain intruder bearing
brn = degrees(atan2(-rx * vreln + rd * vrele, rd * vreln + rx * vrele))
# Calculate intruder lat/lon
aclat, aclon = geo.qdrpos(latref, lonref, brn, dist / nm)
# convert groundspeed to CAS, and track to heading
wn, we = self.wind.getdata(aclat, aclon, acalt)
tasn, tase = gsn - wn, gse - we
acspd = vtas2cas(sqrt(tasn * tasn + tase * tase), acalt)
achdg = degrees(atan2(tase, tasn))
# Create and, when necessary, set vertical speed
self.create(1, actype, acalt, acspd, None, aclat, aclon, achdg, acid)
self.ap.selaltcmd(len(self.lat) - 1, altref, acvs)
self.vs[-1] = acvs
def act4(self):
# Compute 15 degree turn left waypoint.
# Use current speed to compute waypoint.
latA = traf.lat[self.acidx]
lonA = traf.lon[self.acidx]
latB, lonB = qdrpos(latA, lonA, traf.hdg[self.acidx], 0.25)
cmd = "{} BEFORE {} ADDWPT '{},{}'".format(traf.id[self.acidx], traf.ap.route[0].wpname[-2], latB, lonB)
stack.stack(cmd)
def ilsgate(rwyname):
if '/' not in rwyname:
return False, 'Argument is not a runway ' + rwyname
apt, rwy = rwyname.split('/RW')
rwy = rwy.lstrip('Y')
apt_thresholds = navdb.rwythresholds.get(apt)
if not apt_thresholds:
return False, 'Argument is not a runway (airport not found) ' + apt
rwy_threshold = apt_thresholds.get(rwy)
if not rwy_threshold:
return False, 'Argument is not a runway (runway not found) ' + rwy
# Extract runway threshold lat/lon, and runway heading
lat, lon, hdg = rwy_threshold
# The ILS gate is defined as a triangular area pointed away from the runway
# First calculate the two far corners in cartesian coordinates
cone_length = 50 # nautical miles
cone_angle = 20.0 # degrees
lat1, lon1 = geo.qdrpos(lat, lon, hdg - 180.0 + cone_angle, cone_length)
lat2, lon2 = geo.qdrpos(lat, lon, hdg - 180.0 - cone_angle, cone_length)
coordinates = np.array([lat, lon, lat1, lon1, lat2, lon2])
areafilter.defineArea('ILS' + rwyname, 'POLYALT', coordinates, top=4000*ft)
def creconfs(self, acid, actype, targetidx, dpsi, cpa, tlosh, dH=None, tlosv=None, spd=None):
latref = self.lat[targetidx] # deg
lonref = self.lon[targetidx] # deg
altref = self.alt[targetidx] # m
trkref = radians(self.trk[targetidx])
gsref = self.gs[targetidx] # m/s
vsref = self.vs[targetidx] # m/s
cpa = cpa * nm
pzr = settings.asas_pzr * nm
pzh = settings.asas_pzh * ft
trk = trkref + radians(dpsi)
gs = gsref if spd is None else spd
if dH is None:
acalt = altref
acvs = 0.0
else:
acalt = altref + dH
tlosv = tlosh if tlosv is None else tlosv
acvs = vsref - np.sign(dH) * (abs(dH) - pzh) / tlosv
# Horizontal relative velocity vector
gsn, gse = gs * cos(trk), gs * sin(trk)
vreln, vrele = gsref * cos(trkref) - gsn, gsref * sin(trkref) - gse
# Relative velocity magnitude
vrel = sqrt(vreln * vreln + vrele * vrele)
# Relative travel distance to closest point of approach
drelcpa = tlosh * vrel + (0 if cpa > pzr else sqrt(pzr * pzr - cpa * cpa))
# Initial intruder distance
dist = sqrt(drelcpa * drelcpa + cpa * cpa)
# Rotation matrix diagonal and cross elements for distance vector
rd = drelcpa / dist
rx = cpa / dist
# Rotate relative velocity vector to obtain intruder bearing
brn = degrees(atan2(-rx * vreln + rd * vrele,
rd * vreln + rx * vrele))
# Calculate intruder lat/lon
aclat, aclon = geo.qdrpos(latref, lonref, brn, dist / nm)
# convert groundspeed to CAS, and track to heading
wn, we = self.wind.getdata(aclat, aclon, acalt)
tasn, tase = gsn - wn, gse - we
acspd = vtas2cas(sqrt(tasn * tasn + tase * tase), acalt)
achdg = degrees(atan2(tase, tasn))
# Create and, when necessary, set vertical speed
self.create(1, actype, acalt, acspd, None, aclat, aclon, achdg, acid)
self.ap.selaltcmd(len(self.lat) - 1, altref, acvs)
self.vs[-1] = acvs
def calc_turn_wp(delta_qdr, acidx):
latA = traf.lat[acidx]
lonA = traf.lon[acidx]
turnrad = traf.tas[acidx]**2 / (
np.maximum(0.01, np.tan(traf.bank[acidx])) * g0) # [m]
# Turn left
if delta_qdr > 0:
# Compute centre of turning circle
latR, lonR = qdrpos(latA, lonA, traf.hdg[acidx] + 90,
turnrad / nm) # [deg, deg]
# Rotate vector position vector along turning circle
latB, lonB = qdrpos(latR, lonR, traf.hdg[acidx] - 90 + abs(delta_qdr),
turnrad / nm) # [deg, deg]
else:
# Compute centre of turning circle
latR, lonR = qdrpos(latA, lonA, traf.hdg[acidx] - 90,
turnrad / nm) # [deg, deg]
# Rotate vector
latB, lonB = qdrpos(latR, lonR, traf.hdg[acidx] + 90 - abs(delta_qdr),
turnrad / nm) # [deg, deg]
return latB, lonB
def arguments(numargs, cmdargs):
syntaxerror = False
# tunables:
acalt = float(10000) # default
acspd = float(300) # default
actype = "B747" # default
startdistance = 1 # default
ang = float(cmdargs[1])/2
if numargs>2: #process optional arguments
for i in range(2 ,numargs): # loop over arguments (TODO: put arguments in np array)
if cmdargs[i].upper().startswith("-R"): #radius
startdistance = geo.qdrpos(0,0,90,float(cmdargs[i][3:]))[2] #input in nm
elif cmdargs[i].upper().startswith("-A"): #altitude
acalt = txt2alt(cmdargs[i][3:])*ft
elif cmdargs[i].upper().startswith("-S"): #speed
acspd = txt2spd(cmdargs[i][3:],acalt)
elif cmdargs[i].upper().startswith("-T"): #ac type
actype = cmdargs[i][3:].upper()
else:
syntaxerror = True
return syntaxerror,acalt,acspd,actype,startdistance,ang
def addwptStack(self, idx, *args): # args: all arguments of addwpt
"""ADDWPT acid, (wpname/lat,lon),[alt],[spd],[afterwp],[beforewp]"""
# print "addwptStack:",args
# Check FLYBY or FLYOVER switch, instead of adding a waypoint
if len(args) == 1:
swwpmode = args[0].replace('-', '')
if swwpmode == "FLYBY":
self.swflyby = True
self.swflyturn = False
return True
elif swwpmode == "FLYOVER":
self.swflyby = False
self.swflyturn = False
return True
elif swwpmode == "FLYTURN":
self.swflyby = False
self.swflyturn = True
return True
elif len(args) == 2:
swwpmode = args[0].replace('-', '')
if swwpmode == "TURNRAD" or swwpmode == "TURNRADIUS":
try:
self.turnrad = float(
args[1]) / ft # arg was originally parsed as wpalt
except:
return False, "Error in processing value of turn radius"
return True
elif swwpmode == "TURNSPD" or swwpmode == "TURNSPEED":
try:
self.turnspd = args[
1] * kts / ft # [m/s] Arg was wpalt Keep it as IAS/CAS orig in kts, now in m/s
except:
return False, "Error in processing value of turn speed"
return True
# Convert to positions
name = args[0].upper().strip()
# Choose reference position ot look up VOR and waypoints
# First waypoint: own position
if self.nwp == 0:
reflat = bs.traf.lat[idx]
reflon = bs.traf.lon[idx]
# Or last waypoint before destination
else:
if self.wptype[-1] != Route.dest or self.nwp == 1:
reflat = self.wplat[-1]
reflon = self.wplon[-1]
else:
reflat = self.wplat[-2]
reflon = self.wplon[-2]
# Default altitude, speed and afterwp
alt = -999.
spd = -999.
afterwp = ""
beforewp = ""
# Is it aspecial take-off waypoint?
takeoffwpt = name.replace('-', '') == "TAKEOFF"
# Normal waypoint (no take-off waypoint => see else)
if not takeoffwpt:
# Get waypoint position
success, posobj = txt2pos(name, reflat, reflon)
if success:
lat = posobj.lat
lon = posobj.lon
if posobj.type == "nav" or posobj.type == "apt":
wptype = Route.wpnav
elif posobj.type == "rwy":
wptype = Route.runway
else: # treat as lat/lon
name = bs.traf.id[idx]
wptype = Route.wplatlon
if len(args) > 1 and args[1]:
alt = args[1]
if len(args) > 2 and args[2]:
spd = args[2]
if len(args) > 3 and args[3]:
afterwp = args[3]
if len(args) > 4 and args[4]:
beforewp = args[4]
else:
return False, "Waypoint " + name + " not found."
# Take off waypoint: positioned 20% of the runway length after the runway
else:
# Look up runway in route
rwyrteidx = -1
i = 0
while i < self.nwp and rwyrteidx < 0:
if self.wpname[i].count("/") > 0:
# print (self.wpname[i])
rwyrteidx = i
i += 1
# Only TAKEOFF is specified wihtou a waypoint/runway
if len(args) == 1 or not args[1]:
# No runway given: use first in route or current position
# print ("rwyrteidx =",rwyrteidx)
# We find a runway in the route, so use it
if rwyrteidx > 0:
rwylat = self.wplat[rwyrteidx]
rwylon = self.wplon[rwyrteidx]
aptidx = bs.navdb.getapinear(rwylat, rwylon)
aptname = bs.navdb.aptname[aptidx]
rwyname = self.wpname[rwyrteidx].split("/")[1]
rwyid = rwyname.replace("RWY", "").replace("RW", "")
rwyhdg = bs.navdb.rwythresholds[aptname][rwyid][2]
else:
rwylat = bs.traf.lat[idx]
rwylon = bs.traf.lon[idx]
rwyhdg = bs.traf.trk[idx]
elif args[1].count(
"/") > 0 or len(args) > 2 and args[2]: # we need apt,rwy
# Take care of both EHAM/RW06 as well as EHAM,RWY18L (so /&, and RW/RWY)
if args[1].count("/") > 0:
aptid, rwyname = args[1].split("/")
else:
# Runway specified
aptid = args[1]
rwyname = args[2]
rwyid = rwyname.replace("RWY",
"").replace("RW",
"") # take away RW or RWY
# print ("apt,rwy=",aptid,rwyid)
# TODO: Add finding the runway heading with rwyrteidx>0 and navdb!!!
# Try to get it from the database
try:
rwyhdg = bs.navdb.rwythresholds[aptid][rwyid][2]
except:
rwydir = rwyid.replace("L",
"").replace("R",
"").replace("C", "")
try:
rwyhdg = float(rwydir) * 10.
except:
return False, name + " not found."
success, posobj = txt2pos(aptid + "/RW" + rwyid, reflat,
reflon)
if success:
rwylat, rwylon = posobj.lat, posobj.lon
else:
rwylat = bs.traf.lat[idx]
rwylon = bs.traf.lon[idx]
else:
return False, "Use ADDWPT TAKEOFF,AIRPORTID,RWYNAME"
# Create a waypoint 2 nm away from current point
rwydist = 2.0 # [nm] use default distance away from threshold
lat, lon = geo.qdrpos(rwylat, rwylon, rwyhdg, rwydist) #[deg,deg
wptype = Route.wplatlon
# Add after the runwy in the route
if rwyrteidx > 0:
afterwp = self.wpname[rwyrteidx]
elif self.wptype and self.wptype[0] == Route.orig:
afterwp = self.wpname[0]
else:
# Assume we're called before other waypoints are added
afterwp = ""
name = "T/O-" + bs.traf.id[idx] # Use lat/lon naming convention
# Add waypoint
wpidx = self.addwpt(idx, name, wptype, lat, lon, alt, spd, afterwp,
beforewp)
# Recalculate flight plan
self.calcfp()
# Check for success by checking insetred locaiton in flight plan >= 0
if wpidx < 0:
return False, "Waypoint " + name + " not added."
# chekc for presence of orig/dest
norig = int(bs.traf.ap.orig[idx] != "")
ndest = int(bs.traf.ap.dest[idx] != "")
# Check whether this is first 'real' waypoint (not orig & dest),
# And if so, make active
if self.nwp - norig - ndest == 1: # first waypoint: make active
self.direct(idx, self.wpname[norig]) # 0 if no orig
bs.traf.swlnav[idx] = True
if afterwp and self.wpname.count(afterwp) == 0:
return True, "Waypoint " + afterwp + " not found" + \
"waypoint added at end of route"
else:
return True
def update_aircraft_data(self, data):
''' Update GPU buffers with new aircraft simulation data. '''
if not self.initialized:
return
self.glsurface.makeCurrent()
actdata = bs.net.get_nodedata()
if actdata.filteralt:
idx = np.where(
(data.alt >= actdata.filteralt[0]) * (data.alt <= actdata.filteralt[1]))
data.lat = data.lat[idx]
data.lon = data.lon[idx]
data.trk = data.trk[idx]
data.alt = data.alt[idx]
data.tas = data.tas[idx]
data.vs = data.vs[idx]
data.rpz = data.rpz[idx]
naircraft = len(data.lat)
actdata.translvl = data.translvl
# self.asas_vmin = data.vmin # TODO: array should be attribute not uniform
# self.asas_vmax = data.vmax
if naircraft == 0:
self.cpalines.set_vertex_count(0)
else:
# Update data in GPU buffers
self.lat.update(np.array(data.lat, dtype=np.float32))
self.lon.update(np.array(data.lon, dtype=np.float32))
self.hdg.update(np.array(data.trk, dtype=np.float32))
self.alt.update(np.array(data.alt, dtype=np.float32))
self.tas.update(np.array(data.tas, dtype=np.float32))
self.rpz.update(np.array(data.rpz, dtype=np.float32))
if hasattr(data, 'asasn') and hasattr(data, 'asase'):
self.asasn.update(np.array(data.asasn, dtype=np.float32))
self.asase.update(np.array(data.asase, dtype=np.float32))
# CPA lines to indicate conflicts
ncpalines = np.count_nonzero(data.inconf)
cpalines = np.zeros(4 * ncpalines, dtype=np.float32)
self.cpalines.set_vertex_count(2 * ncpalines)
# Labels and colors
rawlabel = ''
color = np.empty(
(min(naircraft, MAX_NAIRCRAFT), 4), dtype=np.uint8)
selssd = np.zeros(naircraft, dtype=np.uint8)
confidx = 0
zdata = zip(data.id, data.ingroup, data.inconf, data.tcpamax, data.trk, data.gs,
data.cas, data.vs, data.alt, data.lat, data.lon)
for i, (acid, ingroup, inconf, tcpa,
trk, gs, cas, vs, alt, lat, lon) in enumerate(zdata):
if i >= MAX_NAIRCRAFT:
break
# Make label: 3 lines of 8 characters per aircraft
if actdata.show_lbl >= 1:
rawlabel += '%-8s' % acid[:8]
if actdata.show_lbl == 2:
if alt <= data.translvl:
rawlabel += '%-5d' % int(alt / ft + 0.5)
else:
rawlabel += 'FL%03d' % int(alt / ft / 100. + 0.5)
vsarrow = 30 if vs > 0.25 else 31 if vs < -0.25 else 32
rawlabel += '%1s %-8d' % (chr(vsarrow),
int(cas / kts + 0.5))
else:
rawlabel += 16 * ' '
if inconf:
if actdata.ssd_conflicts:
selssd[i] = 255
color[i, :] = palette.conflict + (255,)
lat1, lon1 = geo.qdrpos(lat, lon, trk, tcpa * gs / nm)
cpalines[4 * confidx: 4 * confidx +
4] = [lat, lon, lat1, lon1]
confidx += 1
else:
# Get custom color if available, else default
rgb = palette.aircraft
if ingroup:
for groupmask, groupcolor in actdata.custgrclr.items():
if ingroup & groupmask:
rgb = groupcolor
break
rgb = actdata.custacclr.get(acid, rgb)
color[i, :] = tuple(rgb) + (255,)
# Check if aircraft is selected to show SSD
if actdata.ssd_all or acid in actdata.ssd_ownship:
selssd[i] = 255
if len(actdata.ssd_ownship) > 0 or actdata.ssd_conflicts or actdata.ssd_all:
self.ssd.update(selssd=selssd)
self.cpalines.update(vertex=cpalines)
self.color.update(color)
self.lbl.update(np.array(rawlabel.encode('utf8'), dtype=np.string_))
# If there is a visible route, update the start position
if self.route_acid in data.id:
idx = data.id.index(self.route_acid)
self.route.vertex.update(np.array([data.lat[idx], data.lon[idx]], dtype=np.float32))
def update_aircraft_data(self, data):
if not self.initialized:
return
self.makeCurrent()
actdata = bs.net.get_nodedata()
if actdata.filteralt:
idx = np.where((data.alt >= actdata.filteralt[0]) *
(data.alt <= actdata.filteralt[1]))
data.lat = data.lat[idx]
data.lon = data.lon[idx]
data.trk = data.trk[idx]
data.alt = data.alt[idx]
data.tas = data.tas[idx]
data.vs = data.vs[idx]
self.naircraft = len(data.lat)
actdata.translvl = data.translvl
self.asas_vmin = data.vmin
self.asas_vmax = data.vmax
if self.naircraft == 0:
self.cpalines.set_vertex_count(0)
else:
# Update data in GPU buffers
self.aclatbuf.update(np.array(data.lat, dtype=np.float32))
self.aclonbuf.update(np.array(data.lon, dtype=np.float32))
self.achdgbuf.update(np.array(data.trk, dtype=np.float32))
self.acaltbuf.update(np.array(data.alt, dtype=np.float32))
self.actasbuf.update(np.array(data.tas, dtype=np.float32))
self.asasnbuf.update(np.array(data.asasn, dtype=np.float32))
self.asasebuf.update(np.array(data.asase, dtype=np.float32))
# CPA lines to indicate conflicts
ncpalines = np.count_nonzero(data.inconf)
cpalines = np.zeros(4 * ncpalines, dtype=np.float32)
self.cpalines.set_vertex_count(2 * ncpalines)
# Labels and colors
rawlabel = ''
color = np.empty((min(self.naircraft, MAX_NAIRCRAFT), 4),
dtype=np.uint8)
selssd = np.zeros(self.naircraft, dtype=np.uint8)
confidx = 0
zdata = zip(data.id, data.ingroup, data.inconf, data.tcpamax,
data.trk, data.gs, data.cas, data.vs, data.alt,
data.lat, data.lon)
for i, (acid, ingroup, inconf, tcpa, trk, gs, cas, vs, alt, lat,
lon) in enumerate(zdata):
if i >= MAX_NAIRCRAFT:
break
# Make label: 3 lines of 8 characters per aircraft
if actdata.show_lbl >= 1:
rawlabel += '%-8s' % acid[:8]
if actdata.show_lbl == 2:
if alt <= data.translvl:
rawlabel += '%-5d' % int(alt / ft + 0.5)
else:
rawlabel += 'FL%03d' % int(alt / ft / 100. + 0.5)
vsarrow = 30 if vs > 0.25 else 31 if vs < -0.25 else 32
rawlabel += '%1s %-8d' % (chr(vsarrow),
int(cas / kts + 0.5))
else:
rawlabel += 16 * ' '
if inconf:
if actdata.ssd_conflicts:
selssd[i] = 255
color[i, :] = palette.conflict + (255, )
lat1, lon1 = geo.qdrpos(lat, lon, trk, tcpa * gs / nm)
cpalines[4 * confidx:4 * confidx +
4] = [lat, lon, lat1, lon1]
confidx += 1
else:
# Get custom color if available, else default
rgb = palette.aircraft
if ingroup:
for groupmask, groupcolor in actdata.custgrclr.items():
if ingroup & groupmask:
rgb = groupcolor
break
rgb = actdata.custacclr.get(acid, rgb)
color[i, :] = tuple(rgb) + (255, )
# Check if aircraft is selected to show SSD
if actdata.ssd_all or acid in actdata.ssd_ownship:
selssd[i] = 255
if len(actdata.ssd_ownship
) > 0 or actdata.ssd_conflicts or actdata.ssd_all:
self.ssd.selssd.buf.update(selssd)
self.confcpabuf.update(cpalines)
self.accolorbuf.update(color)
self.aclblbuf.update(
np.array(rawlabel.encode('utf8'), dtype=np.string_))
# If there is a visible route, update the start position
if self.route_acid != "":
if self.route_acid in data.id:
idx = data.id.index(self.route_acid)
self.route.vertex.update(
np.array([data.lat[idx], data.lon[idx]],
dtype=np.float32))
# Update trails database with new lines
if data.swtrails:
actdata.traillat0.extend(data.traillat0)
actdata.traillon0.extend(data.traillon0)
actdata.traillat1.extend(data.traillat1)
actdata.traillon1.extend(data.traillon1)
self.traillines.update(vertex=np.array(list(
zip(actdata.traillat0, actdata.traillon0,
actdata.traillat1, actdata.traillon1)) + list(
zip(data.traillastlat, data.traillastlon,
list(data.lat), list(data.lon))),
dtype=np.float32))
else:
actdata.traillat0 = []
actdata.traillon0 = []
actdata.traillat1 = []
actdata.traillon1 = []
self.traillines.set_vertex_count(0)
def addwptStack(self, idx, *args): # args: all arguments of addwpt
"""ADDWPT acid, (wpname/lat,lon),[alt],[spd],[afterwp],[beforewp]"""
# print "addwptStack:",args
# Check FLYBY or FLYOVER switch, instead of adding a waypoint
if len(args) == 1:
isflyby = args[0].replace('-', '')
if isflyby == "FLYBY":
self.swflyby = True
return True
elif isflyby == "FLYOVER":
self.swflyby = False
return True
# Convert to positions
name = args[0].upper().strip()
# Choose reference position ot look up VOR and waypoints
# First waypoint: own position
if self.nwp == 0:
reflat = bs.traf.lat[idx]
reflon = bs.traf.lon[idx]
# Or last waypoint before destination
else:
if self.wptype[-1] != Route.dest or self.nwp == 1:
reflat = self.wplat[-1]
reflon = self.wplon[-1]
else:
reflat = self.wplat[-2]
reflon = self.wplon[-2]
# Default altitude, speed and afterwp
alt = -999.
spd = -999.
afterwp = ""
beforewp = ""
# Is it aspecial take-off waypoint?
takeoffwpt = name.replace('-', '') == "TAKEOFF"
# Normal waypoint (no take-off waypoint => see else)
if not takeoffwpt:
# Get waypoint position
success, posobj = txt2pos(name, reflat, reflon)
if success:
lat = posobj.lat
lon = posobj.lon
if posobj.type == "nav" or posobj.type == "apt":
wptype = Route.wpnav
elif posobj.type == "rwy":
wptype = Route.runway
else: # treat as lat/lon
name = bs.traf.id[idx]
wptype = Route.wplatlon
if len(args) > 1 and args[1]:
alt = args[1]
if len(args) > 2 and args[2]:
spd = args[2]
if len(args) > 3 and args[3]:
afterwp = args[3]
if len(args) > 4 and args[4]:
beforewp = args[4]
else:
return False, "Waypoint " + name + " not found."
# Take off waypoint: positioned 20% of the runway length after the runway
else:
# Look up runway in route
rwyrteidx = -1
i = 0
while i<self.nwp and rwyrteidx<0:
if self.wpname[i].count("/") >0:
# print (self.wpname[i])
rwyrteidx = i
i += 1
# Only TAKEOFF is specified wihtou a waypoint/runway
if len(args) == 1 or not args[1]:
# No runway given: use first in route or current position
# print ("rwyrteidx =",rwyrteidx)
# We find a runway in the route, so use it
if rwyrteidx>0:
rwylat = self.wplat[rwyrteidx]
rwylon = self.wplon[rwyrteidx]
aptidx = bs.navdb.getapinear(rwylat,rwylon)
aptname = bs.navdb.aptname[aptidx]
rwyname = self.wpname[rwyrteidx].split("/")[1]
rwyid = rwyname.replace("RWY","").replace("RW","")
rwyhdg = bs.navdb.rwythresholds[aptname][rwyid][2]
else:
rwylat = bs.traf.lat[idx]
rwylon = bs.traf.lon[idx]
rwyhdg = bs.traf.trk[idx]
elif args[1].count("/") > 0 or len(args) > 2 and args[2]: # we need apt,rwy
# Take care of both EHAM/RW06 as well as EHAM,RWY18L (so /&, and RW/RWY)
if args[1].count("/")>0:
aptid,rwyname = args[1].split("/")
else:
# Runway specified
aptid = args[1]
rwyname = args[2]
rwyid = rwyname.replace("RWY", "").replace("RW", "") # take away RW or RWY
# print ("apt,rwy=",aptid,rwyid)
# TDO: Add fingind the runway heading with rwyrteidx>0 and navdb!!!
# Try to get it from the database
try:
rwyhdg = bs.navdb.rwythresholds[aptid][rwyid][2]
except:
rwydir = rwyid.replace("L","").replace("R","").replace("C","")
try:
rwyhdg = float(rwydir)*10.
except:
return False,name+" not found."
success, posobj = txt2pos(aptid+"/RW"+rwyid, reflat, reflon)
if success:
rwylat,rwylon = posobj.lat,posobj.lon
else:
rwylat = bs.traf.lat[idx]
rwylon = bs.traf.lon[idx]
else:
return False,"Use ADDWPT TAKEOFF,AIRPORTID,RWYNAME"
# Create a waypoint 2 nm away from current point
rwydist = 2.0 # [nm] use default distance away from threshold
lat,lon = geo.qdrpos(rwylat, rwylon, rwyhdg, rwydist) #[deg,deg
wptype = Route.wplatlon
# Add after the runwy in the route
if rwyrteidx > 0:
afterwp = self.wpname[rwyrteidx]
elif self.wptype and self.wptype[0] == Route.orig:
afterwp = self.wpname[0]
else:
# Assume we're called before other waypoints are added
afterwp = ""
name = "T/O-" + bs.traf.id[idx] # Use lat/lon naming convention
# Add waypoint
wpidx = self.addwpt(idx, name, wptype, lat, lon, alt, spd, afterwp, beforewp)
# Check for success by checking insetred locaiton in flight plan >= 0
if wpidx < 0:
return False, "Waypoint " + name + " not added."
# chekc for presence of orig/dest
norig = int(bs.traf.ap.orig[idx] != "")
ndest = int(bs.traf.ap.dest[idx] != "")
# Check whether this is first 'real' wayppint (not orig & dest),
# And if so, make active
if self.nwp - norig - ndest == 1: # first waypoint: make active
self.direct(idx, self.wpname[norig]) # 0 if no orig
bs.traf.swlnav[idx] = True
if afterwp and self.wpname.count(afterwp) == 0:
return True, "Waypoint " + afterwp + " not found" + \
"waypoint added at end of route"
else:
return True
def detect(asas, traf, simt):
if not asas.swasas:
return
# Reset lists before new CD
asas.iconf = [[] for ac in range(traf.ntraf)]
asas.nconf = 0
asas.confpairs = []
asas.latowncpa = []
asas.lonowncpa = []
asas.altowncpa = []
asas.LOSlist_now = []
asas.conflist_now = []
# Horizontal conflict ---------------------------------------------------------
# qdlst is for [i,j] qdr from i to j, from perception of ADSB and own coordinates
qdlst = geo.qdrdist_matrix(np.mat(traf.lat), np.mat(traf.lon),
np.mat(traf.adsb.lat), np.mat(traf.adsb.lon))
# Convert results from mat-> array
asas.qdr = np.array(qdlst[0]) # degrees
I = np.eye(traf.ntraf) # Identity matric of order ntraf
asas.dist = np.array(qdlst[1]) * nm + 1e9 * I # meters i to j
# Transmission noise
if traf.adsb.transnoise:
# error in the determined bearing between two a/c
bearingerror = np.random.normal(0, traf.adsb.transerror[0], asas.qdr.shape) # degrees
asas.qdr += bearingerror
# error in the perceived distance between two a/c
disterror = np.random.normal(0, traf.adsb.transerror[1], asas.dist.shape) # meters
asas.dist += disterror
# Calculate horizontal closest point of approach (CPA)
qdrrad = np.radians(asas.qdr)
asas.dx = asas.dist * np.sin(qdrrad) # is pos j rel to i
asas.dy = asas.dist * np.cos(qdrrad) # is pos j rel to i
trkrad = np.radians(traf.trk)
asas.u = traf.gs * np.sin(trkrad).reshape((1, len(trkrad))) # m/s
asas.v = traf.gs * np.cos(trkrad).reshape((1, len(trkrad))) # m/s
# parameters received through ADSB
adsbtrkrad = np.radians(traf.adsb.trk)
adsbu = traf.adsb.gs * np.sin(adsbtrkrad).reshape((1, len(adsbtrkrad))) # m/s
adsbv = traf.adsb.gs * np.cos(adsbtrkrad).reshape((1, len(adsbtrkrad))) # m/s
du = asas.u - adsbu.T # Speed du[i,j] is perceived eastern speed of i to j
dv = asas.v - adsbv.T # Speed dv[i,j] is perceived northern speed of i to j
dv2 = du * du + dv * dv
dv2 = np.where(np.abs(dv2) < 1e-6, 1e-6, dv2) # limit lower absolute value
vrel = np.sqrt(dv2)
asas.tcpa = -(du * asas.dx + dv * asas.dy) / dv2 + 1e9 * I
# Calculate CPA positions
# xcpa = asas.tcpa * du
# ycpa = asas.tcpa * dv
# Calculate distance^2 at CPA (minimum distance^2)
dcpa2 = asas.dist * asas.dist - asas.tcpa * asas.tcpa * dv2
# Check for horizontal conflict
R2 = asas.R * asas.R
swhorconf = dcpa2 < R2 # conflict or not
# Calculate times of entering and leaving horizontal conflict
dxinhor = np.sqrt(np.maximum(0., R2 - dcpa2)) # half the distance travelled inzide zone
dtinhor = dxinhor / vrel
tinhor = np.where(swhorconf, asas.tcpa - dtinhor, 1e8) # Set very large if no conf
touthor = np.where(swhorconf, asas.tcpa + dtinhor, -1e8) # set very large if no conf
# swhorconf = swhorconf*(touthor>0)*(tinhor<asas.dtlook)
# Vertical conflict -----------------------------------------------------------
# Vertical crossing of disk (-dh,+dh)
alt = traf.alt.reshape((1, traf.ntraf))
adsbalt = traf.adsb.alt.reshape((1, traf.ntraf))
if traf.adsb.transnoise:
# error in the determined altitude of other a/c
alterror = np.random.normal(0, traf.adsb.transerror[2], traf.alt.shape) # degrees
adsbalt += alterror
asas.dalt = alt - adsbalt.T
vs = traf.vs.reshape(1, len(traf.vs))
avs = traf.adsb.vs.reshape(1, len(traf.adsb.vs))
dvs = vs - avs.T
# Check for passing through each others zone
dvs = np.where(np.abs(dvs) < 1e-6, 1e-6, dvs) # prevent division by zero
tcrosshi = (asas.dalt + asas.dh) / -dvs
tcrosslo = (asas.dalt - asas.dh) / -dvs
tinver = np.minimum(tcrosshi, tcrosslo)
toutver = np.maximum(tcrosshi, tcrosslo)
# Combine vertical and horizontal conflict-------------------------------------
asas.tinconf = np.maximum(tinver, tinhor)
asas.toutconf = np.minimum(toutver, touthor)
swconfl = swhorconf * (asas.tinconf <= asas.toutconf) * \
(asas.toutconf > 0.) * (asas.tinconf < asas.dtlookahead) \
* (1. - I)
# ----------------------------------------------------------------------
# Update conflict lists
# ----------------------------------------------------------------------
if len(swconfl) == 0:
return
# Calculate CPA positions of traffic in lat/lon?
# Select conflicting pairs: each a/c gets their own record
confidxs = np.where(swconfl)
iown = confidxs[0]
ioth = confidxs[1]
# Store result
asas.nconf = len(confidxs[0])
for idx in range(asas.nconf):
i = iown[idx]
j = ioth[idx]
if i == j:
continue
asas.iconf[i].append(idx)
asas.confpairs.append((traf.id[i], traf.id[j]))
rng = asas.tcpa[i, j] * traf.gs[i] / nm
lato, lono = geo.qdrpos(traf.lat[i], traf.lon[i], traf.trk[i], rng)
alto = traf.alt[i] + asas.tcpa[i, j] * traf.vs[i]
asas.latowncpa.append(lato)
asas.lonowncpa.append(lono)
asas.altowncpa.append(alto)
dx = (traf.lat[i] - traf.lat[j]) * 111319.
dy = (traf.lon[i] - traf.lon[j]) * 111319.
hdist2 = dx**2 + dy**2
hLOS = hdist2 < asas.R**2
vdist = abs(traf.alt[i] - traf.alt[j])
vLOS = vdist < asas.dh
LOS = (hLOS & vLOS)
# Add to Conflict and LOSlist, to count total conflicts and LOS
# NB: if only one A/C detects a conflict, it is also added to these lists
combi = str(traf.id[i]) + " " + str(traf.id[j])
combi2 = str(traf.id[j]) + " " + str(traf.id[i])
experimenttime = simt > 2100 and simt < 5700 # These parameters may be
# changed to count only conflicts within a given expirement time window
if combi not in asas.conflist_all and combi2 not in asas.conflist_all:
asas.conflist_all.append(combi)
if combi not in asas.conflist_exp and combi2 not in asas.conflist_exp and experimenttime:
asas.conflist_exp.append(combi)
if combi not in asas.conflist_now and combi2 not in asas.conflist_now:
asas.conflist_now.append(combi)
if LOS:
if combi not in asas.LOSlist_all and combi2 not in asas.LOSlist_all:
asas.LOSlist_all.append(combi)
asas.LOSmaxsev.append(0.)
asas.LOShmaxsev.append(0.)
asas.LOSvmaxsev.append(0.)
if combi not in asas.LOSlist_exp and combi2 not in asas.LOSlist_exp and experimenttime:
asas.LOSlist_exp.append(combi)
if combi not in asas.LOSlist_now and combi2 not in asas.LOSlist_now:
asas.LOSlist_now.append(combi)
# Now, we measure intrusion and store it if it is the most severe
Ih = 1.0 - np.sqrt(hdist2) / asas.R
Iv = 1.0 - vdist / asas.dh
severity = min(Ih, Iv)
try: # Only continue if combi is found in LOSlist (and not combi2)
idx = asas.LOSlist_all.index(combi)
except:
idx = -1
if idx >= 0:
if severity > asas.LOSmaxsev[idx]:
asas.LOSmaxsev[idx] = severity
asas.LOShmaxsev[idx] = Ih
asas.LOSvmaxsev[idx] = Iv
# Convert to numpy arrays for vectorisation
asas.latowncpa = np.array(asas.latowncpa)
asas.lonowncpa = np.array(asas.lonowncpa)
asas.altowncpa = np.array(asas.altowncpa)
# Calculate whether ASAS or A/P commands should be followed
ResumeNav(asas, traf)
def update_aircraft_data(self, data):
if not self.initialized:
return
self.makeCurrent()
actdata = bs.net.get_nodedata()
if actdata.filteralt:
idx = np.where((data.alt >= actdata.filteralt[0]) * (data.alt <= actdata.filteralt[1]))
data.lat = data.lat[idx]
data.lon = data.lon[idx]
data.trk = data.trk[idx]
data.alt = data.alt[idx]
data.tas = data.tas[idx]
data.vs = data.vs[idx]
self.naircraft = len(data.lat)
actdata.translvl = data.translvl
self.asas_vmin = data.vmin
self.asas_vmax = data.vmax
if self.naircraft == 0:
self.cpalines.set_vertex_count(0)
else:
# Update data in GPU buffers
update_buffer(self.aclatbuf, np.array(data.lat[:MAX_NAIRCRAFT], dtype=np.float32))
update_buffer(self.aclonbuf, np.array(data.lon[:MAX_NAIRCRAFT], dtype=np.float32))
update_buffer(self.achdgbuf, np.array(data.trk[:MAX_NAIRCRAFT], dtype=np.float32))
update_buffer(self.acaltbuf, np.array(data.alt[:MAX_NAIRCRAFT], dtype=np.float32))
update_buffer(self.actasbuf, np.array(data.tas[:MAX_NAIRCRAFT], dtype=np.float32))
update_buffer(self.asasnbuf, np.array(data.asasn[:MAX_NAIRCRAFT], dtype=np.float32))
update_buffer(self.asasebuf, np.array(data.asase[:MAX_NAIRCRAFT], dtype=np.float32))
# CPA lines to indicate conflicts
ncpalines = np.count_nonzero(data.inconf)
cpalines = np.zeros(4 * ncpalines, dtype=np.float32)
self.cpalines.set_vertex_count(2 * ncpalines)
# Labels and colors
rawlabel = ''
color = np.empty((min(self.naircraft, MAX_NAIRCRAFT), 4), dtype=np.uint8)
selssd = np.zeros(self.naircraft, dtype=np.uint8)
confidx = 0
zdata = zip(data.id, data.inconf, data.tcpamax, data.trk, data.gs,
data.cas, data.vs, data.alt, data.lat, data.lon)
for i, (acid, inconf, tcpa, trk, gs, cas, vs, alt, lat, lon) in enumerate(zdata):
if i >= MAX_NAIRCRAFT:
break
# Make label: 3 lines of 8 characters per aircraft
if actdata.show_lbl >= 1:
rawlabel += '%-8s' % acid[:8]
if actdata.show_lbl == 2:
if alt <= data.translvl:
rawlabel += '%-5d' % int(alt / ft + 0.5)
else:
rawlabel += 'FL%03d' % int(alt / ft / 100. + 0.5)
vsarrow = 30 if vs > 0.25 else 31 if vs < -0.25 else 32
rawlabel += '%1s %-8d' % (chr(vsarrow), int(cas / kts + 0.5))
else:
rawlabel += 16 * ' '
if inconf:
if actdata.ssd_conflicts:
selssd[i] = 255
color[i, :] = palette.conflict + (255,)
lat1, lon1 = geo.qdrpos(lat, lon, trk, tcpa * gs / nm)
cpalines[4 * confidx : 4 * confidx + 4] = [lat, lon, lat1, lon1]
confidx += 1
else:
color[i, :] = palette.aircraft + (255,)
# Check if aircraft is selected to show SSD
if actdata.ssd_all or acid in actdata.ssd_ownship:
selssd[i] = 255
if len(actdata.ssd_ownship) > 0 or actdata.ssd_conflicts or actdata.ssd_all:
update_buffer(self.ssd.selssdbuf, selssd[:MAX_NAIRCRAFT])
update_buffer(self.confcpabuf, cpalines[:MAX_NCONFLICTS * 4])
update_buffer(self.accolorbuf, color)
update_buffer(self.aclblbuf, np.array(rawlabel.encode('utf8'), dtype=np.string_))
# If there is a visible route, update the start position
if self.route_acid != "":
if self.route_acid in data.id:
idx = data.id.index(self.route_acid)
update_buffer(self.routebuf,
np.array([data.lat[idx], data.lon[idx]], dtype=np.float32))
# Update trails database with new lines
if data.swtrails:
actdata.traillat0.extend(data.traillat0)
actdata.traillon0.extend(data.traillon0)
actdata.traillat1.extend(data.traillat1)
actdata.traillon1.extend(data.traillon1)
update_buffer(self.trailbuf, np.array(
list(zip(actdata.traillat0, actdata.traillon0,
actdata.traillat1, actdata.traillon1)) +
list(zip(data.traillastlat, data.traillastlon,
list(data.lat), list(data.lon))),
dtype=np.float32))
self.traillines.set_vertex_count(2 * len(actdata.traillat0) +
2 * len(data.lat))
else:
actdata.traillat0 = []
actdata.traillon0 = []
actdata.traillat1 = []
actdata.traillon1 = []
self.traillines.set_vertex_count(0)
def detect(asas, traf, simt):
if not asas.swasas:
return
# Reset lists before new CD
asas.iconf = [[] for ac in range(traf.ntraf)]
asas.nconf = 0
asas.confpairs = []
asas.latowncpa = []
asas.lonowncpa = []
asas.altowncpa = []
asas.LOSlist_now = []
asas.conflist_now = []
# Horizontal conflict ---------------------------------------------------------
# qdlst is for [i,j] qdr from i to j, from perception of ADSB and own coordinates
qdlst = geo.qdrdist_matrix(np.mat(traf.lat), np.mat(traf.lon),
np.mat(traf.adsb.lat), np.mat(traf.adsb.lon))
# Convert results from mat-> array
asas.qdr = np.array(qdlst[0]) # degrees
I = np.eye(traf.ntraf) # Identity matric of order ntraf
asas.dist = np.array(qdlst[1]) * nm + 1e9 * I # meters i to j
# Transmission noise
if traf.adsb.transnoise:
# error in the determined bearing between two a/c
bearingerror = np.random.normal(0, traf.adsb.transerror[0],
asas.qdr.shape) # degrees
asas.qdr += bearingerror
# error in the perceived distance between two a/c
disterror = np.random.normal(0, traf.adsb.transerror[1],
asas.dist.shape) # meters
asas.dist += disterror
# Calculate horizontal closest point of approach (CPA)
qdrrad = np.radians(asas.qdr)
asas.dx = asas.dist * np.sin(qdrrad) # is pos j rel to i
asas.dy = asas.dist * np.cos(qdrrad) # is pos j rel to i
trkrad = np.radians(traf.trk)
asas.u = traf.gs * np.sin(trkrad).reshape((1, len(trkrad))) # m/s
asas.v = traf.gs * np.cos(trkrad).reshape((1, len(trkrad))) # m/s
# parameters received through ADSB
adsbtrkrad = np.radians(traf.adsb.trk)
adsbu = traf.adsb.gs * np.sin(adsbtrkrad).reshape(
(1, len(adsbtrkrad))) # m/s
adsbv = traf.adsb.gs * np.cos(adsbtrkrad).reshape(
(1, len(adsbtrkrad))) # m/s
du = asas.u - adsbu.T # Speed du[i,j] is perceived eastern speed of i to j
dv = asas.v - adsbv.T # Speed dv[i,j] is perceived northern speed of i to j
dv2 = du * du + dv * dv
dv2 = np.where(np.abs(dv2) < 1e-6, 1e-6, dv2) # limit lower absolute value
vrel = np.sqrt(dv2)
asas.tcpa = -(du * asas.dx + dv * asas.dy) / dv2 + 1e9 * I
# Calculate CPA positions
# xcpa = asas.tcpa * du
# ycpa = asas.tcpa * dv
# Calculate distance^2 at CPA (minimum distance^2)
dcpa2 = asas.dist * asas.dist - asas.tcpa * asas.tcpa * dv2
# Check for horizontal conflict
R2 = asas.R * asas.R
swhorconf = dcpa2 < R2 # conflict or not
# Calculate times of entering and leaving horizontal conflict
dxinhor = np.sqrt(np.maximum(
0., R2 - dcpa2)) # half the distance travelled inzide zone
dtinhor = dxinhor / vrel
tinhor = np.where(swhorconf, asas.tcpa - dtinhor,
1e8) # Set very large if no conf
touthor = np.where(swhorconf, asas.tcpa + dtinhor,
-1e8) # set very large if no conf
# swhorconf = swhorconf*(touthor>0)*(tinhor<asas.dtlook)
# Vertical conflict -----------------------------------------------------------
# Vertical crossing of disk (-dh,+dh)
alt = traf.alt.reshape((1, traf.ntraf))
adsbalt = traf.adsb.alt.reshape((1, traf.ntraf))
if traf.adsb.transnoise:
# error in the determined altitude of other a/c
alterror = np.random.normal(0, traf.adsb.transerror[2],
traf.alt.shape) # degrees
adsbalt += alterror
asas.dalt = alt - adsbalt.T
vs = traf.vs.reshape(1, len(traf.vs))
avs = traf.adsb.vs.reshape(1, len(traf.adsb.vs))
dvs = vs - avs.T
# Check for passing through each others zone
dvs = np.where(np.abs(dvs) < 1e-6, 1e-6, dvs) # prevent division by zero
tcrosshi = (asas.dalt + asas.dh) / -dvs
tcrosslo = (asas.dalt - asas.dh) / -dvs
tinver = np.minimum(tcrosshi, tcrosslo)
toutver = np.maximum(tcrosshi, tcrosslo)
# Combine vertical and horizontal conflict-------------------------------------
asas.tinconf = np.maximum(tinver, tinhor)
asas.toutconf = np.minimum(toutver, touthor)
swconfl = swhorconf * (asas.tinconf <= asas.toutconf) * \
(asas.toutconf > 0.) * (asas.tinconf < asas.dtlookahead) \
* (1. - I)
# ----------------------------------------------------------------------
# Update conflict lists
# ----------------------------------------------------------------------
if len(swconfl) == 0:
return
# Calculate CPA positions of traffic in lat/lon?
# Select conflicting pairs: each a/c gets their own record
confidxs = np.where(swconfl)
iown = confidxs[0]
ioth = confidxs[1]
# Store result
asas.nconf = len(confidxs[0])
for idx in range(asas.nconf):
i = iown[idx]
j = ioth[idx]
if i == j:
continue
asas.iconf[i].append(idx)
asas.confpairs.append((traf.id[i], traf.id[j]))
rng = asas.tcpa[i, j] * traf.gs[i] / nm
lato, lono = geo.qdrpos(traf.lat[i], traf.lon[i], traf.trk[i], rng)
alto = traf.alt[i] + asas.tcpa[i, j] * traf.vs[i]
asas.latowncpa.append(lato)
asas.lonowncpa.append(lono)
asas.altowncpa.append(alto)
dx = (traf.lat[i] - traf.lat[j]) * 111319.
dy = (traf.lon[i] - traf.lon[j]) * 111319.
hdist2 = dx**2 + dy**2
hLOS = hdist2 < asas.R**2
vdist = abs(traf.alt[i] - traf.alt[j])
vLOS = vdist < asas.dh
LOS = (hLOS & vLOS)
# Add to Conflict and LOSlist, to count total conflicts and LOS
# NB: if only one A/C detects a conflict, it is also added to these lists
combi = str(traf.id[i]) + " " + str(traf.id[j])
combi2 = str(traf.id[j]) + " " + str(traf.id[i])
experimenttime = simt > 2100 and simt < 5700 # These parameters may be
# changed to count only conflicts within a given expirement time window
if combi not in asas.conflist_all and combi2 not in asas.conflist_all:
asas.conflist_all.append(combi)
if combi not in asas.conflist_exp and combi2 not in asas.conflist_exp and experimenttime:
asas.conflist_exp.append(combi)
if combi not in asas.conflist_now and combi2 not in asas.conflist_now:
asas.conflist_now.append(combi)
if LOS:
if combi not in asas.LOSlist_all and combi2 not in asas.LOSlist_all:
asas.LOSlist_all.append(combi)
asas.LOSmaxsev.append(0.)
asas.LOShmaxsev.append(0.)
asas.LOSvmaxsev.append(0.)
if combi not in asas.LOSlist_exp and combi2 not in asas.LOSlist_exp and experimenttime:
asas.LOSlist_exp.append(combi)
if combi not in asas.LOSlist_now and combi2 not in asas.LOSlist_now:
asas.LOSlist_now.append(combi)
# Now, we measure intrusion and store it if it is the most severe
Ih = 1.0 - np.sqrt(hdist2) / asas.R
Iv = 1.0 - vdist / asas.dh
severity = min(Ih, Iv)
try: # Only continue if combi is found in LOSlist (and not combi2)
idx = asas.LOSlist_all.index(combi)
except:
idx = -1
if idx >= 0:
if severity > asas.LOSmaxsev[idx]:
asas.LOSmaxsev[idx] = severity
asas.LOShmaxsev[idx] = Ih
asas.LOSvmaxsev[idx] = Iv
# Convert to numpy arrays for vectorisation
asas.latowncpa = np.array(asas.latowncpa)
asas.lonowncpa = np.array(asas.lonowncpa)
asas.altowncpa = np.array(asas.altowncpa)
# Calculate whether ASAS or A/P commands should be followed
ResumeNav(asas, traf)
