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")