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