def generate_shared_observation(self):
"""
Generate the shared observation sequences for the aircraft
States are distance, heading, bearing, latitude longitude, (speed)
The observation are Ntraf sequences of size (Ntraf - 1 , x)
"""
minlat, maxlat = 50.75428888888889, 55.
minlon, maxlon = 2., 7.216944444444445
# Generate distance matrix
qdr, dist = qdrdist_matrix(np.mat(traf.lat), np.mat(traf.lon), np.mat(traf.lat), np.mat(traf.lon))
qdr, dist = np.array(qdr), np.array(dist)
shared_obs = np.zeros((traf.ntraf, traf.ntraf, self.shared_state_size))
destidx = navdb.getaptidx('EHAM')
lat, lon = navdb.aptlat[destidx], navdb.aptlon[destidx]
qdr1, dist1 = qdrdist(traf.lat, traf.lon, lat * np.ones(traf.lat.shape), lon * np.ones(traf.lon.shape))
for i in range(traf.ntraf):
# shared_obs = np.zeros((traf.ntraf - 1, self.shared_state_size))
shared_obs[i, :, 0] = normalize(dist[i,:], 0, 200)
shared_obs[i, :, 1] = degtoqdr180(traf.hdg, qdr1)/180. # divide by 180
shared_obs[i, :, 2] = normalize(traf.lat, minlat, maxlat)
shared_obs[i, :, 3] = normalize(traf.lon, minlon, maxlon)
# if traf.ntraf==1:
# shared_obs = np.zeros((traf.ntraf+1, traf.ntraf, self.shared_state_size))
# print('shard_obs', shared_obs.shape)
return shared_obs
def detect_los(ownship, intruder, RPZ, HPZ):
'''
Stripped down version of the conflict detection in ASAS without fixed
update rate to allow faster running of bluesky when only LOS detection
is required.
'''
# Identity matrix of order ntraf: avoid ownship-ownship detected conflicts
I = np.eye(ownship.ntraf)
# Horizontal conflict ------------------------------------------------------
# qdlst is for [i,j] qdr from i to j, from perception of ADSB and own coordinates
qdr, dist = geo.qdrdist_matrix(np.mat(ownship.lat), np.mat(ownship.lon),
np.mat(intruder.lat), np.mat(intruder.lon))
# Convert back to array to allow element-wise array multiplications later on
# Convert to meters and add large value to own/own pairs
qdr = np.array(qdr)
dist = np.array(dist) * nm + 1e9 * I
# Vertical crossing of disk (-dh,+dh)
dalt = ownship.alt.reshape((1, ownship.ntraf)) - \
intruder.alt.reshape((1, ownship.ntraf)).T + 1e9 * I
swlos = (dist < RPZ) * (np.abs(dalt) < HPZ)
los_pairs = [(ownship.id[i], ownship.id[j]) for i, j in zip(*np.where(swlos))]
return los_pairs
def getXYPosition(plat, plon, p0lat, p0lon):
[qdr, distance] = geo.qdrdist_matrix(p0lat, p0lon, plat, plon)
qdr = np.squeeze(np.asarray(qdr))
distance = np.squeeze(np.asarray(distance))
qdr_radians = np.deg2rad(qdr)
distance = distance * nm
return np.multiply(distance, np.sin(qdr_radians)), np.multiply(
distance, np.cos(qdr_radians)), distance, qdr % 360
def detect(ownship, intruder, RPZ, HPZ, tlookahead):
''' Conflict detection between ownship (traf) and intruder (traf/adsb).'''
# Identity matrix of order ntraf: avoid ownship-ownship detected conflicts
I = np.eye(ownship.ntraf)
# Horizontal conflict ------------------------------------------------------
# qdlst is for [i,j] qdr from i to j, from perception of ADSB and own coordinates
qdr, dist = geo.qdrdist_matrix(np.mat(ownship.lat), np.mat(ownship.lon),
np.mat(intruder.lat), np.mat(intruder.lon))
# Convert back to array to allow element-wise array multiplications later on
# Convert to meters and add large value to own/own pairs
qdr = np.array(qdr)
dist = np.array(dist) * nm + 1e9 * I
# Calculate horizontal closest point of approach (CPA)
qdrrad = np.radians(qdr)
dx = dist * np.sin(qdrrad) # is pos j rel to i
dy = dist * np.cos(qdrrad) # is pos j rel to i
# Ownship track angle and speed
owntrkrad = np.radians(ownship.trk)
ownu = ownship.gs * np.sin(owntrkrad).reshape((1, ownship.ntraf)) # m/s
ownv = ownship.gs * np.cos(owntrkrad).reshape((1, ownship.ntraf)) # m/s
# Intruder track angle and speed
inttrkrad = np.radians(intruder.trk)
intu = intruder.gs * np.sin(inttrkrad).reshape((1, ownship.ntraf)) # m/s
intv = intruder.gs * np.cos(inttrkrad).reshape((1, ownship.ntraf)) # m/s
du = ownu - intu.T # Speed du[i,j] is perceived eastern speed of i to j
dv = ownv - intv.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)
tcpa = -(du * dx + dv * dy) / dv2 + 1e9 * I
# Calculate distance^2 at CPA (minimum distance^2)
dcpa2 = np.abs(dist * dist - tcpa * tcpa * dv2)
# Check for horizontal conflict
R2 = RPZ * RPZ
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, tcpa - dtinhor,
1e8) # Set very large if no conf
touthor = np.where(swhorconf, tcpa + dtinhor,
-1e8) # set very large if no conf
# Vertical conflict --------------------------------------------------------
# Vertical crossing of disk (-dh,+dh)
dalt = ownship.alt.reshape((1, ownship.ntraf)) - \
intruder.alt.reshape((1, ownship.ntraf)).T + 1e9 * I
dvs = ownship.vs.reshape(1, ownship.ntraf) - \
intruder.vs.reshape(1, ownship.ntraf).T
dvs = np.where(np.abs(dvs) < 1e-6, 1e-6, dvs) # prevent division by zero
# Check for passing through each others zone
tcrosshi = (dalt + HPZ) / -dvs
tcrosslo = (dalt - HPZ) / -dvs
tinver = np.minimum(tcrosshi, tcrosslo)
toutver = np.maximum(tcrosshi, tcrosslo)
# Combine vertical and horizontal conflict----------------------------------
tinconf = np.maximum(tinver, tinhor)
toutconf = np.minimum(toutver, touthor)
swconfl = np.array(swhorconf * (tinconf <= toutconf) * (toutconf > 0.0) * \
(tinconf < tlookahead) * (1.0 - I), dtype=np.bool)
# --------------------------------------------------------------------------
# Update conflict lists
# --------------------------------------------------------------------------
# Ownship conflict flag and max tCPA
inconf = np.any(swconfl, 1)
tcpamax = np.max(tcpa * swconfl, 1)
# Select conflicting pairs: each a/c gets their own record
confpairs = [(ownship.id[i], ownship.id[j])
for i, j in zip(*np.where(swconfl))]
swlos = (dist < RPZ) * (np.abs(dalt) < HPZ)
lospairs = [(ownship.id[i], ownship.id[j])
for i, j in zip(*np.where(swlos))]
# bearing, dist, tcpa, tinconf, toutconf per conflict
qdr = qdr[swconfl]
dist = dist[swconfl]
tcpa = tcpa[swconfl]
dcpa = np.sqrt(dcpa2[swconfl])
tinconf = tinconf[swconfl]
return confpairs, lospairs, inconf, tcpamax, qdr, dist, dcpa, tcpa, tinconf
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 constructSSD(asas, traf, priocode="RS1"):
""" Calculates the FRV and ARV of the SSD """
N = 0
# Parameters
N_angle = 180 # [-] Number of points on circle (discretization)
vmin = asas.vmin # [m/s] Defined in asas.py
vmax = asas.vmax # [m/s] Defined in asas.py
hsep = asas.R # [m] Horizontal separation (5 NM)
margin = asas.mar # [-] Safety margin for evasion
hsepm = hsep * margin # [m] Horizontal separation with safety margin
alpham = 0.4999 * np.pi # [rad] Maximum half-angle for VO
betalos = np.pi / 4 # [rad] Minimum divertion angle for LOS (45 deg seems optimal)
adsbmax = 65. * nm # [m] Maximum ADS-B range
beta = np.pi / 4 + betalos / 2
if priocode == "RS7" or priocode == "RS8":
adsbmax /= 2
# Relevant info from traf
gsnorth = traf.gsnorth
gseast = traf.gseast
lat = traf.lat
lon = traf.lon
ntraf = traf.ntraf
hdg = traf.hdg
gs_ap = traf.ap.tas
hdg_ap = traf.ap.trk
apnorth = np.cos(hdg_ap / 180 * np.pi) * gs_ap
apeast = np.sin(hdg_ap / 180 * np.pi) * gs_ap
# Local variables, will be put into asas later
FRV_loc = [None] * traf.ntraf
ARV_loc = [None] * traf.ntraf
# For calculation purposes
ARV_calc_loc = [None] * traf.ntraf
FRV_area_loc = np.zeros(traf.ntraf, dtype=np.float32)
ARV_area_loc = np.zeros(traf.ntraf, dtype=np.float32)
# # Use velocity limits for the ring-shaped part of the SSD
# Discretize the circles using points on circle
angles = np.arange(0, 2 * np.pi, 2 * np.pi / N_angle)
# Put points of unit-circle in a (180x2)-array (CW)
xyc = np.transpose(
np.reshape(np.concatenate((np.sin(angles), np.cos(angles))),
(2, N_angle)))
# Map them into the format pyclipper wants. Outercircle CCW, innercircle CW
circle_tup = (tuple(map(tuple, np.flipud(xyc * vmax))),
tuple(map(tuple, xyc * vmin)))
circle_lst = [
list(map(list, np.flipud(xyc * vmax))),
list(map(list, xyc * vmin))
]
# If no traffic
if ntraf == 0:
return
# If only one aircraft
elif ntraf == 1:
# Map them into the format ARV wants. Outercircle CCW, innercircle CW
ARV_loc[0] = circle_lst
FRV_loc[0] = []
ARV_calc_loc[0] = ARV_loc[0]
# Calculate areas and store in asas
FRV_area_loc[0] = 0
ARV_area_loc[0] = np.pi * (vmax**2 - vmin**2)
return
# Function qdrdist_matrix needs 4 vectors as input (lat1,lon1,lat2,lon2)
# To be efficient, calculate all qdr and dist in one function call
# Example with ntraf = 5: ind1 = [0,0,0,0,1,1,1,2,2,3]
# ind2 = [1,2,3,4,2,3,4,3,4,4]
# This way the qdrdist is only calculated once between every aircraft
# To get all combinations, use this function to get the indices
ind1, ind2 = qdrdist_matrix_indices(ntraf)
# Get absolute bearing [deg] and distance [nm]
# Not sure abs/rel, but qdr is defined from [-180,180] deg, w.r.t. North
[qdr, dist] = geo.qdrdist_matrix(lat[ind1], lon[ind1], lat[ind2],
lon[ind2])
# Put result of function from matrix to ndarray
qdr = np.reshape(np.array(qdr), np.shape(ind1))
dist = np.reshape(np.array(dist), np.shape(ind1))
# SI-units from [deg] to [rad]
qdr = np.deg2rad(qdr)
# Get distance from [nm] to [m]
dist = dist * nm
# In LoS the VO can't be defined, act as if dist is on edge
dist[dist < hsepm] = hsepm
# Calculate vertices of Velocity Obstacle (CCW)
# These are still in relative velocity space, see derivation in appendix
# Half-angle of the Velocity obstacle [rad]
# Include safety margin
alpha = np.arcsin(hsepm / dist)
# Limit half-angle alpha to 89.982 deg. Ensures that VO can be constructed
alpha[alpha > alpham] = alpham
# Relevant sin/cos/tan
sinqdr = np.sin(qdr)
cosqdr = np.cos(qdr)
tanalpha = np.tan(alpha)
cosqdrtanalpha = cosqdr * tanalpha
sinqdrtanalpha = sinqdr * tanalpha
# Relevant x1,y1,x2,y2 (x0 and y0 are zero in relative velocity space)
x1 = (sinqdr + cosqdrtanalpha) * 2 * vmax
x2 = (sinqdr - cosqdrtanalpha) * 2 * vmax
y1 = (cosqdr - sinqdrtanalpha) * 2 * vmax
y2 = (cosqdr + sinqdrtanalpha) * 2 * vmax
# Consider every aircraft
for i in range(ntraf):
# Calculate SSD only for aircraft in conflict (See formulas appendix)
if asas.inconf[i]:
# SSD for aircraft i
# Get indices that belong to aircraft i
ind = np.where(np.logical_or(ind1 == i, ind2 == i))[0]
# Check whether there are any aircraft in the vicinity
if len(ind) == 0:
# No aircraft in the vicinity
# Map them into the format ARV wants. Outercircle CCW, innercircle CW
ARV_loc[i] = circle_lst
FRV_loc[i] = []
ARV_calc_loc[i] = ARV_loc[i]
# Calculate areas and store in asas
FRV_area_loc[i] = 0
ARV_area_loc[i] = np.pi * (vmax**2 - vmin**2)
else:
# The i's of the other aircraft
i_other = np.delete(np.arange(0, ntraf), i)
# Aircraft that are within ADS-B range
ac_adsb = np.where(dist[ind] < adsbmax)[0]
# Now account for ADS-B range in indices of other aircraft (i_other)
ind = ind[ac_adsb]
i_other = i_other[ac_adsb]
if not priocode == "RS7" and not priocode == "RS8":
# Put it in class-object (not for RS7 and RS8)
asas.inrange[i] = i_other
else:
asas.inrange2[i] = i_other
# VO from 2 to 1 is mirror of 1 to 2. Only 1 to 2 can be constructed in
# this manner, so need a correction vector that will mirror the VO
fix = np.ones(np.shape(i_other))
fix[i_other < i] = -1
# Relative bearing [deg] from [-180,180]
# (less required conversions than rad in RotA)
fix_ang = np.zeros(np.shape(i_other))
fix_ang[i_other < i] = 180.
# Get vertices in an x- and y-array of size (ntraf-1)*3x1
x = np.concatenate(
(gseast[i_other], x1[ind] * fix + gseast[i_other],
x2[ind] * fix + gseast[i_other]))
y = np.concatenate(
(gsnorth[i_other], y1[ind] * fix + gsnorth[i_other],
y2[ind] * fix + gsnorth[i_other]))
# Reshape [(ntraf-1)x3] and put arrays in one array [(ntraf-1)x3x2]
x = np.transpose(x.reshape(3, np.shape(i_other)[0]))
y = np.transpose(y.reshape(3, np.shape(i_other)[0]))
xy = np.dstack((x, y))
# Make a clipper object
pc = pyclipper.Pyclipper()
# Add circles (ring-shape) to clipper as subject
pc.AddPaths(pyclipper.scale_to_clipper(circle_tup),
pyclipper.PT_SUBJECT, True)
# Extra stuff needed for RotA
if priocode == "RS6":
# Make another clipper object for RotA
pc_rota = pyclipper.Pyclipper()
pc_rota.AddPaths(pyclipper.scale_to_clipper(circle_tup),
pyclipper.PT_SUBJECT, True)
# Bearing calculations from own view and other view
brg_own = np.mod(
(np.rad2deg(qdr[ind]) + fix_ang - hdg[i]) + 540.,
360.) - 180.
brg_other = np.mod(
(np.rad2deg(qdr[ind]) + 180. - fix_ang - hdg[i_other])
+ 540., 360.) - 180.
# Add each other other aircraft to clipper as clip
for j in range(np.shape(i_other)[0]):
## Debug prints
## print(traf.id[i] + " - " + traf.id[i_other[j]])
## print(dist[ind[j]])
# Scale VO when not in LOS
if dist[ind[j]] > hsepm:
# Normally VO shall be added of this other a/c
VO = pyclipper.scale_to_clipper(
tuple(map(tuple, xy[j, :, :])))
else:
# Pair is in LOS, instead of triangular VO, use darttip
# Check if bearing should be mirrored
if i_other[j] < i:
qdr_los = qdr[ind[j]] + np.pi
else:
qdr_los = qdr[ind[j]]
# Length of inner-leg of darttip
leg = 1.1 * vmax / np.cos(beta) * np.array(
[1, 1, 1, 0])
# Angles of darttip
angles_los = np.array([
qdr_los + 2 * beta, qdr_los, qdr_los - 2 * beta, 0.
])
# Calculate coordinates (CCW)
x_los = leg * np.sin(angles_los)
y_los = leg * np.cos(angles_los)
# Put in array of correct format
xy_los = np.vstack((x_los, y_los)).T
# Scale darttip
VO = pyclipper.scale_to_clipper(
tuple(map(tuple, xy_los)))
# Add scaled VO to clipper
pc.AddPath(VO, pyclipper.PT_CLIP, True)
# For RotA it is possible to ignore
if priocode == "RS6":
if brg_own[j] >= -20. and brg_own[j] <= 110.:
# Head-on or converging from right
pc_rota.AddPath(VO, pyclipper.PT_CLIP, True)
elif brg_other[j] <= -110. or brg_other[j] >= 110.:
# In overtaking position
pc_rota.AddPath(VO, pyclipper.PT_CLIP, True)
# Detect conflicts for smaller layer in RS7 and RS8
if priocode == "RS7" or priocode == "RS8":
if pyclipper.PointInPolygon(
pyclipper.scale_to_clipper(
(gseast[i], gsnorth[i])), VO):
asas.inconf2[i] = True
if priocode == "RS5":
if pyclipper.PointInPolygon(
pyclipper.scale_to_clipper(
(apeast[i], apnorth[i])), VO):
asas.ap_free[i] = False
# Execute clipper command
FRV = pyclipper.scale_from_clipper(
pc.Execute(pyclipper.CT_INTERSECTION,
pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO))
ARV = pc.Execute(pyclipper.CT_DIFFERENCE,
pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)
if not priocode == "RS1" and not priocode == "RS5" and not priocode == "RS7" and not priocode == "RS8":
# Make another clipper object for extra intersections
pc2 = pyclipper.Pyclipper()
# When using RotA clip with pc_rota
if priocode == "RS6":
# Calculate ARV for RotA
ARV_rota = pc_rota.Execute(pyclipper.CT_DIFFERENCE,
pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO)
if len(ARV_rota) > 0:
pc2.AddPaths(ARV_rota, pyclipper.PT_CLIP, True)
else:
# Put the ARV in there, make sure it's not empty
if len(ARV) > 0:
pc2.AddPaths(ARV, pyclipper.PT_CLIP, True)
# Scale back
ARV = pyclipper.scale_from_clipper(ARV)
# Check if ARV or FRV is empty
if len(ARV) == 0:
# No aircraft in the vicinity
# Map them into the format ARV wants. Outercircle CCW, innercircle CW
ARV_loc[i] = []
FRV_loc[i] = circle_lst
ARV_calc_loc[i] = []
# Calculate areas and store in asas
FRV_area_loc[i] = np.pi * (vmax**2 - vmin**2)
ARV_area_loc[i] = 0
elif len(FRV) == 0:
# Should not happen with one a/c or no other a/c in the vicinity.
# These are handled earlier. Happens when RotA has removed all
# Map them into the format ARV wants. Outercircle CCW, innercircle CW
ARV_loc[i] = circle_lst
FRV_loc[i] = []
ARV_calc_loc[i] = circle_lst
# Calculate areas and store in asas
FRV_area_loc[i] = 0
ARV_area_loc[i] = np.pi * (vmax**2 - vmin**2)
else:
# Check multi exteriors, if this layer is not a list, it means it has no exteriors
# In that case, make it a list, such that its format is consistent with further code
if not type(FRV[0][0]) == list:
FRV = [FRV]
if not type(ARV[0][0]) == list:
ARV = [ARV]
# Store in asas
FRV_loc[i] = FRV
ARV_loc[i] = ARV
# Calculate areas and store in asas
FRV_area_loc[i] = area(FRV)
ARV_area_loc[i] = area(ARV)
# For resolution purposes sometimes extra intersections are wanted
if priocode == "RS2" or priocode == "RS9" or priocode == "RS6" or priocode == "RS3" or priocode == "RS4":
# Make a box that covers right or left of SSD
own_hdg = hdg[i] * np.pi / 180
# Efficient calculation of box, see notes
if priocode == "RS2" or priocode == "RS6":
# CW or right-turning
sin_table = np.array(
[[1, 0], [-1, 0], [-1, -1], [1, -1]],
dtype=np.float64)
cos_table = np.array(
[[0, 1], [0, -1], [1, -1], [1, 1]],
dtype=np.float64)
elif priocode == "RS9":
# CCW or left-turning
sin_table = np.array(
[[1, 0], [1, 1], [-1, 1], [-1, 0]],
dtype=np.float64)
cos_table = np.array(
[[0, 1], [-1, 1], [-1, -1], [0, -1]],
dtype=np.float64)
# Overlay a part of the full SSD
if priocode == "RS2" or priocode == "RS9" or priocode == "RS6":
# Normalized coordinates of box
xyp = np.sin(own_hdg) * sin_table + np.cos(
own_hdg) * cos_table
# Scale with vmax (and some factor) and put in tuple
part = pyclipper.scale_to_clipper(
tuple(map(tuple, 1.1 * vmax * xyp)))
pc2.AddPath(part, pyclipper.PT_SUBJECT, True)
elif priocode == "RS3":
# Small ring
xyp = (tuple(
map(tuple,
np.flipud(xyc *
min(vmax, gs_ap[i] + 0.1)))),
tuple(
map(tuple,
xyc * max(vmin, gs_ap[i] - 0.1))))
part = pyclipper.scale_to_clipper(xyp)
pc2.AddPaths(part, pyclipper.PT_SUBJECT, True)
elif priocode == "RS4":
hdg_sel = hdg[i] * np.pi / 180
xyp = np.array([[
np.sin(hdg_sel - 0.0087),
np.cos(hdg_sel - 0.0087)
], [0, 0],
[
np.sin(hdg_sel + 0.0087),
np.cos(hdg_sel + 0.0087)
]],
dtype=np.float64)
part = pyclipper.scale_to_clipper(
tuple(map(tuple, 1.1 * vmax * xyp)))
pc2.AddPath(part, pyclipper.PT_SUBJECT, True)
# Execute clipper command
ARV_calc = pyclipper.scale_from_clipper(
pc2.Execute(pyclipper.CT_INTERSECTION,
pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
N += 1
# If no smaller ARV is found, take the full ARV
if len(ARV_calc) == 0:
ARV_calc = ARV
# Check multi exteriors, if this layer is not a list, it means it has no exteriors
# In that case, make it a list, such that its format is consistent with further code
if not type(ARV_calc[0][0]) == list:
ARV_calc = [ARV_calc]
# Shortest way out prio, so use full SSD (ARV_calc = ARV)
else:
ARV_calc = ARV
# Update calculatable ARV for resolutions
ARV_calc_loc[i] = ARV_calc
# If sequential approach, the local should go elsewhere
if not priocode == "RS7" and not priocode == "RS8":
asas.FRV = FRV_loc
asas.ARV = ARV_loc
asas.ARV_calc = ARV_calc_loc
asas.FRV_area = FRV_area_loc
asas.ARV_area = ARV_area_loc
else:
asas.ARV_calc2 = ARV_calc_loc
return
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 detect(ownship, intruder, RPZ, HPZ, tlookahead):
''' Conflict detection between ownship (traf) and intruder (traf/adsb).'''
# Identity matrix of order ntraf: avoid ownship-ownship detected conflicts
I = np.eye(ownship.ntraf)
# Horizontal conflict ------------------------------------------------------
# qdlst is for [i,j] qdr from i to j, from perception of ADSB and own coordinates
qdr, dist = geo.qdrdist_matrix(np.mat(ownship.lat), np.mat(ownship.lon),
np.mat(intruder.lat), np.mat(intruder.lon))
# Convert back to array to allow element-wise array multiplications later on
# Convert to meters and add large value to own/own pairs
qdr = np.array(qdr)
dist = np.array(dist) * nm + 1e9 * I
# Calculate horizontal closest point of approach (CPA)
qdrrad = np.radians(qdr)
dx = dist * np.sin(qdrrad) # is pos j rel to i
dy = dist * np.cos(qdrrad) # is pos j rel to i
# Ownship track angle and speed
owntrkrad = np.radians(ownship.trk)
ownu = ownship.gs * np.sin(owntrkrad).reshape((1, ownship.ntraf)) # m/s
ownv = ownship.gs * np.cos(owntrkrad).reshape((1, ownship.ntraf)) # m/s
# Intruder track angle and speed
inttrkrad = np.radians(intruder.trk)
intu = intruder.gs * np.sin(inttrkrad).reshape((1, ownship.ntraf)) # m/s
intv = intruder.gs * np.cos(inttrkrad).reshape((1, ownship.ntraf)) # m/s
du = ownu - intu.T # Speed du[i,j] is perceived eastern speed of i to j
dv = ownv - intv.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)
tcpa = -(du * dx + dv * dy) / dv2 + 1e9 * I
# Calculate distance^2 at CPA (minimum distance^2)
dcpa2 = dist * dist - tcpa * tcpa * dv2
# Check for horizontal conflict
R2 = RPZ * RPZ
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, tcpa - dtinhor, 1e8) # Set very large if no conf
touthor = np.where(swhorconf, tcpa + dtinhor, -1e8) # set very large if no conf
# Vertical conflict --------------------------------------------------------
# Vertical crossing of disk (-dh,+dh)
dalt = ownship.alt.reshape((1, ownship.ntraf)) - \
intruder.alt.reshape((1, ownship.ntraf)).T + 1e9 * I
dvs = ownship.vs.reshape(1, ownship.ntraf) - \
intruder.vs.reshape(1, ownship.ntraf).T
dvs = np.where(np.abs(dvs) < 1e-6, 1e-6, dvs) # prevent division by zero
# Check for passing through each others zone
tcrosshi = (dalt + HPZ) / -dvs
tcrosslo = (dalt - HPZ) / -dvs
tinver = np.minimum(tcrosshi, tcrosslo)
toutver = np.maximum(tcrosshi, tcrosslo)
# Combine vertical and horizontal conflict----------------------------------
tinconf = np.maximum(tinver, tinhor)
toutconf = np.minimum(toutver, touthor)
swconfl = np.array(swhorconf * (tinconf <= toutconf) * (toutconf > 0.0) * \
(tinconf < tlookahead) * (1.0 - I), dtype=np.bool)
# --------------------------------------------------------------------------
# Update conflict lists
# --------------------------------------------------------------------------
# Ownship conflict flag and max tCPA
inconf = np.any(swconfl, 1)
tcpamax = np.max(tcpa * swconfl, 1)
# Select conflicting pairs: each a/c gets their own record
confpairs = [(ownship.id[i], ownship.id[j]) for i, j in zip(*np.where(swconfl))]
swlos = (dist < RPZ) * (np.abs(dalt) < HPZ)
lospairs = [(ownship.id[i], ownship.id[j]) for i, j in zip(*np.where(swlos))]
# bearing, dist, tcpa, tinconf, toutconf per conflict
qdr = qdr[swconfl]
dist = dist[swconfl]
tcpa = tcpa[swconfl]
tinconf = tinconf[swconfl]
return confpairs, lospairs, inconf, tcpamax, qdr, dist, tcpa, tinconf
def constructSSD1(asas, traf):
""" Calculates the FRV and ARV of the SSD """
#N = 0
# Parameters
N_angle = 180 # [-] Number of points on circle (discretization)
vmin = asas.vmin # [m/s] Defined in asas.py
vmax = asas.vmax # [m/s] Defined in asas.py
hsep = asas.R # [m] Horizontal separation (5 NM)
margin = asas.mar # [-] Safety margin for evasion
hsepm = hsep * margin # [m] Horizontal separation with safety margin
alpham = 0.4999 * np.pi # [rad] Maximum half-angle for VO
betalos = np.pi / 4 # [rad] Minimum divertion angle for LOS (45 deg seems optimal)
adsbmax = 200. * nm # [m] Maximum ADS-B range PRIOR: 65NM, 250NM
beta = np.pi / 4 + betalos / 2
# Relevant info from traf
gsnorth = traf.gsnorth
gseast = traf.gseast
lat = traf.lat
lon = traf.lon
ntraf = traf.ntraf
# Local variables, will be put into asas later
FRV_loc = [None] * traf.ntraf
FRV_loc_5 = [None] * traf.ntraf #NEWWWW
FRV_loc_3 = [None] * traf.ntraf #NEWWWW
FRV_loc_min = [None] * traf.ntraf #NEWWWW
FRV_loc_tla = [None] * traf.ntraf #NEWWWW
FRV_loc_dlos = [None] * traf.ntraf #NEWWWW
ARV_loc = [None] * traf.ntraf
ARV_loc_3 = [None] * traf.ntraf #NEWWWW
ARV_loc_5 = [None] * traf.ntraf #NEWWWW
ARV_loc_min = [None] * traf.ntraf #NEWWWW
ARV_loc_tla = [None] * traf.ntraf #NEWWWW
# For calculation purposes
ARV_calc_loc = [None] * traf.ntraf
ARV_calc_locmin = [None] * traf.ntraf
ARV_calc_loc_glb = [None] * traf.ntraf
ARV_calc_loc_dlos = [None] * traf.ntraf
ARV_calc_loc_dcpa = [None] * traf.ntraf
FRV_area_loc = np.zeros(traf.ntraf, dtype=np.float32)
FRV_area_loc_5 = np.zeros(traf.ntraf, dtype=np.float32)
FRV_area_loc_3 = np.zeros(traf.ntraf, dtype=np.float32)
FRV_area_loc_min = np.zeros(traf.ntraf, dtype=np.float32)
FRV_area_loc_tla = np.zeros(traf.ntraf, dtype=np.float32)
FRV_area_loc_dlos = np.zeros(traf.ntraf, dtype=np.float32)
ARV_area_loc = np.zeros(traf.ntraf, dtype=np.float32)
ARV_area_loc_5 = np.zeros(traf.ntraf, dtype=np.float32)
ARV_area_loc_3 = np.zeros(traf.ntraf, dtype=np.float32)
ARV_area_loc_min = np.zeros(traf.ntraf, dtype=np.float32)
ARV_area_loc_tla = np.zeros(traf.ntraf, dtype=np.float32)
ARV_area_loc_dlos = np.zeros(traf.ntraf, dtype=np.float32)
ARV_area_loc_dcpa = np.zeros(traf.ntraf, dtype=np.float32)
# # Use velocity limits for the ring-shaped part of the SSD
# Discretize the circles using points on circle
angles = np.arange(0, 2 * np.pi, 2 * np.pi / N_angle)
# Put points of unit-circle in a (180x2)-array (CW)
xyc = np.transpose(
np.reshape(np.concatenate((np.sin(angles), np.cos(angles))),
(2, N_angle)))
# Map them into the format pyclipper wants. Outercircle CCW, innercircle CW
circle_tup = (tuple(map(tuple, np.flipud(xyc * vmax))),
tuple(map(tuple, xyc * vmin)))
circle_lst = [
list(map(list, np.flipud(xyc * vmax))),
list(map(list, xyc * vmin))
]
#A default priocide must be defined for this CR method, otherwise it won't work with the predefined one
if asas.priocode not in asas.strategy_dict:
asas.priocode = "SRS1"
# If no traffic
if ntraf == 0:
return
# If only one aircraft
elif ntraf == 1:
# Map them into the format ARV wants. Outercircle CCW, innercircle CW
asas.ARV[0] = circle_lst
asas.ARV_3[0] = circle_lst
asas.ARV_5[0] = circle_lst
asas.ARV_min[0] = circle_lst
asas.ARV_tla[0] = circle_lst
asas.FRV[0] = []
asas.FRV_5[0] = []
asas.FRV_3[0] = []
asas.FRV_min[0] = []
asas.FRV_tla[0] = []
asas.FRV_dlos[0] = []
asas.ARV_calc[0] = circle_lst
asas.ARV_calc_min[0] = circle_lst
asas.ARV_calc_glb[0] = circle_lst
asas.ARV_calc_dlos[0] = circle_lst
asas.ARV_calc_dcpa[0] = circle_lst
# Calculate areas and store in asas
asas.FRV_area[0] = 0
asas.FRV_area_3[0] = 0
asas.FRV_area_5[0] = 0
asas.FRV_area_min[0] = 0
asas.FRV_area_tla[0] = 0
asas.FRV_area_dlos[0] = 0
asas.ARV_area[0] = np.pi * (vmax**2 - vmin**2)
asas.ARV_area_5[0] = np.pi * (vmax**2 - vmin**2)
asas.ARV_area_3[0] = np.pi * (vmax**2 - vmin**2)
asas.ARV_area_min[0] = np.pi * (vmax**2 - vmin**2)
asas.ARV_area_tla[0] = np.pi * (vmax**2 - vmin**2)
asas.ARV_area_dlos[0] = np.pi * (vmax**2 - vmin**2)
return
# Function qdrdist_matrix needs 4 vectors as input (lat1,lon1,lat2,lon2)
# To be efficient, calculate all qdr and dist in one function call
# Example with ntraf = 5: ind1 = [0,0,0,0,1,1,1,2,2,3]
# ind2 = [1,2,3,4,2,3,4,3,4,4]
# This way the qdrdist is only calculated once between every aircraft
# To get all combinations, use this function to get the indices
ind1, ind2 = qdrdist_matrix_indices(ntraf)
# Get absolute bearing [deg] and distance [nm]
# Not sure abs/rel, but qdr is defined from [-180,180] deg, w.r.t. North
[qdr, dist] = geo.qdrdist_matrix(lat[ind1], lon[ind1], lat[ind2],
lon[ind2])
# Put result of function from matrix to ndarray
qdr = np.reshape(np.array(qdr), np.shape(ind1))
dist = np.reshape(np.array(dist), np.shape(ind1))
# SI-units from [deg] to [rad]
qdr = np.deg2rad(qdr)
# Get distance from [nm] to [m]
dist = dist * nm
# In LoS the VO can't be defined, act as if dist is on edge
dist[dist < hsepm] = hsepm
# Calculate vertices of Velocity Obstacle (CCW)
# These are still in relative velocity space, see derivation in appendix
# Half-angle of the Velocity obstacle [rad]
# Include safety margin
alpha = np.arcsin(hsepm / dist)
# Limit half-angle alpha to 89.982 deg. Ensures that VO can be constructed
alpha[alpha > alpham] = alpham
#Take the cosinus of alpha to calculate the maximum length of the VO's legs
cosalpha = np.cos(alpha)
# Consider every aircraft
for i in range(ntraf):
# Calculate SSD only for aircraft in conflict (See formulas appendix)
if asas.inconf[i] == True:
# SSD for aircraft i
# Get indices that belong to aircraft i
ind = np.where(np.logical_or(ind1 == i, ind2 == i))[0]
# The i's of the other aircraft
i_other = np.delete(np.arange(0, ntraf), i)
# Aircraft that are within ADS-B range
ac_adsb = np.where(dist[ind] < adsbmax)[0]
# Now account for ADS-B range in indices of other aircraft (i_other)
ind = ind[ac_adsb]
i_other = i_other[ac_adsb]
asas.inrange[i] = i_other
# VO from 2 to 1 is mirror of 1 to 2. Only 1 to 2 can be constructed in
# this manner, so need a correction vector that will mirror the VO
fix = np.ones(np.shape(i_other))
fix[i_other < i] = -1
drel_x, drel_y = fix * dist[ind] * np.sin(
qdr[ind]), fix * dist[ind] * np.cos(qdr[ind])
drel = np.dstack((drel_x, drel_y))
cosalpha_i = cosalpha[ind]
# Make a clipper object
pc = pyclipper.Pyclipper()
pc_5 = pyclipper.Pyclipper() #NEWWW
pc_3 = pyclipper.Pyclipper() #NEWWW
pc_min = pyclipper.Pyclipper() #NEWWW
pc_tla = pyclipper.Pyclipper() #NEWWW
pc_calc = pyclipper.Pyclipper() #NEWWW
pc_calc_min = pyclipper.Pyclipper() #NEWWW
pc_calc_global = pyclipper.Pyclipper() #NEWWW
pc_calc_dlos = pyclipper.Pyclipper() #NEWWW
pc_calc_dcpa = pyclipper.Pyclipper() #NEWWW
# Add circles (ring-shape) to clipper as subject
pc.AddPaths(pyclipper.scale_to_clipper(circle_tup),
pyclipper.PT_SUBJECT, True)
pc_5.AddPaths(pyclipper.scale_to_clipper(circle_tup),
pyclipper.PT_SUBJECT, True)
pc_3.AddPaths(pyclipper.scale_to_clipper(circle_tup),
pyclipper.PT_SUBJECT, True)
pc_min.AddPaths(pyclipper.scale_to_clipper(circle_tup),
pyclipper.PT_SUBJECT, True)
pc_tla.AddPaths(pyclipper.scale_to_clipper(circle_tup),
pyclipper.PT_SUBJECT, True)
pc_calc.AddPaths(pyclipper.scale_to_clipper(circle_tup),
pyclipper.PT_SUBJECT, True)
pc_calc_min.AddPaths(pyclipper.scale_to_clipper(circle_tup),
pyclipper.PT_SUBJECT, True)
pc_calc_global.AddPaths(pyclipper.scale_to_clipper(circle_tup),
pyclipper.PT_SUBJECT, True)
pc_calc_dlos.AddPaths(pyclipper.scale_to_clipper(circle_tup),
pyclipper.PT_SUBJECT, True)
pc_calc_dcpa.AddPaths(pyclipper.scale_to_clipper(circle_tup),
pyclipper.PT_SUBJECT, True)
#To analyze current conflicts only it is more pratical
#to take indexes of the returning variables of the CD method: asas.confpairs, asas.dist, etc..
#Conflict pairs and intruders within tla
ind_current = [
index for index, item in enumerate(asas.confpairs)
if item[0] == traf.id[i]
] #original indexs
conf_pairs = [
list(item) for item in asas.confpairs if item[0] == traf.id[i]
]
inconf_with = np.array([item[1] for item in conf_pairs])
#Minimum time to LoS and intruders at minimum time to LoS +1 min threshold
min_tlos = np.around(min(asas.tLOS[ind_current]), decimals=0)
ind_mintlos = [
index for index, item in enumerate(asas.tLOS[ind_current])
if item <= min_tlos + 60
] #non original indexes
ids_min = inconf_with[ind_mintlos]
#Minimum distance to LoS and intruders at that distance
dlos = asas.tLOS[ind_current] * asas.vrel[ind_current]
min_dlos = min(dlos)
ind_mindlos = [
index for index, item in enumerate(dlos)
if item <= min_dlos + 60
] #the threshold is only to make sure computational errors don't mess this
ids_dlos = inconf_with[ind_mindlos]
#Minimum distance at CPA
min_dcpa2 = np.around(min(asas.dcpa2[ind_current]), decimals=0)
ind_mindcpa = [
index for index, item in enumerate(asas.dcpa2[ind_current])
if item <= min_dcpa2 + 60
] #threshold for safety only
ids_dcpa = inconf_with[ind_mindcpa]
"""
#Debug prints
print("Confpairs",conf_pairs)
print("In Conflict with",inconf_with)
print("minimum time to los",min_tlos)
print("mintlos indexes", ind_mintlos)
print("ids min", ids_min)
print("ids dlos", ids_dlos)
print("ids dcpa", ids_dcpa)
"""
# Add each other other aircraft to clipper as clip
for j in range(np.shape(i_other)[0]):
## Debug prints
##print(traf.id[i] + " - " + traf.id[i_other[j]])
## print(dist[ind[j]])
# Scale VO when not in LOS
if dist[ind[j]] > hsepm:
#Make an auxiliary pyclipper object for resolution purposes
pc_aux = pyclipper.Pyclipper() #NEWWW
#Add the area defined by the current velocity with a threshold of 5m/s
pc_aux.AddPaths(pyclipper.scale_to_clipper(circle_tup),
pyclipper.PT_SUBJECT, True)
dist_mod = dist[ind[
j]] #the value (not array) of the distance is needed for future computations
#direction of the VO's bisector
nd = drel[0, j, :] / dist_mod
R_pz = asas.R * asas.mar
#R_pz = asas.R*1.1
R = np.array(
[[np.sqrt(1 - (R_pz / dist_mod)**2), R_pz / dist_mod],
[-R_pz / dist_mod,
np.sqrt(1 - (R_pz / dist_mod)**2)]])
n_t1 = np.matmul(nd, R) #Direction of leg2
n_t2 = np.matmul(nd, np.transpose(R)) #Direction of leg1
#VO points
v_other = [gseast[i_other[j]], gsnorth[i_other[j]]]
legs_length = 10 * vmax / cosalpha_i[j]
VO_points = np.array([
v_other,
np.add(n_t2 * legs_length, v_other),
np.add(n_t1 * legs_length, v_other)
])
#take only the farthest 2 vertices of the VO and make a tupple
vertexes = tuple(map(tuple, VO_points[1:, :]))
# Normally VO shall be added of this other a/c
VO = pyclipper.scale_to_clipper(
tuple(map(tuple, VO_points)))
"""
#Define cuts
First cut: @5mins to LoS
Second cut: @3mins to LoS
Third cut : @min time to LoS
Fourth cut : @Look-ahead time
"""
#==========
#First cut
#==========
tau = 5 * 60 #5min to LoS
VO_5, _ = roundoff(tau, R_pz, dist_mod, VO_points[0, :],
nd, n_t1, n_t2, vertexes, xyc)
pc_5.AddPath(pyclipper.scale_to_clipper(VO_5),
pyclipper.PT_CLIP, True)
#==========
#Second cut
#==========
tau = 3 * 60 #3min for LoS
VO_3, _ = roundoff(tau, R_pz, dist_mod, VO_points[0, :],
nd, n_t1, n_t2, vertexes, xyc)
pc_3.AddPath(pyclipper.scale_to_clipper(VO_3),
pyclipper.PT_CLIP, True)
#==========
#Third cut
#==========
if np.around(min_tlos, decimals=0) != 0:
tau = min_tlos + 60 #closest conflict with a threshold one minute
v = [gseast[i], gsnorth[i]]
VO_min, leg_points = roundoff(tau, R_pz, dist_mod,
VO_points[0, :], nd,
n_t1, n_t2, vertexes,
xyc)
if pyclipper.PointInPolygon(
pyclipper.scale_to_clipper(
(gseast[i], gsnorth[i])),
pyclipper.scale_to_clipper(VO_min)):
v = [gseast[i], gsnorth[i]]
leg_points.insert(1, v)
pc_min.AddPath(pyclipper.scale_to_clipper(VO_min),
pyclipper.PT_CLIP, True)
#Current conflicts at mintlos
if traf.id[i_other[j]] in ids_min:
pc_calc_min.AddPath(VO, pyclipper.PT_CLIP, True)
#==========
#Fourth cut- at tLA (for resolution purposes only)
#==========
#For global resolution @tla considering all VOs
if asas.dtlookahead > 0:
#Make cut at tla with a threshold of 1 min
tau = asas.dtlookahead + 60
v = [gseast[i], gsnorth[i]]
VO_tla, leg_points = roundoff(tau, R_pz, dist_mod,
VO_points[0, :], nd,
n_t1, n_t2, vertexes,
xyc)
if pyclipper.PointInPolygon(
pyclipper.scale_to_clipper(
(gseast[i], gsnorth[i])),
pyclipper.scale_to_clipper(VO_tla)):
v = [gseast[i], gsnorth[i]]
leg_points.insert(1, v)
pc_calc_global.AddPath(
pyclipper.scale_to_clipper(leg_points),
pyclipper.PT_CLIP, True)
pc_tla.AddPath(pyclipper.scale_to_clipper(VO_tla),
pyclipper.PT_CLIP, True)
if traf.id[i_other[j]] in inconf_with:
pc_calc.AddPath(VO, pyclipper.PT_CLIP, True)
#======================================================
#Selection of conflicts based on distance to LoS
#======================================================
if traf.id[i_other[j]] in ids_dlos:
#previous:with VO, with leg_points
pc_calc_dlos.AddPath(VO, pyclipper.PT_CLIP, True)
#======================================================
#Selection of conflicts based on distance at CPA
#======================================================
if traf.id[i_other[j]] in ids_dcpa:
pc_calc_dcpa.AddPath(VO, pyclipper.PT_CLIP, True)
else:
# Pair is in LOS
asas.los[i] = True
#Instead of triangular VO, use darttip
# Check if bearing should be mirrored
if i_other[j] < i:
qdr_los = qdr[ind[j]] + np.pi
else:
qdr_los = qdr[ind[j]]
# Length of inner-leg of darttip
leg = 1.1 * vmax / np.cos(beta) * np.array([1, 1, 1, 0])
# Angles of darttip
angles_los = np.array(
[qdr_los + 2 * beta, qdr_los, qdr_los - 2 * beta, 0.])
# Calculate coordinates (CCW)
x_los = leg * np.sin(angles_los)
y_los = leg * np.cos(angles_los)
# Put in array of correct format
xy_los = np.vstack((x_los, y_los)).T
# Scale darttip
VO = pyclipper.scale_to_clipper(tuple(map(tuple, xy_los)))
#Store into the calculation object
#pc_calc.AddPath(VO, pyclipper.PT_CLIP, True)
# Add scaled VO to clipper
pc.AddPath(VO, pyclipper.PT_CLIP, True)
#(end of the for cycle)
# Execute clipper command
FRV = pyclipper.scale_from_clipper(
pc.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
ARV = pc.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO)
# Scale back
ARV = pyclipper.scale_from_clipper(ARV)
#5mins layer (cuts @5mins)
FRV_5 = pyclipper.scale_from_clipper(
pc_5.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
ARV_5 = pyclipper.scale_from_clipper(
pc_5.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
#3mins layer (cuts @3mins)
FRV_3 = pyclipper.scale_from_clipper(
pc_3.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
ARV_3 = pyclipper.scale_from_clipper(
pc_3.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
#mintlos layer (cuts @tlos)
FRV_min = pyclipper.scale_from_clipper(
pc_min.Execute(pyclipper.CT_INTERSECTION,
pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO))
ARV_min = pyclipper.scale_from_clipper(
pc_min.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
#Special cuts for calculation purposes
ARV_calc_min = pyclipper.scale_from_clipper(
pc_calc_min.Execute(pyclipper.CT_DIFFERENCE,
pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
#cuts @tla
FRV_tla = pyclipper.scale_from_clipper(
pc_tla.Execute(pyclipper.CT_INTERSECTION,
pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO))
ARV_tla = pyclipper.scale_from_clipper(
pc_tla.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
#cuts @tla for computation purposes
ARV_tla_glb = pyclipper.scale_from_clipper(
pc_calc_global.Execute(pyclipper.CT_DIFFERENCE,
pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
#Current conflicts within tla (take the full layer of VO)
ARV_calc_tla = pyclipper.scale_from_clipper(
pc_calc.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
#dlos layer (takes current conflicts at minimum dlos)
FRV_dlos = pyclipper.scale_from_clipper(
pc_calc_dlos.Execute(pyclipper.CT_INTERSECTION,
pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
ARV_dlos = pyclipper.scale_from_clipper(
pc_calc_dlos.Execute(pyclipper.CT_DIFFERENCE,
pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
#dcpa layer
ARV_dcpa = pyclipper.scale_from_clipper(
pc_calc_dcpa.Execute(pyclipper.CT_DIFFERENCE,
pyclipper.PFT_NONZERO,
pyclipper.PFT_NONZERO))
# Check multi exteriors, if this layer is not a list, it means it has no exteriors
# In that case, make it a list, such that its format is consistent with further code
if len(ARV) == 0:
ARV_loc[i] = []
FRV_loc[i] = circle_lst
# Calculate areas and store in asas
FRV_area_loc[i] = np.pi * (vmax**2 - vmin**2)
ARV_area_loc[i] = 0
if len(ARV_min) == 0:
ARV_loc_min[i] = []
ARV_area_loc_min[i] = 0
ARV_calc_locmin[i] = []
else:
if not type(ARV_min[0][0]) == list:
ARV_min = [ARV_min]
ARV_loc_min[i] = ARV_min
ARV_area_loc_min[i] = area(ARV_min)
if len(ARV_tla) == 0:
ARV_loc_tla[i] = []
ARV_area_loc_tla[i] = 0
else:
if not type(ARV_tla[0][0]) == list:
ARV_tla = [ARV_tla]
ARV_loc_tla[i] = ARV_tla
ARV_area_loc_tla[i] = area(ARV_tla)
if len(ARV_dlos) == 0:
ARV_calc_loc_dlos[i] = []
ARV_area_loc_dlos[i] = 0
else:
if not type(ARV_dlos[0][0]) == list:
ARV_dlos = [ARV_dlos]
ARV_calc_loc_dlos[i] = ARV_dlos
ARV_area_loc_dlos[i] = area(ARV_dlos)
if len(ARV_dcpa) == 0:
ARV_calc_loc_dcpa[i] = []
ARV_area_loc_dcpa[i] = 0
else:
if not type(ARV_dcpa[0][0]) == list:
ARV_dcpa = [ARV_dcpa]
ARV_calc_loc_dcpa[i] = ARV_dcpa
ARV_area_loc_dcpa[i] = area(ARV_dcpa)
if len(ARV_calc_tla) == 0:
ARV_calc_loc[i] = []
else:
if not type(ARV_calc_tla[0][0]) == list:
ARV_calc_tla = [ARV_calc_tla]
ARV_calc_loc[i] = ARV_calc_tla
if len(ARV_tla_glb) == 0:
ARV_calc_loc_glb[i] = []
else:
if not type(ARV_tla_glb[0][0]) == list:
ARV_tla_glb = [ARV_tla_glb]
ARV_calc_loc_glb[i] = ARV_tla_glb
if len(ARV_calc_min) == 0:
ARV_calc_locmin[i] = []
else:
if not type(ARV_calc_min[0][0]) == list:
ARV_calc_min = [ARV_calc_min]
ARV_calc_locmin[i] = ARV_calc_min
else:
#Then:
ARV_calc_loc[i] = ARV_calc_tla
ARV_calc_loc_glb[i] = ARV_tla_glb
ARV_calc_locmin[i] = ARV_calc_min
#Then/Except:
if FRV_5:
#Calcular tambem ARVs
if not type(ARV_5[0][0]) == list:
ARV_5 = [ARV_5]
ARV_loc_5[i] = ARV_5
ARV_area_loc_5[i] = area(ARV_5)
if not type(FRV_5[0][0]) == list:
FRV_5 = [FRV_5]
FRV_loc_5[i] = FRV_5
FRV_area_loc_5[i] = area(FRV_5)
else:
FRV_loc_5[i] = []
FRV_area_loc_5[i] = 0
ARV_loc_5[i] = circle_lst
ARV_area_loc_5[i] = np.pi * (vmax**2 - vmin**2)
if FRV_3:
if not type(ARV_3[0][0]) == list:
ARV_3 = [ARV_3]
ARV_loc_3[i] = ARV_3
ARV_area_loc_3[i] = area(ARV_3)
if not type(FRV_3[0][0]) == list:
FRV_3 = [FRV_3]
FRV_loc_3[i] = FRV_3
FRV_area_loc_3[i] = area(FRV_3)
else:
FRV_loc_3[i] = []
FRV_area_loc_3[i] = 0
ARV_loc_3[i] = circle_lst
ARV_area_loc_3[i] = np.pi * (vmax**2 - vmin**2)
if FRV_min:
if not type(ARV_min[0][0]) == list:
ARV_min = [ARV_min]
ARV_loc_min[i] = ARV_min
ARV_area_loc_min[i] = area(ARV_min)
if not type(FRV_min[0][0]) == list:
FRV_min = [FRV_min]
FRV_area_loc_min[i] = area(FRV_min)
FRV_loc_min[i] = FRV_min
else:
FRV_loc_min[i] = []
FRV_area_loc_min[i] = 0
ARV_loc_min[i] = circle_lst
ARV_area_loc_min[i] = np.pi * (vmax**2 - vmin**2)
ARV_calc_locmin[i] = circle_lst
if FRV_tla:
if not type(ARV_tla[0][0]) == list:
ARV_tla = [ARV_tla]
ARV_loc_tla[i] = ARV_tla
ARV_area_loc_tla[i] = area(ARV_tla)
if not type(FRV_tla[0][0]) == list:
FRV_tla = [FRV_tla]
FRV_area_loc_tla[i] = area(FRV_tla)
FRV_loc_tla[i] = FRV_tla
else:
FRV_loc_tla[i] = []
FRV_area_loc_tla[i] = 0
ARV_loc_tla[i] = circle_lst
ARV_area_loc_tla[i] = np.pi * (vmax**2 - vmin**2)
ARV_calc_loc[i] = circle_lst
ARV_calc_loc_glb[i] = circle_lst
if FRV_dlos:
if not type(ARV_dlos[0][0]) == list:
ARV_dlos = [ARV_dlos]
ARV_calc_loc_dlos[i] = ARV_dlos
ARV_area_loc_dlos[i] = area(ARV_dlos)
if not type(FRV_dlos[0][0]) == list:
FRV_dlos = [FRV_dlos]
FRV_area_loc_dlos[i] = area(FRV_dlos)
FRV_loc_dlos[i] = FRV_dlos
else:
FRV_loc_dlos[i] = []
FRV_area_loc_dlos[i] = 0
ARV_calc_loc_dlos[i] = circle_lst
ARV_area_loc_dlos[i] = np.pi * (vmax**2 - vmin**2)
if not type(FRV[0][0]) == list:
FRV = [FRV]
if not type(ARV[0][0]) == list:
ARV = [ARV]
if not type(ARV_dcpa[0][0]) == list:
ARV_dcpa = [ARV_dcpa]
# Store in asas
FRV_loc[i] = FRV
ARV_loc[i] = ARV
ARV_calc_loc_dcpa[i] = ARV_dcpa
# Calculate areas and store in asas
FRV_area_loc[i] = area(FRV)
ARV_area_loc[i] = area(ARV)
ARV_area_loc_dcpa[i] = area(ARV_dcpa)
#Storing the results into asas
asas.FRV = FRV_loc
asas.FRV_5 = FRV_loc_5
asas.FRV_3 = FRV_loc_3
asas.FRV_min = FRV_loc_min
asas.FRV_tla = FRV_loc_tla
asas.FRV_dlos = FRV_loc_dlos
asas.ARV = ARV_loc
asas.ARV_3 = ARV_loc_3
asas.ARV_5 = ARV_loc_5
asas.ARV_min = ARV_loc_min
asas.ARV_tla = ARV_loc_tla
asas.ARV_calc = ARV_calc_loc
asas.ARV_calc_min = ARV_calc_locmin
asas.ARV_calc_glb = ARV_calc_loc_glb
asas.ARV_calc_dlos = ARV_calc_loc_dlos
asas.ARV_calc_dcpa = ARV_calc_loc_dcpa
asas.FRV_area = FRV_area_loc
asas.FRV_area_5 = FRV_area_loc_5
asas.FRV_area_3 = FRV_area_loc_3
asas.FRV_area_min = FRV_area_loc_min
asas.FRV_area_tla = FRV_area_loc_tla
asas.FRV_area_dlos = FRV_area_loc_dlos
asas.ARV_area = ARV_area_loc
asas.ARV_area_5 = ARV_area_loc_5
asas.ARV_area_3 = ARV_area_loc_3
asas.ARV_area_min = ARV_area_loc_min
asas.ARV_area_tla = ARV_area_loc_tla
asas.ARV_area_dlos = ARV_area_loc_dlos
asas.ARV_area_dcpa = ARV_area_loc_dcpa
#The layers list
asas.layers = [
None, asas.ARV, asas.ARV_tla, asas.ARV_calc, asas.ARV_min,
asas.ARV_calc_min, asas.ARV_calc_dlos, asas.ARV_calc_dcpa
]
#asas.layers = [None, asas.ARV, asas.ARV_tla, asas.ARV_min, asas.ARV_calc_min, asas.ARV_calc_dlos, asas.ARV_calc_dcpa]
return
def constructSSD(asas, traf, priocode = "RS1"):
""" Calculates the FRV and ARV of the SSD """
N = 0
# Parameters
N_angle = 180 # [-] Number of points on circle (discretization)
vmin = asas.vmin # [m/s] Defined in asas.py
vmax = asas.vmax # [m/s] Defined in asas.py
hsep = asas.R # [m] Horizontal separation (5 NM)
margin = asas.mar # [-] Safety margin for evasion
hsepm = hsep * margin # [m] Horizontal separation with safety margin
alpham = 0.4999 * np.pi # [rad] Maximum half-angle for VO
betalos = np.pi / 4 # [rad] Minimum divertion angle for LOS (45 deg seems optimal)
adsbmax = 65. * nm # [m] Maximum ADS-B range
beta = np.pi/4 + betalos/2
if priocode == "RS7" or priocode == "RS8":
adsbmax /= 2
# Relevant info from traf
gsnorth = traf.gsnorth
gseast = traf.gseast
lat = traf.lat
lon = traf.lon
ntraf = traf.ntraf
hdg = traf.hdg
gs_ap = traf.ap.tas
hdg_ap = traf.ap.trk
apnorth = np.cos(hdg_ap / 180 * np.pi) * gs_ap
apeast = np.sin(hdg_ap / 180 * np.pi) * gs_ap
# Local variables, will be put into asas later
FRV_loc = [None] * traf.ntraf
ARV_loc = [None] * traf.ntraf
# For calculation purposes
ARV_calc_loc = [None] * traf.ntraf
FRV_area_loc = np.zeros(traf.ntraf, dtype=np.float32)
ARV_area_loc = np.zeros(traf.ntraf, dtype=np.float32)
# # Use velocity limits for the ring-shaped part of the SSD
# Discretize the circles using points on circle
angles = np.arange(0, 2 * np.pi, 2 * np.pi / N_angle)
# Put points of unit-circle in a (180x2)-array (CW)
xyc = np.transpose(np.reshape(np.concatenate((np.sin(angles), np.cos(angles))), (2, N_angle)))
# Map them into the format pyclipper wants. Outercircle CCW, innercircle CW
circle_tup = (tuple(map(tuple, np.flipud(xyc * vmax))), tuple(map(tuple , xyc * vmin)))
circle_lst = [list(map(list, np.flipud(xyc * vmax))), list(map(list , xyc * vmin))]
# If no traffic
if ntraf == 0:
return
# If only one aircraft
elif ntraf == 1:
# Map them into the format ARV wants. Outercircle CCW, innercircle CW
ARV_loc[0] = circle_lst
FRV_loc[0] = []
ARV_calc_loc[0] = ARV_loc[0]
# Calculate areas and store in asas
FRV_area_loc[0] = 0
ARV_area_loc[0] = np.pi * (vmax **2 - vmin ** 2)
return
# Function qdrdist_matrix needs 4 vectors as input (lat1,lon1,lat2,lon2)
# To be efficient, calculate all qdr and dist in one function call
# Example with ntraf = 5: ind1 = [0,0,0,0,1,1,1,2,2,3]
# ind2 = [1,2,3,4,2,3,4,3,4,4]
# This way the qdrdist is only calculated once between every aircraft
# To get all combinations, use this function to get the indices
ind1, ind2 = qdrdist_matrix_indices(ntraf)
# Get absolute bearing [deg] and distance [nm]
# Not sure abs/rel, but qdr is defined from [-180,180] deg, w.r.t. North
[qdr, dist] = geo.qdrdist_matrix(lat[ind1], lon[ind1], lat[ind2], lon[ind2])
# Put result of function from matrix to ndarray
qdr = np.reshape(np.array(qdr), np.shape(ind1))
dist = np.reshape(np.array(dist), np.shape(ind1))
# SI-units from [deg] to [rad]
qdr = np.deg2rad(qdr)
# Get distance from [nm] to [m]
dist = dist * nm
# In LoS the VO can't be defined, act as if dist is on edge
dist[dist < hsepm] = hsepm
# Calculate vertices of Velocity Obstacle (CCW)
# These are still in relative velocity space, see derivation in appendix
# Half-angle of the Velocity obstacle [rad]
# Include safety margin
alpha = np.arcsin(hsepm / dist)
# Limit half-angle alpha to 89.982 deg. Ensures that VO can be constructed
alpha[alpha > alpham] = alpham
# Relevant sin/cos/tan
sinqdr = np.sin(qdr)
cosqdr = np.cos(qdr)
tanalpha = np.tan(alpha)
cosqdrtanalpha = cosqdr * tanalpha
sinqdrtanalpha = sinqdr * tanalpha
# Relevant x1,y1,x2,y2 (x0 and y0 are zero in relative velocity space)
x1 = (sinqdr + cosqdrtanalpha) * 2 * vmax
x2 = (sinqdr - cosqdrtanalpha) * 2 * vmax
y1 = (cosqdr - sinqdrtanalpha) * 2 * vmax
y2 = (cosqdr + sinqdrtanalpha) * 2 * vmax
# Consider every aircraft
for i in range(ntraf):
# Calculate SSD only for aircraft in conflict (See formulas appendix)
if asas.inconf[i]:
# SSD for aircraft i
# Get indices that belong to aircraft i
ind = np.where(np.logical_or(ind1 == i,ind2 == i))[0]
# Check whether there are any aircraft in the vicinity
if len(ind) == 0:
# No aircraft in the vicinity
# Map them into the format ARV wants. Outercircle CCW, innercircle CW
ARV_loc[i] = circle_lst
FRV_loc[i] = []
ARV_calc_loc[i] = ARV_loc[i]
# Calculate areas and store in asas
FRV_area_loc[i] = 0
ARV_area_loc[i] = np.pi * (vmax **2 - vmin ** 2)
else:
# The i's of the other aircraft
i_other = np.delete(np.arange(0, ntraf), i)
# Aircraft that are within ADS-B range
ac_adsb = np.where(dist[ind] < adsbmax)[0]
# Now account for ADS-B range in indices of other aircraft (i_other)
ind = ind[ac_adsb]
i_other = i_other[ac_adsb]
if not priocode == "RS7" and not priocode == "RS8":
# Put it in class-object (not for RS7 and RS8)
asas.inrange[i] = i_other
else:
asas.inrange2[i] = i_other
# VO from 2 to 1 is mirror of 1 to 2. Only 1 to 2 can be constructed in
# this manner, so need a correction vector that will mirror the VO
fix = np.ones(np.shape(i_other))
fix[i_other < i] = -1
# Relative bearing [deg] from [-180,180]
# (less required conversions than rad in RotA)
fix_ang = np.zeros(np.shape(i_other))
fix_ang[i_other < i] = 180.
# Get vertices in an x- and y-array of size (ntraf-1)*3x1
x = np.concatenate((gseast[i_other],
x1[ind] * fix + gseast[i_other],
x2[ind] * fix + gseast[i_other]))
y = np.concatenate((gsnorth[i_other],
y1[ind] * fix + gsnorth[i_other],
y2[ind] * fix + gsnorth[i_other]))
# Reshape [(ntraf-1)x3] and put arrays in one array [(ntraf-1)x3x2]
x = np.transpose(x.reshape(3, np.shape(i_other)[0]))
y = np.transpose(y.reshape(3, np.shape(i_other)[0]))
xy = np.dstack((x,y))
# Make a clipper object
pc = pyclipper.Pyclipper()
# Add circles (ring-shape) to clipper as subject
pc.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True)
# Extra stuff needed for RotA
if priocode == "RS6":
# Make another clipper object for RotA
pc_rota = pyclipper.Pyclipper()
pc_rota.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True)
# Bearing calculations from own view and other view
brg_own = np.mod((np.rad2deg(qdr[ind]) + fix_ang - hdg[i]) + 540., 360.) - 180.
brg_other = np.mod((np.rad2deg(qdr[ind]) + 180. - fix_ang - hdg[i_other]) + 540., 360.) - 180.
# Add each other other aircraft to clipper as clip
for j in range(np.shape(i_other)[0]):
## Debug prints
## print(traf.id[i] + " - " + traf.id[i_other[j]])
## print(dist[ind[j]])
# Scale VO when not in LOS
if dist[ind[j]] > hsepm:
# Normally VO shall be added of this other a/c
VO = pyclipper.scale_to_clipper(tuple(map(tuple,xy[j,:,:])))
else:
# Pair is in LOS, instead of triangular VO, use darttip
# Check if bearing should be mirrored
if i_other[j] < i:
qdr_los = qdr[ind[j]] + np.pi
else:
qdr_los = qdr[ind[j]]
# Length of inner-leg of darttip
leg = 1.1 * vmax / np.cos(beta) * np.array([1,1,1,0])
# Angles of darttip
angles_los = np.array([qdr_los + 2 * beta, qdr_los, qdr_los - 2 * beta, 0.])
# Calculate coordinates (CCW)
x_los = leg * np.sin(angles_los)
y_los = leg * np.cos(angles_los)
# Put in array of correct format
xy_los = np.vstack((x_los,y_los)).T
# Scale darttip
VO = pyclipper.scale_to_clipper(tuple(map(tuple,xy_los)))
# Add scaled VO to clipper
pc.AddPath(VO, pyclipper.PT_CLIP, True)
# For RotA it is possible to ignore
if priocode == "RS6":
if brg_own[j] >= -20. and brg_own[j] <= 110.:
# Head-on or converging from right
pc_rota.AddPath(VO, pyclipper.PT_CLIP, True)
elif brg_other[j] <= -110. or brg_other[j] >= 110.:
# In overtaking position
pc_rota.AddPath(VO, pyclipper.PT_CLIP, True)
# Detect conflicts for smaller layer in RS7 and RS8
if priocode == "RS7" or priocode == "RS8":
if pyclipper.PointInPolygon(pyclipper.scale_to_clipper((gseast[i],gsnorth[i])),VO):
asas.inconf2[i] = True
if priocode == "RS5":
if pyclipper.PointInPolygon(pyclipper.scale_to_clipper((apeast[i],apnorth[i])),VO):
asas.ap_free[i] = False
# Execute clipper command
FRV = pyclipper.scale_from_clipper(pc.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO))
ARV = pc.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)
if not priocode == "RS1" and not priocode == "RS5" and not priocode == "RS7" and not priocode == "RS8":
# Make another clipper object for extra intersections
pc2 = pyclipper.Pyclipper()
# When using RotA clip with pc_rota
if priocode == "RS6":
# Calculate ARV for RotA
ARV_rota = pc_rota.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)
if len(ARV_rota) > 0:
pc2.AddPaths(ARV_rota, pyclipper.PT_CLIP, True)
else:
# Put the ARV in there, make sure it's not empty
if len(ARV) > 0:
pc2.AddPaths(ARV, pyclipper.PT_CLIP, True)
# Scale back
ARV = pyclipper.scale_from_clipper(ARV)
# Check if ARV or FRV is empty
if len(ARV) == 0:
# No aircraft in the vicinity
# Map them into the format ARV wants. Outercircle CCW, innercircle CW
ARV_loc[i] = []
FRV_loc[i] = circle_lst
ARV_calc_loc[i] = []
# Calculate areas and store in asas
FRV_area_loc[i] = np.pi * (vmax **2 - vmin ** 2)
ARV_area_loc[i] = 0
elif len(FRV) == 0:
# Should not happen with one a/c or no other a/c in the vicinity.
# These are handled earlier. Happens when RotA has removed all
# Map them into the format ARV wants. Outercircle CCW, innercircle CW
ARV_loc[i] = circle_lst
FRV_loc[i] = []
ARV_calc_loc[i] = circle_lst
# Calculate areas and store in asas
FRV_area_loc[i] = 0
ARV_area_loc[i] = np.pi * (vmax **2 - vmin ** 2)
else:
# Check multi exteriors, if this layer is not a list, it means it has no exteriors
# In that case, make it a list, such that its format is consistent with further code
if not type(FRV[0][0]) == list:
FRV = [FRV]
if not type(ARV[0][0]) == list:
ARV = [ARV]
# Store in asas
FRV_loc[i] = FRV
ARV_loc[i] = ARV
# Calculate areas and store in asas
FRV_area_loc[i] = area(FRV)
ARV_area_loc[i] = area(ARV)
# For resolution purposes sometimes extra intersections are wanted
if priocode == "RS2" or priocode == "RS9" or priocode == "RS6" or priocode == "RS3" or priocode == "RS4":
# Make a box that covers right or left of SSD
own_hdg = hdg[i] * np.pi / 180
# Efficient calculation of box, see notes
if priocode == "RS2" or priocode == "RS6":
# CW or right-turning
sin_table = np.array([[1,0],[-1,0],[-1,-1],[1,-1]], dtype=np.float64)
cos_table = np.array([[0,1],[0,-1],[1,-1],[1,1]], dtype=np.float64)
elif priocode == "RS9":
# CCW or left-turning
sin_table = np.array([[1,0],[1,1],[-1,1],[-1,0]], dtype=np.float64)
cos_table = np.array([[0,1],[-1,1],[-1,-1],[0,-1]], dtype=np.float64)
# Overlay a part of the full SSD
if priocode == "RS2" or priocode == "RS9" or priocode == "RS6":
# Normalized coordinates of box
xyp = np.sin(own_hdg) * sin_table + np.cos(own_hdg) * cos_table
# Scale with vmax (and some factor) and put in tuple
part = pyclipper.scale_to_clipper(tuple(map(tuple, 1.1 * vmax * xyp)))
pc2.AddPath(part, pyclipper.PT_SUBJECT, True)
elif priocode == "RS3":
# Small ring
xyp = (tuple(map(tuple, np.flipud(xyc * min(vmax,gs_ap[i] + 0.1)))), tuple(map(tuple , xyc * max(vmin,gs_ap[i] - 0.1))))
part = pyclipper.scale_to_clipper(xyp)
pc2.AddPaths(part, pyclipper.PT_SUBJECT, True)
elif priocode == "RS4":
hdg_sel = hdg[i] * np.pi / 180
xyp = np.array([[np.sin(hdg_sel-0.0087),np.cos(hdg_sel-0.0087)],
[0,0],
[np.sin(hdg_sel+0.0087),np.cos(hdg_sel+0.0087)]],
dtype=np.float64)
part = pyclipper.scale_to_clipper(tuple(map(tuple, 1.1 * vmax * xyp)))
pc2.AddPath(part, pyclipper.PT_SUBJECT, True)
# Execute clipper command
ARV_calc = pyclipper.scale_from_clipper(pc2.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO))
N += 1
# If no smaller ARV is found, take the full ARV
if len(ARV_calc) == 0:
ARV_calc = ARV
# Check multi exteriors, if this layer is not a list, it means it has no exteriors
# In that case, make it a list, such that its format is consistent with further code
if not type(ARV_calc[0][0]) == list:
ARV_calc = [ARV_calc]
# Shortest way out prio, so use full SSD (ARV_calc = ARV)
else:
ARV_calc = ARV
# Update calculatable ARV for resolutions
ARV_calc_loc[i] = ARV_calc
# If sequential approach, the local should go elsewhere
if not priocode == "RS7" and not priocode == "RS8":
asas.FRV = FRV_loc
asas.ARV = ARV_loc
asas.ARV_calc = ARV_calc_loc
asas.FRV_area = FRV_area_loc
asas.ARV_area = ARV_area_loc
else:
asas.ARV_calc2 = ARV_calc_loc
return
def calculate_resolution(self, conf, traf):
""" Calculates closest conflict-free point according to ruleset """
ind1, ind2 = self.qdrdist_matrix_indices(traf.ntraf)
[qdr, dist] = geo.qdrdist_matrix(traf.lat[ind1], traf.lon[ind1],
traf.lat[ind2], traf.lon[ind2])
qdr = np.reshape(np.array(qdr), np.shape(ind1))
dist = np.reshape(np.array(dist), np.shape(ind1))
# SI-units from [deg] to [rad]
qdr = np.deg2rad(qdr)
# Get distance from [nm] to [m]
dist = dist * nm
hsepm = conf.rpz * self.resofach # [m] Horizontal separation with safety margin
# In LoS the VO can't be defined, act as if dist is on edge
dist[dist < hsepm] = hsepm
self.inconf = np.copy(conf.inconf)
# Calculate vertices of Velocity Obstacle (CCW)
# These are still in relative velocity space, see derivation in appendix
# Half-angle of the Velocity obstacle [rad]
# Include safety margin
alpha = np.arcsin(hsepm / dist)
# Limit half-angle alpha to 89.982 deg. Ensures that VO can be constructed
alpham = 0.4999 * np.pi
alpha[alpha > alpham] = alpham
# Relevant sin/cos/tan
sinqdr = np.sin(qdr)
cosqdr = np.cos(qdr)
tanalpha = np.tan(alpha)
cosqdrtanalpha = cosqdr * tanalpha
sinqdrtanalpha = sinqdr * tanalpha
# Relevant x1,y1,x2,y2 (x0 and y0 are zero in relative velocity space)
x1 = (sinqdr + cosqdrtanalpha) * 2 * self.vmax
x2 = (sinqdr - cosqdrtanalpha) * 2 * self.vmax
y1 = (cosqdr - sinqdrtanalpha) * 2 * self.vmax
y2 = (cosqdr + sinqdrtanalpha) * 2 * self.vmin
previous_solutions = dict()
for cycle in range(MAX_IT):
# Consider every aircraft
for it in range(traf.ntraf):
if previous_solutions.get(it) is None:
previous_solutions[it] = []
# Calculate solution for aircraft in conflict
if self.inconf[it]:
ind1, ind2 = self.qdrdist_matrix_indices(traf.ntraf)
# Get indices that belong to aircraft i
ind = np.where(np.logical_or(ind1 == it, ind2 == it))[0]
# other traffic
i_other = np.delete(np.arange(0, traf.ntraf), it)
# find new solution
asase, asasn = self.calculate_best_maneuver(
self.vmax, self.vmin, i_other, it, x1, x2, y1, y2,
traf.gseast, traf.gsnorth, traf.gs, traf.trk, ind,
PREFERENCES[it], previous_solutions[it])
previous_solutions[it].append([asase, asasn])
self.new_track[it] = np.arctan2(asase, asasn) * 180 / np.pi
self.new_gs[it] = np.sqrt(asase**2 + asasn**2)
traf.trk = np.copy(self.new_track)
traf.gs = np.copy(self.new_gs)
traf.gsnorth = np.cos(traf.trk / 180 * np.pi) * traf.gs
traf.gseast = np.sin(traf.trk / 180 * np.pi) * traf.gs
# the negotiations is over for aircraft that are no longer in conflict
self.inconf, dist = self.detect_conflicts(traf, traf, conf.rpz,
conf.hpz,
conf.dtlookahead)
