def createPath(node0, node1):
new_route = [node0]
lat0, lon0 = cb.getLatLonnode(cb.GRAPH, node0)
lat1, lon1 = cb.getLatLonnode(cb.GRAPH, node1)
qdr, dist = geo.kwikqdrdist(lat0, lon0, lat1, lon1)
possible_directions = [86.8, 176.8, 266.8, 356.8]
distances = [94.4, 207.6, 94.4, 207.6] # meters
closest_heading = min(range(len(possible_directions)),
key=lambda i: abs(possible_directions[i] - qdr))
dist = dist * nm
current_node = node0
while dist > distances[closest_heading]:
edges_current_node = cb.EDGES_PER_NODE[current_node]
lat0, lon0, edges_current_node, new_node = getNewNode(
current_node, closest_heading, edges_current_node)
new_route.append(new_node)
qdr, dist = geo.kwikqdrdist(lat0, lon0, lat1, lon1)
dist = dist * nm
closest_heading = min(range(len(possible_directions)),
key=lambda i: abs(possible_directions[i] - qdr))
current_node = new_node
if new_route[-1] != node1:
if cb.EDGES_PER_NODE[new_route[-1]][closest_heading] is None:
cb.EDGES_PER_NODE[new_route[-1]][closest_heading] = (node1, qdr,
dist)
new_route.append(node1)
return new_route
def createAircraft(self, aicraftname, alts, route, vars):
lat0, lon0 = cb.getLatLonnode(cb.GRAPH, route[0])
lat1, lon1 = cb.getLatLonnode(cb.GRAPH, route[1])
qdr, dummy = geo.kwikqdrdist(lat0, lon0, lat1, lon1)
creation_speed = TAS_MAX
if vars[0] or vars[1]:
creation_speed = TAS_CURVE
bs.traf.create(1, AC_TYPE, ALTS[int(alts[0])] * ft,
creation_speed * kts, None, lat0, lon0, qdr,
aicraftname)
def getAltitudeLayer(point0, point1):
expected_headings = [356, 86, 176, 266]
lat0, lon0 = cb.getLatLonnode(cb.GRAPH, point0)
lat1, lon1 = cb.getLatLonnode(cb.GRAPH, point1)
qdr, dummy = geo.kwikqdrdist(lat0, lon0, lat1, lon1)
altitude = min(range(len(expected_headings)),
key=lambda i: abs(expected_headings[i] - qdr))
# main altitudes start at layer 2
altitude = 2 + altitude * 2
return altitude, qdr, lat0, lon0, lat1, lon1
def check_conflict(lat_own, lon_own, lat_int, lon_int, gs_own, trk_own, gs_int,
trk_int, rpz, dtlookahead):
''' Horizontal conflict detection for two-criteria method by Schaberg '''
# Horizontal conflict ------------------------------------------------------
# qdrlst is for [i,j] qdr from i to j, from perception of ADSB and own coordinates
qdr, dist = geo.kwikqdrdist(lat_own, lon_own, lat_int, lon_int)
dist *= nm
# 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(trk_own)
ownu = gs_own * np.sin(owntrkrad) # m/s
ownv = gs_own * np.cos(owntrkrad) # m/s
# Intruder track angle and speed
inttrkrad = np.radians(trk_int)
intu = gs_int * np.sin(inttrkrad) # m/s
intv = gs_int * np.cos(inttrkrad) # m/s
du = intu - ownu # Speed du[i,j] is perceived eastern speed of i to j
dv = intv - ownv # Speed dv[i,j] is perceived northern speed of i to j
dv2 = du * du + dv * dv
dv2 = 1e-6 if np.abs(dv2) < 1e-6 else dv2 # limit lower absolute value
vrel = np.sqrt(dv2)
tcpa = -(du * dx + dv * dy) / dv2
# 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 time of entering PZ
dxinhor = np.sqrt(np.maximum(
0., R2 - dcpa2)) # half the distance travelled inzide zone)
dtinhor = dxinhor / vrel
tinhor = tcpa - dtinhor if swhorconf else 1e8 # Set very large if no conf
return tinhor < dtlookahead
def getNewNode(previous_node, step, edges):
lat_node, lon_node = cb.getLatLonnode(cb.GRAPH, previous_node)
possible_directions = [86.8, 176.8, 266.8, 356.8]
distances = [94.4, 207.6, 94.4, 207.6] # meters
lat0, lon0 = geo.kwikpos(lat_node, lon_node, possible_directions[step],
distances[step] / nm)
# check if we ended up in an existing node
lat_nodes, lon_nodes, node_ids = cb.getLatLonAllNodes(cb.GRAPH)
closest_node, dummy = cb.getclosestnode([lat0], [lon0], lat_nodes,
lon_nodes, node_ids)
lat_closest_node, lon_closest_node = cb.getLatLonnode(
cb.GRAPH, closest_node)
qdr, dist = geo.kwikqdrdist(lat0, lon0, lat_closest_node, lon_closest_node)
if closest_node != previous_node and dist * nm < 15: # lets use this existent node
lat0, lon0 = lat_closest_node, lon_closest_node
edges[step] = (closest_node, qdr, dist * nm)
cb.EDGES_PER_NODE[previous_node] = edges
# create a new node
else:
existent_nodes = list(cb.NODES.keys())
# find the smallest positive number not in list
new_nodes_key = next(
filterfalse(set(existent_nodes).__contains__, count(1)))
cb.addtoNodes(lat0, lon0, new_nodes_key, True)
if edges[step] is None:
edges[step] = (new_nodes_key, possible_directions[step],
distances[step])
cb.EDGES_PER_NODE[previous_node] = edges
# create edges for the new node, create only the ones that we are going to need
new_nodes_edges = [None, None, None, None]
opposite_step = getOppositeStep(step)
new_nodes_edges[opposite_step] = (previous_node,
possible_directions[opposite_step],
distances[step])
cb.EDGES_PER_NODE[new_nodes_key] = new_nodes_edges
closest_node = new_nodes_key
return lat0, lon0, edges, closest_node
def update(self,gain):
# Time step update of source
# Get time step
dt = sim.simt - self.tupdate
self.tupdate = sim.simt
# Time for a new aircraft?
if dt>0.0:
# Calculate probability of a geberate occurring with flow
chances = 1.0-gain*self.flow*dt/3600. #flow is in a/c per hour=360 seconds
if random.random() >= chances:
# Runways defined? => use runway lines (queues)
if len(self.runways)>0:
# We do not yet need to create an aircraft
gennow = False
# Find shortest line and put it in
isel = random.randint(0,len(self.runways)-1)
self.rwyline[isel] = self.rwyline[isel] + 1
else:
# Yes we do need to generate one now
gennow = True
else:
gennow = False
# Check for generating aircraft
# First check runways for a/c already in line:
txt = ""
for i in range(len(self.runways)):
# Runway vacated and there is one waiting?
if sim.simt-self.rwytotime[i]>self.dtakeoff and self.rwyline[i]>0:
#if self.name == "EHAM":
# print(sim.simt, self.runways[i], self.rwytotime[i])
self.rwytotime[i] = sim.simt
self.rwyline[i] = self.rwyline[i]-1
# Choose and aicraft type, check for distance
if len(self.dest)>0:
idest = int(random.random() * len(self.dest))
else:
idest = -1
if idest>=0:
acid = randacname(self.name,self.dest[idest])
if self.desttype[idest]=="seg" or self.dest[idest][:4]=="SEGM":
lat,lon,hdg = getseg(self.dest[idest])
else:
success,posobj = txt2pos(self.dest[idest],ctrlat,ctrlon)
lat,lon = posobj.lat,posobj.lon
distroute = latlondist(self.lat,self.lon,lat,lon)/nm
else:
acid = randacname(self.name, self.name)
if self.destactypes[idest] == []:
actype = random.choice(self.actypes)
actype = checkactype(actype, distroute, self.actypes)
else:
actype = random.choice(self.destactypes[idest])
stack.stack("CRE "+",".join([acid, actype,
str(self.rwylat[i]),str(self.rwylon[i]),str(self.rwyhdg[i]),
"0.0","0.0"]))
#wplat,wplon = kwikpos(self.rwylat[i],self.rwylon[i],self.rwyhdg[i],5.0*nm)
#stack.stack(acid + " ADDWPT ",wplat," ",wplon)
#stack.stack(acid+"LNAV ON")
if idest>=0:
if self.dest[idest][:4] != "SEGM":
stack.stack(acid + " DEST " + self.dest[idest])
else:
stack.stack(acid + " DEST " + str(self.destlat[idest])
+ " " + str(self.destlon[idest]))
if self.name[:4] != "SEGM":
stack.stack(acid + " ORIG " + self.name)
else:
stack.stack(acid + " ORIG " + str(self.lat) + " " + str(self.lon))
stack.stack(acid + " SPD 250")
stack.stack(acid + " ALT FL100")
stack.stack(acid + " HDG " + str(self.rwyhdg[i]))
stack.stack(acid+" LNAV OFF")
# Not runway, then define instantly at position with random heading or in case of segment inward heading
if gennow:
if not self.incircle:
lat,lon = kwikpos(ctrlat,ctrlon,self.segdir,radius)
hdg = self.segdir-180
elif self.type=="seg":
lat,lon,brg = getseg(self.name)
hdg = (brg+180)%360
elif self.type=="rwy":
lat,lon = self.lat,self.lon
hdg = self.hdg # Runway heading
else:
hdg = random.random()*360.
if self.startaltmin and self.startaltmax:
alt = random.randint(int(self.startaltmin), int(self.startaltmax))
else:
alt = random.randint(200, 300) * 100 * ft
if self.startspdmin and self.startspdmax:
spd = random.randint(int(self.startspdmin), int(self.startspdmax))
else:
spd = random.randint(250, 350)
alttxt, spdtxt = "FL" + str(int(round(alt / (100 * ft)))), str(spd)
# Add destination
if len(self.dest)>0:
idest = int(random.random() * len(self.dest))
acid = randacname(self.name,self.dest[idest])
else:
acid = randacname(self.name,self.name)
idest = -1
stack.stack("CRE " + ",".join([acid, random.choice(self.actypes),
str(self.lat), str(self.lon), str(int(hdg%360)),
alttxt,spdtxt]))
if idest>=0:
if self.dest[idest][:4] != "SEGM":
stack.stack(acid + " DEST " + self.dest[idest])
else:
stack.stack(acid + " DEST " + str(self.destlat[idest])+" "+str(self.destlon[idest]))
if self.name[:4] != "SEGM":
stack.stack(acid + " ORIG " + self.name)
else:
stack.stack(acid + " ORIG " + str(self.lat)+" "+str(self.lon))
if alttxt=="0" and spdtxt =="0":
stack.stack(acid+" SPD 250")
stack.stack(acid+" ALT FL100")
else:
if idest>=0:
if self.desttype[idest] == "seg":
lat, lon, hdg = getseg(self.dest[idest])
brg, dist = kwikqdrdist(self.lat, self.lon, lat, lon)
stack.stack(acid + " HDG " + str(brg))
else:
stack.stack(acid + " LNAV ON")
def update(self,gain):
# Time step update of source
# Get time step
dt = sim.simt - self.tupdate
self.tupdate = sim.simt
# Time for a new aircraft?
if dt>0.0:
# Calculate probability of a geberate occurring with flow
chances = 1.0-gain*self.flow*dt/3600. #flow is in a/c per hour=360 seconds
if random.random() >= chances:
# Runways defined? => use runway lines (queues)
if len(self.runways)>0:
# We do not yet need to create an aircraft
gennow = False
# Find shortest line and put it in
isel = random.randint(0,len(self.runways)-1)
self.rwyline[isel] = self.rwyline[isel] + 1
else:
# Yes we do need to generate one now
gennow = True
else:
gennow = False
# Check for generating aircraft
# First check runways for a/c already in line:
txt = ""
for i in range(len(self.runways)):
# Runway vacated and there is one waiting?
if sim.simt-self.rwytotime[i]>self.dtakeoff and self.rwyline[i]>0:
#if self.name == "EHAM":
# print(sim.simt, self.runways[i], self.rwytotime[i])
self.rwytotime[i] = sim.simt
self.rwyline[i] = self.rwyline[i]-1
# Choose and aicraft type, check for distance
if len(self.dest)>0:
idest = int(random.random() * len(self.dest))
else:
idest = -1
if idest>=0:
acid = randacname(self.name,self.dest[idest])
if self.desttype[idest]=="seg" or self.dest[idest][:4]=="SEGM":
lat,lon,hdg = getseg(self.dest[idest])
else:
success,posobj = txt2pos(self.dest[idest],ctrlat,ctrlon)
lat,lon = posobj.lat,posobj.lon
distroute = latlondist(self.lat,self.lon,lat,lon)/nm
else:
acid = randacname(self.name, self.name)
if self.destactypes[idest] == []:
actype = random.choice(self.actypes)
actype = checkactype(actype, distroute, self.actypes)
else:
actype = random.choice(self.destactypes[idest])
stack.stack("CRE "+",".join([acid, actype,
str(self.rwylat[i]),str(self.rwylon[i]),str(self.rwyhdg[i]),
"0.0","0.0"]))
stack.stack(acid + " SPD 250")
stack.stack(acid + " ALT FL100")
stack.stack(acid + " HDG " + str(self.rwyhdg[i]))
# TBD: Add waypoint for after take-off?
if idest>=0:
if self.dest[idest][:4] != "SEGM":
stack.stack(acid + " DEST " + self.dest[idest])
else:
stack.stack(acid + " DEST " + str(self.destlat[idest])
+ " " + str(self.destlon[idest]))
if self.name[:4] != "SEGM":
stack.stack(acid + " ORIG " + self.name)
else:
stack.stack(acid + " ORIG " + str(self.lat) + " " + str(self.lon))
if idest>=0 and self.desttype[idest]=="seg":
lat,lon,hdg = getseg(self.dest[idest])
brg,dist = kwikqdrdist(self.lat,self.lon,lat,lon)
#stack.stack(acid+" HDG "+str(brg))
else:
stack.stack(acid+" LNAV OFF")
#stack.stack(acid+" VNAV ON")
# Not runway, then define instantly at position with random heading or in case of segment inward heading
if gennow:
if not self.incircle:
lat,lon = kwikpos(ctrlat,ctrlon,self.segdir,radius)
hdg = self.segdir-180
elif self.type=="seg":
lat,lon,brg = getseg(self.name)
hdg = (brg+180)%360
elif self.type=="rwy":
lat,lon = self.lat,self.lon
hdg = self.hdg # Runway heading
else:
hdg = random.random()*360.
if (self.type=="apt" or self.type=="rwy") and self.incircle:
alttxt,spdtxt = str(0),str(0)
else:
alttxt,spdtxt = "FL"+str(random.randint(200,300)), str(random.randint(250,350))
# Add destination
if len(self.dest)>0:
idest = int(random.random() * len(self.dest))
acid = randacname(self.name,self.dest[idest])
else:
acid = randacname(self.name,self.name)
idest = -1
stack.stack("CRE " + ",".join([acid, random.choice(self.actypes),
str(self.lat), str(self.lon), str(int(hdg%360)),
alttxt,spdtxt]))
if idest>=0:
if self.dest[idest][:4] != "SEGM":
stack.stack(acid + " DEST " + self.dest[idest])
else:
stack.stack(acid + " DEST " + str(self.destlat[idest])+" "+str(self.destlon[idest]))
if self.name[:4] != "SEGM":
stack.stack(acid + " ORIG " + self.name)
else:
stack.stack(acid + " ORIG " + str(self.lat)+" "+str(self.lon))
if alttxt=="0" and spdtxt =="0":
stack.stack(acid+" SPD 250")
stack.stack(acid+" ALT FL100")
else:
if idest>=0:
if self.desttype[idest] == "seg":
lat, lon, hdg = getseg(self.dest[idest])
brg, dist = kwikdist(self.lat, self.lon, lat, lon)
stack.stack(acid + " HDG " + str(brg))
else:
stack.stack(acid + " LNAV ON")