class Traffic:
"""
Traffic class definition : Traffic data
Methods:
Traffic() : constructor
reset() : Reset traffic database w.r.t a/c data
create(acid,actype,aclat,aclon,achdg,acalt,acspd) : create aircraft
delete(acid) : delete an aircraft from traffic data
deletall() : delete all traffic
update(sim) : do a numerical integration step
id2idx(name) : return index in traffic database of given call sign
selhdg(i,hdg) : set autopilot heading and activate heading select mode
selspd(i,spd) : set autopilot CAS/Mach and activate heading select mode
engchange(i,engtype) : change engine type of an aircraft
changeTrailColor(color,idx) : change colour of trail of aircraft idx
setNoise(A) : Add turbulence
Members: see create
Created by : Jacco M. Hoekstra
"""
def __init__(self, navdb):
# ASAS object
self.asas = ASAS()
# All traffic data is initialized in the reset function
self.reset(navdb)
def reset(self, navdb):
# model-specific parameters.
# Default: BlueSky internal performance model.
# Insert your BADA files to the folder "BlueSky/data/coefficients/BADA"
# for working with EUROCONTROL`s Base of Aircraft Data revision 3.12
self.perf = Perf(self)
self.ntraf = 0
# Traffic list & arrays definition
# !!!IMPORTANT NOTE!!!
# Any variables added here should also be added in the Traffic
# methods self.create() (append) and self.delete() (delete)
# which can be found directly below __init__
# Traffic basic flight data
# Traffic basic flight data
self.id = [] # identifier (string)
self.type = [] # aircaft type (string)
self.lat = np.array([]) # latitude [deg]
self.lon = np.array([]) # longitude [deg]
self.trk = np.array([]) # track angle [deg]
self.tas = np.array([]) # true airspeed [m/s]
self.gs = np.array([]) # ground speed [m/s]
self.cas = np.array([]) # calibrated airspeed [m/s]
self.M = np.array([]) # mach number
self.alt = np.array([]) # altitude [m]
self.fll = np.array([]) # flight level [ft/100]
self.vs = np.array([]) # vertical speed [m/s]
self.p = np.array([]) # atmospheric air pressure [N/m2]
self.rho = np.array([]) # atmospheric air density [kg/m3]
self.Temp = np.array([]) # atmospheric air temperature [K]
self.dtemp = np.array([]) # delta t for non-ISA conditions
# Traffic performance data
self.avsdef = np.array([]) # [m/s]default vertical speed of autopilot
self.aphi = np.array([]) # [rad] bank angle setting of autopilot
self.ax = np.array([]) # [m/s2] absolute value of longitudinal accelleration
self.bank = np.array([]) # nominal bank angle, [radian]
self.bphase = np.array([]) # standard bank angles per phase
self.hdgsel = np.array([]) # determines whether aircraft is turning
# Help variables to save computation time
self.coslat = np.array([]) # Cosine of latitude for flat-earth aproximations
# Crossover altitude
self.abco = np.array([])
self.belco = np.array([])
# Traffic autopilot settings
self.ahdg = [] # selected heading [deg]
self.aspd = [] # selected spd(CAS) [m/s]
self.aptas = [] # just for initializing
self.ama = [] # selected spd above crossover altitude (Mach) [-]
self.apalt = [] # selected alt[m]
self.apfll = [] # selected fl [ft/100]
self.avs = [] # selected vertical speed [m/s]
# limit settings
self.limspd = [] # limit speed
self.limalt = [] # limit altitude
self.limvs = [] # limit vertical speed due to thrust limitation
# Traffic navigation information
self.orig = [] # Four letter code of origin airport
self.dest = [] # Four letter code of destination airport
# LNAV route navigation
self.swlnav = np.array([]) # Lateral (HDG) based on nav?
self.swvnav = np.array([]) # Vertical/longitudinal (ALT+SPD) based on nav info
self.actwplat = np.array([]) # Active WP latitude
self.actwplon = np.array([]) # Active WP longitude
self.actwpalt = np.array([]) # Active WP altitude to arrive at
self.actwpspd = np.array([]) # Active WP speed
self.actwpturn = np.array([]) # Distance when to turn to next waypoint
self.actwpflyby = np.array([]) # Distance when to turn to next waypoint
# VNAV variablescruise level
self.crzalt = np.array([]) # Cruise altitude[m]
self.dist2vs = np.array([]) # Distance to start V/S of VANAV
self.actwpvs = np.array([]) # Actual V/S to use
# Route info
self.route = []
self.desalt = np.array([]) # desired altitude [m]
self.deshdg = np.array([]) # desired heading
self.desvs = np.array([]) # desired vertical speed [m/s]
self.desspd = np.array([]) # desired speed [m/s]
# Display information on label
self.label = [] # Text and bitmap of traffic label
self.trailcol = [] # Trail color: default 'Blue'
# Transmitted data to other aircraft due to truncated effect
self.adsbtime = np.array([])
self.adsblat = np.array([])
self.adsblon = np.array([])
self.adsbalt = np.array([])
self.adsbtrk = np.array([])
self.adsbtas = np.array([])
self.adsbgs = np.array([])
self.adsbvs = np.array([])
#-----------------------------------------------------------------------------
# Not per aircraft data
# Scheduling of FMS and ASAS
self.t0fms = -999. # last time fms was called
self.dtfms = 1.01 # interval for fms
# Flight performance scheduling
self.perfdt = 0.1 # [s] update interval of performance limits
self.perft0 = -self.perfdt # [s] last time checked (in terms of simt)
self.warned2 = False # Flag: Did we warn for default engine parameters yet?
# ADS-B transmission-receiver model
self.adsb = ADSBModel(self)
# Import navigation data base
self.navdb = navdb
# Traffic area: delete traffic when it leaves this area (so not when outside)
self.swarea = False
self.arealat0 = 0.0 # [deg] lower latitude defining area
self.arealat1 = 0.0 # [deg] upper latitude defining area
self.arealon0 = 0.0 # [deg] lower longitude defining area
self.arealon1 = 0.0 # [deg] upper longitude defining area
self.areafloor = -999999.0 # [m] Delete when descending through this h
self.areadt = 5.0 # [s] frequency of area check (simtime)
self.areat0 = -100. # last time checked
self.arearadius = 100.0 # [NM] radius of experiment area if it is a circle
self.inside = []
self.fir_circle_point = (0.0, 0.0)
self.fir_circle_radius = 1.0
# Taxi switch
self.swtaxi = False # Default OFF: delete traffic below 1500 ft
# Research Area ("Square" for Square, "Circle" for Circle area)
self.area = ""
# Bread crumbs for trails
self.lastlat = []
self.lastlon = []
self.lasttim = []
self.trails = Trails()
self.swtrails = False # Default switched off
# Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)
self.setNoise(False)
self.eps = np.array([])
self.asas.reset()
def mcreate(self, count, actype=None, alt=None, spd=None, dest=None, area=None):
""" Create multiple random aircraft in a specified area """
idbase = chr(randint(65, 90)) + chr(randint(65, 90))
if actype is None:
actype = 'B744'
for i in xrange(count):
acid = idbase + '%05d' % i
aclat = random() * (area[1] - area[0]) + area[0]
aclon = random() * (area[3] - area[2]) + area[2]
achdg = float(randint(1, 360))
acalt = (randint(2000, 39000) * ft) if alt is None else alt
acspd = (randint(250, 450) * kts) if spd is None else spd
self.create(acid, actype, aclat, aclon, achdg, acalt, acspd)
def create(self, acid, actype, aclat, aclon, achdg, acalt, casmach):
"""Create an aircraft"""
# Check if not already exist
if self.id.count(acid.upper()) > 0:
return False, acid + " already exists." # already exists do nothing
# Increase number of aircraft
self.ntraf = self.ntraf + 1
# Convert speed
if 0.1 < casmach < 1.0 :
acspd = mach2tas(casmach, acalt)
else:
acspd = cas2tas(casmach, acalt)
# Process input
self.id.append(acid.upper())
self.type.append(actype)
self.lat = np.append(self.lat, aclat)
self.lon = np.append(self.lon, aclon)
self.trk = np.append(self.trk, achdg) # TBD: add conversion hdg => trk
self.alt = np.append(self.alt, acalt)
self.fll = np.append(self.fll, (acalt) / (100 * ft))
self.vs = np.append(self.vs, 0.)
c_temp, c_rho, c_p = vatmos(acalt)
self.p = np.append(self.p, c_p)
self.rho = np.append(self.rho, c_rho)
self.Temp = np.append(self.Temp, c_temp)
self.dtemp = np.append(self.dtemp, 0) # at the moment just ISA conditions
self.tas = np.append(self.tas, acspd)
self.gs = np.append(self.gs, acspd)
self.cas = np.append(self.cas, tas2cas(acspd, acalt))
self.M = np.append(self.M, tas2mach(acspd, acalt))
# AC is initialized with neutral max bank angle
self.bank = np.append(self.bank, radians(25.))
if self.ntraf < 2:
self.bphase = np.deg2rad(np.array([15, 35, 35, 35, 15, 45]))
self.hdgsel = np.append(self.hdgsel, False)
#------------------------------Performance data--------------------------------
# Type specific data
#(temporarily default values)
self.avsdef = np.append(self.avsdef, 1500. * fpm) # default vertical speed of autopilot
self.aphi = np.append(self.aphi, radians(25.)) # bank angle setting of autopilot
self.ax = np.append(self.ax, kts) # absolute value of longitudinal accelleration
# Crossover altitude
self.abco = np.append(self.abco, 0)
self.belco = np.append(self.belco, 1)
# performance data
self.perf.create(actype)
# Traffic autopilot settings: hdg[deg], spd (CAS,m/s), alt[m], vspd[m/s]
self.ahdg = np.append(self.ahdg, achdg) # selected heading [deg]
self.aspd = np.append(self.aspd, tas2cas(acspd, acalt)) # selected spd(cas) [m/s]
self.aptas = np.append(self.aptas, acspd) # [m/s]
self.ama = np.append(self.ama, 0.) # selected spd above crossover (Mach) [-]
self.apalt = np.append(self.apalt, acalt) # selected alt[m]
self.apfll = np.append(self.apfll, (acalt / 100)) # selected fl[ft/100]
self.avs = np.append(self.avs, 0.) # selected vertical speed [m/s]
# limit settings: initialize with 0
self.limspd = np.append(self.limspd, 0.0)
self.limalt = np.append(self.limalt, 0.0)
self.limvs = np.append(self.limvs, 0.0)
# Help variables to save computation time
self.coslat = np.append(self.coslat, cos(radians(aclat))) # Cosine of latitude for flat-earth aproximations
# Traffic navigation information
self.dest.append("")
self.orig.append("")
# LNAV route navigation
self.swlnav = np.append(self.swlnav, False) # Lateral (HDG) based on nav
self.swvnav = np.append(self.swvnav, False) # Vertical/longitudinal (ALT+SPD) based on nav info
self.actwplat = np.append(self.actwplat, 89.99) # Active WP latitude
self.actwplon = np.append(self.actwplon, 0.0) # Active WP longitude
self.actwpalt = np.append(self.actwpalt, 0.0) # Active WP altitude
self.actwpspd = np.append(self.actwpspd, -999.) # Active WP speed
self.actwpturn = np.append(self.actwpturn, 1.0) # Distance to active waypoint where to turn
self.actwpflyby = np.append(self.actwpflyby, 1.0) # Flyby/fly-over switch
# VNAV cruise level
self.crzalt = np.append(self.crzalt, -999.) # Cruise altitude[m] <0=None
self.dist2vs = np.append(self.dist2vs, -999.) # Distance to start V/S of VANAV
self.actwpvs = np.append(self.actwpvs, 0.0) # Actual V/S to use then
# Route info
self.route.append(Route(self.navdb)) # create empty route connected with nav databse
eas = tas2eas(acspd, acalt)
self.desalt = np.append(self.desalt, acalt)
self.desvs = np.append(self.desvs, 0.0)
self.desspd = np.append(self.desspd, eas)
self.deshdg = np.append(self.deshdg, achdg)
# Area variable set to False to avoid deletion upon creation outside
self.inside.append(False)
# Display information on label
self.label.append(['', '', '', 0])
# Bread crumbs for trails
self.trailcol.append(self.trails.defcolor)
self.lastlat = np.append(self.lastlat, aclat)
self.lastlon = np.append(self.lastlon, aclon)
self.lasttim = np.append(self.lasttim, 0.0)
# Transmitted data to other aircraft due to truncated effect
self.adsbtime = np.append(self.adsbtime, np.random.rand(self.trunctime))
self.adsblat = np.append(self.adsblat, aclat)
self.adsblon = np.append(self.adsblon, aclon)
self.adsbalt = np.append(self.adsbalt, acalt)
self.adsbtrk = np.append(self.adsbtrk, achdg)
self.adsbtas = np.append(self.adsbtas, acspd)
self.adsbgs = np.append(self.adsbgs, acspd)
self.adsbvs = np.append(self.adsbvs, 0.)
self.eps = np.append(self.eps, 0.01)
self.asas.create(achdg, eas, acalt)
return True
def delete(self, acid):
"""Delete an aircraft"""
# Look up index of aircraft
idx = self.id2idx(acid)
# Do nothing if not found
if idx < 0:
return False
del self.id[idx]
del self.type[idx]
# Traffic basic data
self.lat = np.delete(self.lat, idx)
self.lon = np.delete(self.lon, idx)
self.trk = np.delete(self.trk, idx)
self.alt = np.delete(self.alt, idx)
self.fll = np.delete(self.fll, idx)
self.vs = np.delete(self.vs, idx)
self.tas = np.delete(self.tas, idx)
self.gs = np.delete(self.gs, idx)
self.cas = np.delete(self.cas, idx)
self.M = np.delete(self.M, idx)
self.p = np.delete(self.p, idx)
self.rho = np.delete(self.rho, idx)
self.Temp = np.delete(self.Temp, idx)
self.dtemp = np.delete(self.dtemp, idx)
self.hdgsel = np.delete(self.hdgsel, idx)
self.bank = np.delete(self.bank, idx)
# Crossover altitude
self.abco = np.delete(self.abco, idx)
self.belco = np.delete(self.belco, idx)
# Type specific data (temporarily default values)
self.avsdef = np.delete(self.avsdef, idx)
self.aphi = np.delete(self.aphi, idx)
self.ax = np.delete(self.ax, idx)
# performance data
self.perf.delete(idx)
# Traffic autopilot settings: hdg[deg], spd (CAS,m/s), alt[m], vspd[m/s]
self.ahdg = np.delete(self.ahdg, idx)
self.aspd = np.delete(self.aspd, idx)
self.ama = np.delete(self.ama, idx)
self.aptas = np.delete(self.aptas, idx)
self.apalt = np.delete(self.apalt, idx)
self.apfll = np.delete(self.apfll, idx)
self.avs = np.delete(self.avs, idx)
# limit settings
self.limspd = np.delete(self.limspd, idx)
self.limalt = np.delete(self.limalt, idx)
self.limvs = np.delete(self.limvs, idx)
# Help variables to save computation time
self.coslat = np.delete(self.coslat, idx) # Cosine of latitude for flat-earth aproximations
# Traffic navigation variables
del self.dest[idx]
del self.orig[idx]
self.swlnav = np.delete(self.swlnav, idx)
self.swvnav = np.delete(self.swvnav, idx)
self.actwplat = np.delete(self.actwplat, idx)
self.actwplon = np.delete(self.actwplon, idx)
self.actwpalt = np.delete(self.actwpalt, idx)
self.actwpspd = np.delete(self.actwpspd, idx)
self.actwpturn = np.delete(self.actwpturn, idx)
self.actwpflyby = np.delete(self.actwpflyby, idx)
# VNAV cruise level
self.crzalt = np.delete(self.crzalt, idx)
self.dist2vs = np.delete(self.dist2vs, idx) # Distance to start V/S of VANAV
self.actwpvs = np.delete(self.actwpvs, idx) # Actual V/S to use
# Route info
del self.route[idx]
self.desalt = np.delete(self.desalt, idx)
self.desvs = np.delete(self.desvs, idx)
self.desspd = np.delete(self.desspd, idx)
self.deshdg = np.delete(self.deshdg, idx)
# Metrics, area
del self.inside[idx]
# Traffic display data: label
del self.label[idx]
# Delete bread crumb data
self.lastlat = np.delete(self.lastlat, idx)
self.lastlon = np.delete(self.lastlon, idx)
self.lasttim = np.delete(self.lasttim, idx)
del self.trailcol[idx]
# Transmitted data to other aircraft due to truncated effect
self.adsbtime = np.delete(self.adsbtime, idx)
self.adsblat = np.delete(self.adsblat, idx)
self.adsblon = np.delete(self.adsblon, idx)
self.adsbalt = np.delete(self.adsbalt, idx)
self.adsbtrk = np.delete(self.adsbtrk, idx)
self.adsbtas = np.delete(self.adsbtas, idx)
self.adsbgs = np.delete(self.adsbgs, idx)
self.adsbvs = np.delete(self.adsbvs, idx)
# Decrease number fo aircraft
self.ntraf = self.ntraf - 1
self.eps = np.delete(self.eps, idx)
self.asas.delete(idx)
return True
def update(self, simt, simdt):
# Update only necessary if there is traffic
if self.ntraf == 0:
return
#---------------- Atmosphere ----------------
self.p, self.rho, self.Temp = vatmos(self.alt)
###############################################################################
# Debugging: add 10000 random aircraft
# if simt>1.0 and self.ntraf<1000:
# for i in range(10000):
# acid="KL"+str(i)
# aclat = random.random()*180.-90.
# aclon = random.random()*360.-180.
# achdg = random.random()*360.
# acalt = (random.random()*18000.+2000.)*0.3048
# self.create(acid,'B747',aclat,aclon,achdg,acalt,350.)
#
#################################################################################
#-------------------- ADSB update: --------------------
self.adsbtime = self.adsbtime + simdt
if self.ADSBtrunc:
ADSB_update = np.where(self.adsbtime > self.trunctime)
else:
ADSB_update = range(self.ntraf)
for i in ADSB_update:
self.adsbtime[i] = self.adsbtime[i] - self.trunctime
self.adsblat[i] = self.lat[i]
self.adsblon[i] = self.lon[i]
self.adsbalt[i] = self.alt[i]
self.adsbtrk[i] = self.trk[i]
self.adsbtas[i] = self.tas[i]
self.adsbgs[i] = self.gs[i]
self.adsbvs[i] = self.vs[i]
# New version ADSB Model
self.adsb.update()
#------------------- ASAS update: ---------------------
# Reset label because of colour change
# Save old result
iconf0 = np.array(self.asas.iconf)
self.asas.update(self, simt)
# TODO: this doesn't work anymore when asas.iconf is a list of lists
# chnged = np.where(iconf0 != np.array(self.asas.iconf))[0]
if settings.gui=="pygame":
for i in range(self.ntraf):
if np.any(iconf0[i] != self.asas.iconf[i]):
self.label[i] = [" ", " ", "", " "]
#----------------- FMS GUIDANCE & NAVIGATION ------------------
# Scheduling: when dt has passed or restart:
if self.t0fms + self.dtfms < simt or simt < self.t0fms:
self.t0fms = simt
# FMS LNAV mode:
qdr, dist = geo.qdrdist(self.lat, self.lon, self.actwplat, self.actwplon) # [deg][nm])
# Check whether shift based dist [nm] is required, set closer than WP turn distance
iwpclose = np.where(self.swlnav * (dist < self.actwpturn))[0]
# Shift waypoints for aircraft i where necessary
for i in iwpclose:
# Get next wp (lnavon = False if no more waypoints)
lat, lon, alt, spd, xtoalt, toalt, lnavon, flyby = \
self.route[i].getnextwp() # note: xtoalt,toalt in [m]
# End of route/no more waypoints: switch off LNAV
if not lnavon:
self.swlnav[i] = False # Drop LNAV at end of route
# In case of no LNAV, do not allow VNAV mode on it sown
if not self.swlnav[i]:
self.swvnav[i] = False
self.actwplat[i] = lat
self.actwplon[i] = lon
self.actwpflyby[i] = int(flyby) # 1.0 in case of fly by, els fly over
# User has entered an altitude for this waypoint
if alt >= 0.:
self.actwpalt[i] = alt
# VNAV = FMS ALT/SPD mode
# calculated altitude is available and active
if toalt >= 0. and self.swvnav[i]: # somewhere there is an altitude constraint ahead
# Descent VNAV mode (T/D logic)
if self.alt[i] > toalt + 10. * ft:
#Steepness dh/dx in [m/m], for now 1:3 rule of thumb
steepness = 3000. * ft / (10. * nm)
#Calculate max allowed altitude at next wp (above toalt)
self.actwpalt[i] = toalt + xtoalt * steepness
# Dist to waypoint where descent should start
self.dist2vs[i] = (self.alt[i] - self.actwpalt[i]) / steepness
# Flat earth distance to next wp
dy = (lat - self.lat[i])
dx = (lon - self.lon[i]) * self.coslat[i]
legdist = 60. * nm * sqrt(dx * dx + dy * dy)
# If descent is urgent, descent with maximum steepness
if legdist < self.dist2vs[i]:
self.apalt[i] = self.actwpalt[i] # dial in altitude of next waypoint as calculated
t2go = max(0.1, legdist) / max(0.01, self.gs[i])
self.actwpvs[i] = (self.actwpalt[i] - self.alt[i]) / t2go
else:
# normal case: still time till descent starts
# Calculate V/s using steepness,
# protect against zero/invalid ground speed value
self.actwpvs[i] = -steepness*(self.gs[i] + \
(self.gs[i]<0.2*self.tas[i])*self.tas[i])
# Climb VNAV mode: climb as soon as possible (T/C logic)
elif self.swvnav[i] and self.alt[i]<toalt-10.*ft:
self.actwpalt[i] = toalt
self.apalt[i] = self.actwpalt[i] # dial in altitude of next waypoint as calculated
self.dist2vs[i] = 9999.
# Level leg: never start V/S
else:
self.dist2vs[i] = -999.
#No altirude defined: never start V/S
else:
self.dist2vs[i] = -999.
# VNAV spd mode: use speed of this waypoint as commanded speed
# while passing waypoint and save next speed for passing next wp
if self.swvnav[i] and self.actwpspd[i]>0.0: # check mode and value
# Select CAS or Mach command by checking value of actwpspd
if self.actwpspd[i]<2.0: # Mach command
self.aspd[i] = mach2cas(self.actwpspd[i],self.alt[i])
self.ama[i] = self.actwpspd[i]
else: # CAS command
self.aspd[i] = self.actwpspd[i]
self.ama[i] = cas2tas(spd,self.alt[i])
if spd>0. and self.swlnav[i] and self.swvnav[i]: # Valid speed and LNAV and VNAV ap modes are on
self.actwpspd[i] = spd
else:
self.actwpspd[i] = -999.
# Calculate distance before waypoint where to start the turn
# Turn radius: R = V2 tan phi / g
# Distance to turn: wpturn = R * tan (1/2 delhdg) but max 4 times radius
# using default bank angle per flight phase
turnrad = self.tas[i]*self.tas[i]/tan(self.bank[i]) /g0 /nm # [nm]
dy = (self.actwplat[i]-self.lat[i])
dx = (self.actwplon[i]-self.lon[i])*self.coslat[i]
qdr[i] = degrees(atan2(dx,dy))
self.actwpturn[i] = self.actwpflyby[i]* \
max(3.,abs(turnrad*tan(radians(0.5*degto180(qdr[i]- \
self.route[i].wpdirfrom[self.route[i].iactwp]))))) # [nm]
#=============== End of Waypoint switching loop ===================
# VNAV Guidance
# Do VNAV start of descent check
dy = (self.actwplat-self.lat)
dx = (self.actwplon-self.lon)*self.coslat
dist2wp = 60.*nm*np.sqrt(dx*dx+dy*dy)
steepness = 3000.*ft/(10.*nm)
# VNAV AP LOGIC: descend as late as possible, climb as soon as possible
# First term: descend when distance to next wp is descent distance
# Second term: climb when still below altitude of next waypoint
self.swvnavvs = self.swlnav*self.swvnav*((dist2wp<self.dist2vs) + \
(self.actwpalt>self.alt))
self.avs = (1-self.swvnavvs)*self.avs + self.swvnavvs*steepness*self.gs
self.aalt = (1-self.swvnavvs)*self.apalt + self.swvnavvs*self.actwpalt
# Set headings based on swlnav
self.ahdg = np.where(self.swlnav, qdr, self.ahdg)
#-------------END of FMS update -------------------
# NOISE: Turbulence
if self.turbulence:
timescale=np.sqrt(simdt)
trkrad=np.radians(self.trk)
#write turbulences in array
turb=np.array(self.standardturbulence)
turb=np.where(turb>1e-6,turb,1e-6)
#horizontal flight direction
turbhf=np.random.normal(0,turb[0]*timescale,self.ntraf) #[m]
#horizontal wing direction
turbhw=np.random.normal(0,turb[1]*timescale,self.ntraf) #[m]
#vertical direction
turbalt=np.random.normal(0,turb[2]*timescale,self.ntraf) #[m]
#lateral, longitudinal direction
turblat=np.cos(trkrad)*turbhf-np.sin(trkrad)*turbhw #[m]
turblon=np.sin(trkrad)*turbhf+np.cos(trkrad)*turbhw #[m]
else:
turbalt=np.zeros(self.ntraf) #[m]
turblat=np.zeros(self.ntraf) #[m]
turblon=np.zeros(self.ntraf) #[m]
# ASAS AP switches
#--------- Input to Autopilot settings to follow: destination or ASAS ----------
# desired autopilot settings due to ASAS
self.deshdg = self.asas.asasactive*self.asas.asashdg + (1-self.asas.asasactive)*self.ahdg
self.desspd = self.asas.asasactive*self.asas.asasspd + (1-self.asas.asasactive)*self.aspd
self.desalt = self.asas.asasactive*self.asas.asasalt + (1-self.asas.asasactive)*self.apalt
self.desvs = self.asas.asasactive*self.asas.asasvsp + (1-self.asas.asasactive)*self.avs
#-------------- Performance limits autopilot settings --------------
# Check difference with AP settings for trafperf and autopilot
self.delalt = self.desalt - self.alt # [m]
# below crossover altitude: CAS=const, above crossover altitude: Mach = const
# aptas has to be calculated before delspd
self.aptas = vcas2tas(self.desspd, self.alt) * self.belco + \
vmach2tas(self.ama, self.alt) * self.abco
self.delspd = self.desspd - self.tas
# check for the flight envelope
self.perf.limits()
# Update autopilot settings with values within the flight envelope
# Autopilot selected speed setting [m/s]
# To do: add const Mach const CAS mode
self.aspd = (self.limspd ==0)*self.desspd + (self.limspd!=0)*self.limspd
# Autopilot selected altitude [m] limited when necessary
self.aalt = (self.limalt ==0)*self.desalt + (self.limalt!=0)*self.limalt
# Autopilot selected heading
self.ahdg = self.deshdg
# Autopilot selected vertical speed (V/S)
self.avs = (self.limvs==0)*self.desvs + (self.limvs!=0)*self.limvs
# To be discussed: Following change in VNAV mode only?
# below crossover altitude: CAS=const, above crossover altitude: MA = const
#climb/descend above crossover: Ma = const, else CAS = const
#ama is fixed when above crossover
swma = np.where(self.abco*(self.ama == 0.)) # Above cross-over
self.ama[swma] = vcas2mach(self.aspd[swma], self.alt[swma])
# ama is deleted when below crossover
swma2 = np.where(self.belco*(self.ama!=0.)) # below corss-over
self.ama[swma2] = 0.
#---------- Basic Autopilot modes ----------
# SPD HOLD/SEL mode: aspd = autopilot selected speed (first only eas)
# for information:
# no more ? self.aptas = (self.actwpspd > 0.01)*self.actwpspd*self.swvnav + \
# np.logical_or((self.actwpspd <= 0.01),np.logical_not (self.swvnav))*self.aptas
self.delspd = self.aptas - self.tas
swspdsel = np.abs(self.delspd) > 0.4 # <1 kts = 0.514444 m/s
ax = np.minimum(abs(self.delspd / max(1e-8,simdt)), self.ax)
self.tas = swspdsel * (self.tas + ax * np.sign(self.delspd) * \
simdt) + (1. - swspdsel) * self.aptas
# Speed conversions
self.cas = vtas2cas(self.tas, self.alt)
self.gs = self.tas
self.M = vtas2mach(self.tas, self.alt)
# Update performance every self.perfdt seconds
if abs(simt - self.perft0) >= self.perfdt:
self.perft0 = simt
self.perf.perf()
# Update aircraft altitude
self.eps = np.array(self.ntraf * [0.01]) # almost zero for misc purposes
swaltsel = np.abs(self.aalt-self.alt) > \
np.maximum(3.,np.abs(2. * simdt * np.abs(self.vs))) # 3.[m] = 10 [ft] eps alt
self.vs = swaltsel*np.sign(self.aalt-self.alt)* \
( (1-self.swvnav)*np.abs(1500./60.*ft) + \
self.swvnav*np.abs(self.avs) )
self.alt = swaltsel * (self.alt + self.vs * simdt) + \
(1. - swaltsel) * self.aalt + turbalt
# HDG HOLD/SEL mode: ahdg = ap selected heading
delhdg = (self.ahdg - self.trk + 180.) % 360 - 180. # [deg]
# omega = np.degrees(g0 * np.tan(self.aphi) / \
# np.maximum(self.tas, self.eps))
# nominal bank angles per phase from BADA 3.12
omega = np.degrees(g0 * np.tan(self.bank) / \
np.maximum(self.tas, self.eps))
self.hdgsel = np.abs(delhdg) > np.abs(2. * simdt * omega)
self.trk = (self.trk + simdt * omega * self.hdgsel * np.sign(delhdg)) % 360.
#--------- Kinematics: update lat,lon,alt ----------
ds = simdt * self.gs
self.lat = self.lat + \
np.degrees((ds * np.cos(np.radians(self.trk)) + turblat) \
/ Rearth)
self.coslat = np.cos(np.deg2rad(self.lat))
self.lon = self.lon + \
np.degrees((ds * np.sin(np.radians(self.trk)) + turblon) \
/ self.coslat / Rearth)
# Update trails when switched on
if self.swtrails:
self.trails.update(simt, self.lat, self.lon,
self.lastlat, self.lastlon,
self.lasttim, self.id, self.trailcol)
else:
self.lastlat = self.lat
self.lastlon = self.lon
self.lasttim[:] = simt
# ----------------AREA check----------------
# Update area once per areadt seconds:
if self.swarea and abs(simt - self.areat0) > self.areadt:
# Update loop timer
self.areat0 = simt
# Check all aircraft
i = 0
while (i < self.ntraf):
# Current status
if self.area == "Square":
inside = self.arealat0 <= self.lat[i] <= self.arealat1 and \
self.arealon0 <= self.lon[i] <= self.arealon1 and \
self.alt[i] >= self.areafloor and \
(self.alt[i] >= 1500 or self.swtaxi)
elif self.area == "Circle":
# delete aircraft if it is too far from the center of the circular area, or if has decended below the minimum altitude
distance = geo.kwikdist(self.arealat0, self.arealon0, self.lat[i], self.lon[i]) # [NM]
inside = distance < self.arearadius and self.alt[i] >= self.areafloor
# Compare with previous: when leaving area: delete command
if self.inside[i] and not inside:
self.delete(self.id[i])
else:
# Update area status
self.inside[i] = inside
i = i + 1
return
def id2idx(self, acid):
"""Find index of aircraft id"""
try:
return self.id.index(acid.upper())
except:
return -1
def setTrails(self, *args):
""" Set trails on/off, or change trail color of aircraft """
if type(args[0]) == bool:
# Set trails on/off
self.swtrails = args[0]
if len(args) > 1:
self.trails.dt = args[1]
if not self.swtrails:
self.trails.clear()
else:
# Change trail color
if len(args) < 2 or args[2] not in ["BLUE", "RED", "YELLOW"]:
return False, "Set aircraft trail color with: TRAIL acid BLUE/RED/YELLOW"
self.changeTrailColor(args[1], args[0])
def changeTrailColor(self, color, idx):
"""Change color of aircraft trail"""
self.trailcol[idx] = self.trails.colorList[color]
return
def setNoise(self, noiseflag=None):
"""Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)"""
if noiseflag is None:
return True, "Noise is currently " + ("on" if self.noise else "off")
self.noise = noiseflag # Noise/turbulence switch
self.trunctime = 1 # seconds
self.transerror = [1, 100, 100 * ft] # [degree,m,m] standard bearing, distance, altitude error
self.standardturbulence = [0, 0.1, 0.1] # m/s standard turbulence (nonnegative)
# in (horizontal flight direction, horizontal wing direction, vertical)
self.turbulence = self.noise
self.ADSBtransnoise = self.noise
self.ADSBtrunc = self.noise
return True
def engchange(self, acid, engid):
"""Change of engines"""
self.perf.engchange(acid, engid)
return
def selhdg(self, idx, hdg): # HDG command
""" Select heading command: HDG acid, hdg """
# Give autopilot commands
self.ahdg[idx] = float(hdg)
self.swlnav[idx] = False
# Everything went ok!
return True
def selspd(self, idx, casmach): # SPD command
""" Select speed command: SPD acid, casmach (= CASkts/Mach) """
# When >=1.0 it is probably CASkts else it is Mach
if 0.1 < casmach < 1.0:
self.aspd[idx] = mach2cas(casmach, self.alt[idx]) # Convert Mach to CAS m/s
self.ama[idx] = casmach
else:
self.aspd[idx] = casmach # CAS m/s
self.ama[idx] = cas2mach(casmach, self.alt[idx])
# Switch off VNAV: SPD command overrides
self.swvnav[idx] = False
return True
def move(self, idx, lat, lon, alt=None, hdg=None, casmach=None, vspd=None):
self.lat[idx] = lat
self.lon[idx] = lon
if alt:
self.alt[idx] = alt
self.aalt[idx] = alt
if hdg:
self.trk[idx] = hdg
self.ahdg[idx] = hdg
if casmach:
# Convert speed
if 0.1 < casmach < 1.0:
self.tas[idx] = mach2tas(casmach, alt)
self.aspd[idx] = mach2cas(casmach, alt)
else:
self.tas[idx] = cas2tas(casmach, alt)
self.aspd[idx] = casmach
if vspd:
self.vs[idx] = vspd
self.swvnav[idx] = False
def selalt(self, idx, alt, vspd=None):
""" Select altitude command: ALT acid, alt, [vspd] """
self.apalt[idx] = alt
self.apfll[idx] = alt / (100. * ft)
self.swvnav[idx] = False
# Check for optional VS argument
if vspd:
self.avs[idx] = vspd
else:
delalt = alt - self.alt[idx]
# Check for VS with opposite sign => use default vs
# by setting autopilot vs to zero
if self.avs[idx] * delalt < 0. and abs(self.avs[idx]) > 0.01:
self.avs[idx] = 0.
def selvspd(self, idx, vspd):
""" Vertical speed autopilot command: VS acid vspd """
self.avs[idx] = vspd
# self.vs[idx] = vspd
self.swvnav[idx] = False
def nom(self, idx):
""" Reset acceleration back to nominal (1 kt/s^2): NOM acid """
self.ax[idx] = kts
def setTaxi(self, flag):
""" Set taxi delete flag: OFF auto deletes traffic below 1500 ft """
self.swtaxi = flag
def setLNAV(self, idx, flag=None):
""" Set LNAV on or off for a specific or for all aircraft """
if idx is None:
# All aircraft are targeted
self.swlnav = np.array(self.ntraf*[flag])
elif flag is None:
return True, (self.id[idx] + ": LNAV is " + "ON" if self.swlnav[idx] else "OFF")
elif flag:
route = self.route[idx]
if route.nwp > 0:
self.swlnav[idx] = True
route.direct(self, idx, route.wpname[route.findact(self, idx)])
else:
return False, ("LNAV " + self.id[idx] + ": no waypoints or destination specified")
else:
self.swlnav[idx] = False
def setVNAV(self, idx, flag=None):
""" Set VNAV on or off for a specific or for all aircraft """
if idx is None:
# All aircraft are targeted
self.swvnav = np.array(self.ntraf*[flag])
elif flag is None:
return True, (self.id[idx] + ": VNAV is " + "ON" if self.swvnav[idx] else "OFF")
elif flag:
if not self.swlnav[idx]:
return False, (self.id[idx] + ": VNAV ON requires LNAV to be ON")
route = self.route[idx]
if route.nwp > 0:
self.swvnav[idx] = True
route.direct(self, idx, route.wpname[route.findact(self, idx)])
else:
return False, ("VNAV " + self.id[idx] + ": no waypoints or destination specified")
else:
self.swvnav[idx] = False
def setDestOrig(self, cmd, idx, *args):
if len(args) == 0:
if cmd == 'DEST':
return True, ('DEST ' + self.id[idx] + ': ' + self.dest[idx])
else:
return True, ('ORIG ' + self.id[idx] + ': ' + self.orig[idx])
route = self.route[idx]
if len(args) == 1:
name = args[0]
apidx = self.navdb.getapidx(name)
if apidx < 0:
return False, (cmd + ": Airport " + name + " not found.")
lat = self.navdb.aplat[apidx]
lon = self.navdb.aplon[apidx]
else:
name = self.id[idx] + cmd
lat, lon = args
if cmd == "DEST":
self.dest[idx] = name
iwp = route.addwpt(self, idx, self.dest[idx], route.dest,
lat, lon, 0.0, self.cas[idx])
# If only waypoint: activate
if (iwp == 0) or (self.orig[idx] != "" and route.nwp == 2):
self.actwplat[idx] = route.wplat[iwp]
self.actwplon[idx] = route.wplon[iwp]
self.actwpalt[idx] = route.wpalt[iwp]
self.actwpspd[idx] = route.wpspd[iwp]
self.swlnav[idx] = True
route.iactwp = iwp
# If not found, say so
elif iwp < 0:
return False, (self.dest[idx] + " not found.")
# Origin: bookkeeping only for now
else:
self.orig[idx] = name
iwp = route.addwpt(self, idx, self.orig[idx], route.orig,
self.lat[idx], self.lon[idx], 0.0, self.cas[idx])
if iwp < 0:
return False, (self.orig[idx] + " not found.")
def acinfo(self, acid):
idx = self.id.index(acid)
actype = self.type[idx]
lat, lon = self.lat[idx], self.lon[idx]
alt, hdg = self.alt[idx] / ft, self.trk[idx]
cas = tas2cas(self.tas[idx], self.alt[idx]) / kts
tas = self.tas[idx] / kts
route = self.route[idx]
line = "Info on %s %s index = %d\n" % (acid, actype, idx) \
+ "Pos = %.2f, %.2f. Spd: %d kts CAS, %d kts TAS\n" % (lat, lon, cas, tas) \
+ "Alt = %d ft, Hdg = %d\n" % (alt, hdg)
if self.swlnav[idx] and route.nwp > 0 and route.iactwp >= 0:
if self.swvnav[idx]:
line += "VNAV, "
line += "LNAV to " + route.wpname[route.iactwp] + "\n"
if self.orig[idx] != "" or self.dest[idx] != "":
line += "Flying"
if self.orig[idx] != "":
line += " from " + self.orig[idx]
if self.dest[idx] != "":
line += " to " + self.dest[idx]
return line
def setArea(self, scr, metric, *args):
if args[0] == 'OFF':
self.swarea = False
self.area = ""
scr.objappend(2, "AREA", None) # delete square areas
scr.objappend(3, "AREA", None) # delete circle areas
return True
if type(args[0]) == float and len(args) >= 4:
# This is a square area
self.arealat0 = min(args[0], args[2])
self.arealat1 = max(args[0], args[2])
self.arealon0 = min(args[1], args[3])
self.arealon1 = max(args[1], args[3])
if len(args) == 5:
self.areafloor = args[4] * ft
else:
self.areafloor = -9999999.
self.area = "Square"
self.swarea = True
scr.objappend(2, "AREA", [args[0], args[1], args[2], args[3]])
# Avoid mass delete due to redefinition of area
self.inside = self.ntraf * [False]
return True
elif args[0] == "FIR" and len(args) <= 3:
for i in range(0, len(self.navdb.fir)):
if args[1] == self.navdb.fir[i][0]:
break
if args[1] != self.navdb.fir[i][0]:
return False, "Unknown FIR, try again"
metric.fir_number = i
metric.fir_circle_point = metric.metric_Area.FIR_circle(self.navdb, metric.fir_number)
metric.fir_circle_radius = float(args[1])
if len(args) == 3:
self.areafloor = args[2] * ft
else:
self.areafloor = -9999999.
self.area = "Circle"
self.swarea = True
self.inside = self.ntraf * [False]
scr.objappend(3, "AREA", [metric.fir_circle_point[0] , metric.fir_circle_point[1], metric.fir_circle_radius])
return True
elif args[0] == "CIRCLE" and len(args) in [4, 5]:
# draw circular experiment area
self.arealat0 = args[1] # Latitude of circle center [deg]
self.arealon0 = args[2] # Longitude of circle center [deg]
self.arearadius = args[3] # Radius of circle Center [NM]
# Deleting traffic flying out of experiment area
self.area = "Circle"
self.swarea = True
if len(args) == 5:
self.areafloor = args[4] * ft # [m]
else:
self.areafloor = -9999999. # [m]
# draw the circular experiment area on the radar gui
scr.objappend(3, "AREA", [self.arealat0, self.arealon0, self.arearadius])
# Avoid mass delete due to redefinition of area
self.inside = self.ntraf * [False]
return True
return False
class Traffic:
"""
Traffic class definition : Traffic data
Methods:
Traffic() : constructor
create(acid,actype,aclat,aclon,achdg,acalt,acspd) : create aircraft
delete(acid) : delete an aircraft from traffic data
update(sim) : do a numerical integration step
trafperf () : calculate aircraft performance parameters
Members: see create
Created by : Jacco M. Hoekstra
"""
def __init__(self, navdb):
self.reset(navdb)
return
def reset(self, navdb):
self.dts = []
self.ntraf = 0
# model-specific parameters.
# Default: BlueSky internal performance model.
# Insert your BADA files to the folder "BlueSky/data/coefficients/BADA"
# for working with EUROCONTROL`s Base of Aircraft Data revision 3.12
self.perf = Perf(self)
self.dts = []
self.ntraf = 0
# Create datalog instance
self.log = Datalog()
# Traffic list & arrays definition
# !!!IMPORTANT NOTE!!!
# Any variables added here should also be added in the Traffic
# methods self.create() (append) and self.delete() (delete)
# which can be found directly below __init__
# Traffic basic flight data
# Traffic basic flight data
self.id = [] # identifier (string)
self.type = [] # aircaft type (string)
self.lat = np.array([]) # latitude [deg]
self.lon = np.array([]) # longitude [deg]
self.trk = np.array([]) # track angle [deg]
self.tas = np.array([]) # true airspeed [m/s]
self.gs = np.array([]) # ground speed [m/s]
self.cas = np.array([]) # callibrated airspeed [m/s]
self.M = np.array([]) # mach number
self.alt = np.array([]) # altitude [m]
self.fll = np.array([]) # flight level [ft/100]
self.vs = np.array([]) # vertical speed [m/s]
self.rho = np.array([]) # atmospheric air density [m/s]
self.temp = np.array([]) # atmospheric air temperature [K]
self.dtemp = np.array([]) # delta t for non-ISA conditions
# Traffic performance data
self.avsdef = np.array([]) # [m/s]default vertical speed of autopilot
self.aphi = np.array([]) # [rad] bank angle setting of autopilot
self.ax = np.array(
[]) # [m/s2] absolute value of longitudinal accelleration
self.bank = np.array([]) # nominal bank angle, [radian]
self.bphase = np.array([]) # standard bank angles per phase
self.hdgsel = np.array([]) # determines whether aircraft is turning
# Crossover altitude
self.abco = np.array([])
self.belco = np.array([])
# Traffic autopilot settings
self.ahdg = [] # selected heading [deg]
self.aspd = [] # selected spd(eas) [m/s]
self.aptas = [] # just for initializing
self.ama = [] # selected spd above crossover altitude (Mach) [-]
self.aalt = [] # selected alt[m]
self.afll = [] # selected fl [ft/100]
self.avs = [] # selected vertical speed [m/s]
# limit settings
self.lspd = [] # limit speed
self.lalt = [] # limit altitude
self.lvs = [] # limit vertical speed due to thrust limitation
# Traffic navigation information
self.orig = [] # Four letter code of origin airport
self.dest = [] # Four letter code of destination airport
# LNAV route navigation
self.swlnav = np.array([]) # Lateral (HDG) based on nav?
self.swvnav = np.array(
[]) # Vertical/longitudinal (ALT+SPD) based on nav info
self.actwplat = np.array([]) # Active WP latitude
self.actwplon = np.array([]) # Active WP longitude
self.actwpalt = np.array([]) # Active WP altitude to arrive at
self.actwpspd = np.array([]) # Active WP speed
self.actwpturn = np.array([]) # Distance when to turn to next waypoint
# VNAV cruise level
self.crzalt = np.array([]) # Cruise altitude[m]
# Route info
self.route = []
# ASAS info per aircraft:
self.iconf = [] # index in 'conflicting' aircraft database
self.asasactive = np.array(
[]) # whether the autopilot follows ASAS or not
self.asashdg = np.array([]) # heading provided by the ASAS [deg]
self.asasspd = np.array([]) # speed provided by the ASAS (eas) [m/s]
self.asasalt = np.array([]) # speed alt by the ASAS [m]
self.asasvsp = np.array([]) # speed vspeed by the ASAS [m/s]
self.desalt = np.array([]) #desired altitude [m]
self.deshdg = np.array([]) #desired heading
self.desvs = np.array([]) #desired vertical speed [m/s]
self.desspd = np.array([]) #desired speed [m/s]
# Display information on label
self.label = [] # Text and bitmap of traffic label
self.trailcol = [] # Trail color: default 'Blue'
# Transmitted data to other aircraft due to truncated effect
self.adsbtime = np.array([])
self.adsblat = np.array([])
self.adsblon = np.array([])
self.adsbalt = np.array([])
self.adsbtrk = np.array([])
self.adsbtas = np.array([])
self.adsbgs = np.array([])
self.adsbvs = np.array([])
self.inconflict = np.array([], dtype=bool)
#-----------------------------------------------------------------------------
# Not per aircraft data
# Scheduling of FMS and ASAS
self.t0fms = -999. # last time fms was called
self.dtfms = 1.01 # interval for fms
self.t0asas = -999. # last time ASAS was called
self.dtasas = 1.00 # interval for ASAS
# Flight performance scheduling
self.perfdt = 0.1 # [s] update interval of performance limits
self.perft0 = -self.perfdt # [s] last time checked (in terms of simt)
self.warned2 = False # Flag: Did we warn for default engine parameters yet?
# ADS-B transmission-receiver model
self.adsb = ADSBModel(self)
# ASAS objects: Conflict Database
self.dbconf = Dbconf(self, 300., 5. * nm,
1000. * ft) # hard coded values to be replaced
# Import navigation data base
self.navdb = navdb
# Traffic area: delete traffic when it leaves this area (so not when outside)
self.swarea = False
self.arealat0 = 0.0 # [deg] lower latitude defining area
self.arealat1 = 0.0 # [deg] upper latitude defining area
self.arealat0 = 0.0 # [deg] lower longitude defining area
self.arealat1 = 0.0 # [deg] upper longitude defining area
self.areafloor = -999999.0 # [m] Delete when descending through this h
self.areadt = 5.0 # [s] frequency of area check (simtime)
self.areat0 = -100. # last time checked
self.inside = []
self.fir_circle_point = (0.0, 0.0)
self.fir_circle_radius = 1.0
# Taxi switch
self.swtaxi = False # Default OFF: delete traffic below 1500 ft
# Research Area ("Square" for Square, "Circle" for Circle area)
self.area = ""
# Bread crumbs for trails
self.lastlat = []
self.lastlon = []
self.lasttim = []
self.trails = Trails()
self.swtrails = False # Default switched off
# ADS-B Coverage area
self.swAdsbCoverage = False
# Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)
self.setNoise(False)
self.eps = np.array([])
return
def create_ac(self, acid, actype, aclat, aclon, achdg, acalt, acspd):
"""Create an aircraft"""
# Check if not already exist
if self.id.count(acid.upper()) > 0:
return False # already exists do nothing
# Increase number of aircraft
self.ntraf = self.ntraf + 1
# Process input
self.id.append(acid.upper())
self.type.append(actype)
self.lat = np.append(self.lat, aclat)
self.lon = np.append(self.lon, aclon)
self.trk = np.append(self.trk, achdg) # TBD: add conversion hdg => trk
self.alt = np.append(self.alt, acalt)
self.fll = np.append(self.fll, (acalt) / 100)
self.vs = np.append(self.vs, 0.)
self.rho = np.append(self.rho, density(acalt))
self.temp = np.append(self.temp, temp(acalt))
self.dtemp = np.append(self.dtemp,
0) # at the moment just ISA conditions
self.tas = np.append(self.tas, acspd)
self.gs = np.append(self.gs, acspd)
self.cas = np.append(self.cas, tas2cas(acspd, acalt))
self.M = np.append(self.M, tas2mach(acspd, acalt))
# AC is initialized with neutral max bank angle
self.bank = np.append(self.bank, 25.)
if self.ntraf < 2:
self.bphase = np.deg2rad(np.array([15, 35, 35, 35, 15, 45]))
self.hdgsel = np.append(self.hdgsel, False)
#------------------------------Performance data--------------------------------
# Type specific data
#(temporarily default values)
self.avsdef = np.append(
self.avsdef, 1500. * fpm) # default vertical speed of autopilot
self.aphi = np.append(self.aphi,
radians(25.)) # bank angle setting of autopilot
self.ax = np.append(
self.ax, kts) # absolute value of longitudinal accelleration
# Crossover altitude
self.abco = np.append(self.abco, 0)
self.belco = np.append(self.belco, 1)
# performance data
self.perf.create(actype)
# Traffic autopilot settings: hdg[deg], spd (CAS,m/s), alt[m], vspd[m/s]
self.ahdg = np.append(self.ahdg, achdg) # selected heading [deg]
self.aspd = np.append(self.aspd,
tas2eas(acspd, acalt)) # selected spd(eas) [m/s]
self.aptas = np.append(self.aptas, vcas2tas(self.aspd,
self.alt)) # [m/s]
self.ama = np.append(self.ama,
0.) # selected spd above crossover (Mach) [-]
self.aalt = np.append(self.aalt, acalt) # selected alt[m]
self.afll = np.append(self.afll, (acalt / 100)) # selected fl[ft/100]
self.avs = np.append(self.avs, 0.) # selected vertical speed [m/s]
# limit settings: initialize with 0
self.lspd = np.append(self.lspd, 0.0)
self.lalt = np.append(self.lalt, 0.0)
self.lvs = np.append(self.lvs, 0.0)
# Traffic navigation information
self.dest.append("")
self.orig.append("")
# LNAV route navigation
self.swlnav = np.append(self.swlnav,
False) # Lateral (HDG) based on nav
self.swvnav = np.append(
self.swvnav,
False) # Vertical/longitudinal (ALT+SPD) based on nav info
self.actwplat = np.append(self.actwplat, 89.99) # Active WP latitude
self.actwplon = np.append(self.actwplon, 0.0) # Active WP longitude
self.actwpalt = np.append(self.actwpalt, 0.0) # Active WP altitude
self.actwpspd = np.append(self.actwpspd, -999.) # Active WP speed
self.actwpturn = np.append(
self.actwpturn, 1.0) # Distance to active waypoint where to turn
# VNAV cruise level
self.crzalt = np.append(self.crzalt,
-999.) # Cruise altitude[m] <0=None
# Route info
self.route.append(Route(
self.navdb)) # create empty route connected with nav databse
# ASAS info: no conflict => -1
self.iconf.append(-1) # index in 'conflicting' aircraft database
self.asasactive = np.append(self.asasactive, False)
self.asashdg = np.append(self.asashdg, achdg)
self.asasspd = np.append(self.asasspd, tas2eas(acspd, acalt))
self.asasalt = np.append(self.asasalt, acalt)
self.asasvsp = np.append(self.asasvsp, 0.)
# Area variable set to False to avoid deletion upon creation outside
self.inside.append(False)
# Display information on label
self.label.append(['', '', '', 0])
# Bread crumbs for trails
self.trailcol.append(self.trails.defcolor)
self.lastlat = np.append(self.lastlat, aclat)
self.lastlon = np.append(self.lastlon, aclon)
self.lasttim = np.append(self.lasttim, 0.0)
# ADS-B Coverage area
self.swAdsbCoverage = False
# Transmitted data to other aircraft due to truncated effect
self.adsbtime = np.append(self.adsbtime,
np.random.rand(self.trunctime))
self.adsblat = np.append(self.adsblat, aclat)
self.adsblon = np.append(self.adsblon, aclon)
self.adsbalt = np.append(self.adsbalt, acalt)
self.adsbtrk = np.append(self.adsbtrk, achdg)
self.adsbtas = np.append(self.adsbtas, acspd)
self.adsbgs = np.append(self.adsbgs, acspd)
self.adsbvs = np.append(self.adsbvs, 0.)
self.inconflict = np.append(self.inconflict, False)
self.eps = np.append(self.eps, 0.01)
return True
def delete(self, acid):
"""Delete an aircraft"""
try: # prevent error due to not found
idx = self.id.index(acid)
except:
return False
del self.id[idx]
del self.type[idx]
# Traffic basic data
self.lat = np.delete(self.lat, idx)
self.lon = np.delete(self.lon, idx)
self.trk = np.delete(self.trk, idx)
self.alt = np.delete(self.alt, idx)
self.fll = np.delete(self.fll, idx)
self.vs = np.delete(self.vs, idx)
self.tas = np.delete(self.tas, idx)
self.gs = np.delete(self.gs, idx)
self.cas = np.delete(self.cas, idx)
self.M = np.delete(self.M, idx)
self.T = np.delete(self.T, idx)
self.p = np.delete(self.p, idx)
self.rho = np.delete(self.rho, idx)
self.temp = np.delete(self.temp, idx)
self.dtemp = np.delete(self.dtemp, idx)
self.hdgsel = np.delete(self.hdgsel, idx)
self.bank = np.delete(self.bank, idx)
# Crossover altitude
self.abco = np.delete(self.abco, idx)
self.belco = np.delete(self.belco, idx)
# Type specific data (temporarily default values)
self.avsdef = np.delete(self.avsdef, idx)
self.aphi = np.delete(self.aphi, idx)
self.ax = np.delete(self.ax, idx)
# performance data
self.perf.delete(idx)
# Traffic autopilot settings: hdg[deg], spd (CAS,m/s), alt[m], vspd[m/s]
self.ahdg = np.delete(self.ahdg, idx)
self.aspd = np.delete(self.aspd, idx)
self.ama = np.delete(self.ama, idx)
self.aptas = np.delete(self.aptas, idx)
self.aalt = np.delete(self.aalt, idx)
self.afll = np.delete(self.afll, idx)
self.avs = np.delete(self.avs, idx)
# limit settings
self.lspd = np.delete(self.lspd, idx)
self.lalt = np.delete(self.lalt, idx)
self.lvs = np.delete(self.lvs, idx)
# Traffic navigation variables
del self.dest[idx]
del self.orig[idx]
self.swlnav = np.delete(self.swlnav, idx)
self.swvnav = np.delete(self.swvnav, idx)
self.actwplat = np.delete(self.actwplat, idx)
self.actwplon = np.delete(self.actwplon, idx)
self.actwpalt = np.delete(self.actwpalt, idx)
self.actwpspd = np.delete(self.actwpspd, idx)
self.actwpturn = np.delete(self.actwpturn, idx)
# VNAV cruise level
self.crzalt = np.delete(self.crzalt, idx)
# Route info
del self.route[idx]
# ASAS info
del self.iconf[idx]
self.asasactive = np.delete(self.asasactive, idx)
self.asashdg = np.delete(self.asashdg, idx)
self.asasspd = np.delete(self.asasspd, idx)
self.asasalt = np.delete(self.asasalt, idx)
self.asasvsp = np.delete(self.asasvsp, idx)
self.desalt = np.delete(self.desalt, idx)
self.desvs = np.delete(self.desvs, idx)
self.desspd = np.delete(self.desspd, idx)
self.deshdg = np.delete(self.deshdg, idx)
# Metrics, area
del self.inside[idx]
# Traffic display data: label
del self.label[idx]
# Delete bread crumb data
self.lastlat = np.delete(self.lastlat, idx)
self.lastlon = np.delete(self.lastlon, idx)
self.lasttim = np.delete(self.lasttim, idx)
del self.trailcol[idx]
# Transmitted data to other aircraft due to truncated effect
self.adsbtime = np.delete(self.adsbtime, idx)
self.adsblat = np.delete(self.adsblat, idx)
self.adsblon = np.delete(self.adsblon, idx)
self.adsbalt = np.delete(self.adsbalt, idx)
self.adsbtrk = np.delete(self.adsbtrk, idx)
self.adsbtas = np.delete(self.adsbtas, idx)
self.adsbgs = np.delete(self.adsbgs, idx)
self.adsbvs = np.delete(self.adsbvs, idx)
self.inconflict = np.delete(self.inconflict, idx)
# Decrease number fo aircraft
self.ntraf = self.ntraf - 1
self.eps = np.delete(self.eps, idx)
return True
def deleteall(self):
"""Clear traffic buffer"""
ndel = self.ntraf
for i in range(ndel):
self.delete(self.id[-1])
self.ntraf = 0
self.dbconf.reset()
self.perf.reset()
return
def update(self, simt, simdt):
# i = self.id2idx("TP233")
# if i>=0:
# print "LNAV TP233:",self.swlnav[i]
# Update only necessary if there is traffic
if self.ntraf == 0:
return
self.dts.append(simdt)
#---------------- Atmosphere ----------------
self.T, self.rho, self.p = vatmos(self.alt)
#-------------- Performance limits autopilot settings --------------
# Check difference with AP settings for trafperf and autopilot
self.delalt = self.aalt - self.alt # [m]
# below crossover altitude: CAS=const, above crossover altitude: MA = const
# aptas hast to be calculated before delspd
self.aptas = vcas2tas(self.aspd, self.alt) * self.belco + vmach2tas(
self.ama, self.alt) * self.abco
self.delspd = self.aptas - self.tas
###############################################################################
# Debugging: add 10000 random aircraft
# if simt>1.0 and self.ntraf<1000:
# for i in range(10000):
# acid="KL"+str(i)
# aclat = random.random()*180.-90.
# aclon = random.random()*360.-180.
# achdg = random.random()*360.
# acalt = (random.random()*18000.+2000.)*0.3048
# self.create(acid,'B747',aclat,aclon,achdg,acalt,350.)
#
#################################################################################
#-------------------- ADSB update: --------------------
self.adsbtime = self.adsbtime + simdt
if self.ADSBtrunc:
ADSB_update = np.where(self.adsbtime > self.trunctime)
else:
ADSB_update = range(self.ntraf)
for i in ADSB_update:
self.adsbtime[i] = self.adsbtime[i] - self.trunctime
self.adsblat[i] = self.lat[i]
self.adsblon[i] = self.lon[i]
self.adsbalt[i] = self.alt[i]
self.adsbtrk[i] = self.trk[i]
self.adsbtas[i] = self.tas[i]
self.adsbgs[i] = self.gs[i]
self.adsbvs[i] = self.vs[i]
# New version ADSB Model
self.adsb.update()
#------------------- ASAS update: ---------------------
# Scheduling: when dt has passed or restart:
if self.t0asas+self.dtasas<simt or simt<self.t0asas \
and self.dbconf.swasas:
self.t0asas = simt
# Save old result
iconf0 = np.array(self.iconf)
# Call with traffic database and sim data
self.dbconf.detect()
self.dbconf.conflictlist(simt)
self.dbconf.APorASAS()
self.dbconf.resolve()
# Reset label because of colour change
chnged = np.where(iconf0 != np.array(self.iconf))[0]
for i in chnged:
self.label[i] = [" ", " ", "", " "]
#----------------- FMS GUIDANCE & NAVIGATION ------------------
# Scheduling: when dt has passed or restart:
if self.t0fms + self.dtfms < simt or simt < self.t0fms:
self.t0fms = simt
# FMS LNAV mode:
qdr, dist = qdrdist(self.lat, self.lon, self.actwplat,
self.actwplon) #[deg][nm]
# Check whether shift based dist [nm] is required, set closer than WP turn distance
iwpclose = np.where(self.swlnav * (dist < self.actwpturn))[0]
# Shift for aircraft i where necessary
for i in iwpclose:
# Get next wp (lnavon = False if no more waypoints)
lat, lon, alt, spd, xtoalt, toalt, lnavon = \
self.route[i].getnextwp() # note: xtoalt,toalt in [m]
# End of route/no more waypoints: switch off LNAV
if not lnavon:
self.swlnav[i] = False # Drop LNAV at end of route
# In case of no LNAV, do not allow VNAV mode on it sown
if not self.swlnav[i]:
self.swvnav[i] = False
self.actwplat[i] = lat
self.actwplon[i] = lon
# User entered altitude
if alt >= 0.:
self.actwpalt[i] = alt
# VNAV calculated altitude is available and active
if toalt >= 0. and self.swvnav[i]:
# Descent VNAV mode (T/D logic)
if self.alt[
i] > toalt + 10. * ft: # Descent part is in this range of waypoints:
# Flat earth distance to next wp
dy = (lat - self.lat[i])
dx = (lon - self.lon[i]) * cos(radians(lat))
dist2wp = 60. * nm * sqrt(dx * dx + dy * dy)
steepness = 3000. * ft / (
10. * nm) # 1:3 rule of thumb for now
maxaltwp = toalt + xtoalt * steepness # max allowed altitude at next wp
self.actwpalt[i] = min(
self.alt[i],
maxaltwp) #To descend now or descend later?
if maxaltwp < self.alt[
i]: # if descent is necessary with maximum steepness
self.aalt[i] = self.actwpalt[
i] # dial in altitude of next waypoint as calculated
t2go = max(0.1, dist2wp) / max(0.01, self.gs[i])
self.avs[i] = (
self.actwpalt[i] - self.alt[i]) / t2go
else:
print "else 1"
pass # TBD
# Climb VNAV mode: climb as soon as possible (T/C logic)
elif self.swvnav[i] and self.alt[i] < toalt - 10. * ft:
# Flat earth distance to next wp
dy = (lat - self.lat[i])
dx = (lon - self.lon[i]) * cos(radians(lat))
dist2wp = 60. * nm * sqrt(dx * dx + dy * dy)
self.aalt[i] = self.actwpalt[
i] # dial in altitude of next waypoint as calculated
t2go = max(0.1, dist2wp) / max(0.01, self.gs[i])
self.avs[i] = (self.actwpalt[i] - self.alt[i]) / t2go
if spd > 0. and lnavon and self.swvnav[i]:
if spd < 2.0:
self.actwpspd[i] = mach2cas(spd, self.alt[i])
else:
self.actwpspd[i] = cas2tas(spd, self.alt[i])
else:
self.actwpspd[i] = -999.
# Calculate distance before waypoint where to start the turn
# Turn radius: R = V2 tan phi / g
# Distance to turn: wpturn = R * tan (1/2 delhdg) but max 4 times radius
turnrad = self.tas[i] * self.tas[i] / tan(
self.bank[i]
) / g0 / nm # [nm] default bank angle per flight phase
# print turnrad
dy = (self.actwplat[i] - self.lat[i])
dx = (self.actwplon[i] - self.lon[i]) * cos(
radians(self.lat[i]))
qdr[i] = degrees(atan2(dx, dy))
self.actwpturn[i] = max(3.,abs(turnrad*tan(radians(0.5*degto180(qdr[i]- \
self.route[i].wpdirfrom[self.route[i].iactwp]))))) # [nm]
# Set headings based on swlnav
self.ahdg = np.where(self.swlnav, qdr, self.ahdg)
# NOISE: Turbulence
if self.turbulence:
timescale = np.sqrt(simdt)
trkrad = np.radians(self.trk)
#write turbulences in array
turb = np.array(self.standardturbulence)
turb = np.where(turb > 1e-6, turb, 1e-6)
#horizontal flight direction
turbhf = np.random.normal(0, turb[0] * timescale, self.ntraf) #[m]
#horizontal wing direction
turbhw = np.random.normal(0, turb[1] * timescale, self.ntraf) #[m]
#vertical direction
turbalt = np.random.normal(0, turb[2] * timescale,
self.ntraf) #[m]
#latitudinal, longitudinal direction
turblat = np.cos(trkrad) * turbhf - np.sin(trkrad) * turbhw #[m]
turblon = np.sin(trkrad) * turbhf + np.cos(trkrad) * turbhw #[m]
else:
turbalt = np.zeros(self.ntraf) #[m]
turblat = np.zeros(self.ntraf) #[m]
turblon = np.zeros(self.ntraf) #[m]
# ASAS AP switches
#--------- Select Autopilot settings to follow: destination or ASAS ----------
# desired autopilot settings due to ASAS
self.deshdg = self.asasactive * self.asashdg + (
1 - self.asasactive) * self.ahdg
self.desspd = self.asasactive * self.asasspd + (
1 - self.asasactive) * self.aspd
self.desalt = self.asasactive * self.asasalt + (
1 - self.asasactive) * self.aalt
self.desvs = self.asasactive * self.asasvsp + (
1 - self.asasactive) * self.avs
# check for the flight envelope
self.perf.limits()
# update autopilot settings with values within the flight envelope
# speed
self.aspd = (self.lspd == 0) * self.desspd + (self.lspd !=
0) * self.lspd
# altitude
self.aalt = (self.lalt == 0) * self.desalt + (self.lalt !=
0) * self.lalt
# hdg
self.ahdg = self.deshdg
# vs
self.avs = (self.lvs == 0) * self.desvs + (self.lvs != 0) * self.lvs
# below crossover altitude: CAS=const, above crossover altitude: MA = const
#climb/descend above crossover: Ma = const, else CAS = const
#ama is fixed when above crossover
check = self.abco * (self.ama == 0.)
swma = np.where(check == True)
self.ama[swma] = cas2mach(self.aspd[swma], self.alt[swma])
# ama is deleted when below crossover
check2 = self.belco * (self.ama != 0.)
swma2 = np.where(check2 == True)
self.ama[swma2] = 0.
#---------- Basic Autopilot modes ----------
# SPD HOLD/SEL mode: aspd = autopilot selected speed (first only eas)
# for information:
self.aptas = (self.actwpspd>0.)*self.actwpspd + \
(self.actwpspd<=0.)*self.aptas
self.delspd = self.aptas - self.tas
swspdsel = np.abs(self.delspd) > 0.4 # <1 kts = 0.514444 m/s
ax = np.minimum(abs(self.delspd / max(1e-8, simdt)), self.ax)
self.tas = swspdsel * (self.tas + ax * np.sign(self.delspd) * \
simdt) + (1. - swspdsel) * self.aptas
# print "DELSPD", self.delspd/simdt, "AX", self.ax, "SELECTED", ax
# without that part: non-accelerating ac would have TAS = 0
# print "1-sw", (1. - swspdsel) * self.aptas
# print "NEW TAS", self.tas
# Speed conversions
self.cas = vtas2cas(self.tas, self.alt)
self.gs = self.tas
self.M = vtas2mach(self.tas, self.alt)
# Update performance every self.perfdt seconds
if abs(simt - self.perft0) >= self.perfdt:
self.perft0 = simt
self.perf.perf()
# update altitude
self.eps = np.array(
self.ntraf * [0.01]) # almost zero for misc purposes
swaltsel = np.abs(self.aalt-self.alt) > \
np.maximum(3.,np.abs(2. * simdt * np.abs(self.vs))) # 3.[m] = 10 [ft] eps alt
# print swaltsel
self.vs = swaltsel*np.sign(self.aalt-self.alt)* \
( (1-self.swvnav)*np.abs(1500./60.*ft) + \
self.swvnav*np.abs(self.avs) )
self.alt = swaltsel * (self.alt + self.vs * simdt) + \
(1. - swaltsel) * self.aalt + turbalt
# HDG HOLD/SEL mode: ahdg = ap selected heading
delhdg = (self.ahdg - self.trk + 180.) % 360 - 180. #[deg]
# print delhdg
# omega = np.degrees(g0 * np.tan(self.aphi) / \
# np.maximum(self.tas, self.eps))
# nominal bank angles per phase from BADA 3.12
omega = np.degrees(g0 * np.tan(self.bank) / \
np.maximum(self.tas, self.eps))
self.hdgsel = np.abs(delhdg) > np.abs(2. * simdt * omega)
self.trk = (
self.trk + simdt * omega * self.hdgsel * np.sign(delhdg)) % 360.
#--------- Kinematics: update lat,lon,alt ----------
ds = simdt * self.gs
self.lat = self.lat + np.degrees(ds * np.cos(np.radians(self.trk)+turblat) \
/ Rearth)
self.lon = self.lon + np.degrees(ds * np.sin(np.radians(self.trk)+turblon) \
/ np.cos(np.radians(self.lat)) / Rearth)
# Update trails when switched on
if self.swtrails:
self.trails.update(simt, self.lat, self.lon, self.lastlat,
self.lastlon, self.lasttim, self.id,
self.trailcol)
else:
self.lastlat = self.lat
self.lastlon = self.lon
self.lattime = simt
# ----------------AREA check----------------
# Update area once per areadt seconds:
if self.swarea and abs(simt - self.areat0) > self.areadt:
# Update loop timer
self.areat0 = simt
# Check all aircraft
i = 0
while (i < self.ntraf):
# Current status
if self.area == "Square":
inside = self.arealat0 <= self.lat[i] <= self.arealat1 and \
self.arealon0 <= self.lon[i] <= self.arealon1 and \
self.alt[i] >= self.areafloor and \
(self.alt[i] >= 1500 or self.swtaxi)
elif self.area == "Circle":
pass
## Average of lat
latavg = (radians(self.lat[i]) + radians(
self.fir_circle_point[0])) / 2
cosdlat = (cos(latavg))
# Distance x to centroid
dx = (
self.lon[i] - self.fir_circle_point[1]) * cosdlat * 60
dx2 = dx * dx
# Distance y to centroid
dy = self.lat[i] - self.fir_circle_point[0]
dy2 = dy * dy * 3600
# Radius squared
r2 = self.fir_circle_radius * self.fir_circle_radius
# Inside if smaller
inside = (dx2 + dy2) < r2
# Compare with previous: when leaving area: delete command
if self.inside[i] and not inside:
self.delete(self.id[i])
else:
# Update area status
self.inside[i] = inside
i = i + 1
return
def id2idx(self, acid):
"""Find index of aircraft id"""
try:
return self.id.index(acid.upper())
except:
return -1
def changeTrailColor(self, color, idx):
"""Change color of aircraft trail"""
# print color
# print idx
# print " " + str(self.trails.colorsOfAC[idx])
self.trailcol[idx] = self.trails.colorList[color]
# print " " + str(self.trails.colorsOfAC[idx])
return
def setNoise(self, A):
"""Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)"""
self.noise = A
self.trunctime = 1 # seconds
self.transerror = [
1, 100, 100 * ft
] #[degree,m,m] standard bearing, distance, altitude error
self.standardturbulence = [0, 0.1, 0.1
] #m/s standard turbulence (nonnegative)
# in (horizontal flight direction, horizontal wing direction, vertical)
self.turbulence = self.noise
self.ADSBtransnoise = self.noise
self.ADSBtrunc = self.noise
def engchange(self, acid, engid):
"""Change of engines"""
self.perf.engchange(acid, engid)
return
def create(self, arglist): # CRE command
if len(arglist) < 7:
return False
acid = arglist[0]
if self.id.count(acid.upper()) > 0:
return False, acid + " already exists"
actype = arglist[1]
aclat = arglist[2]
aclon = arglist[3]
achdg = arglist[4]
acalt = arglist[5] # m
cmdspd = arglist[6] # Mach/IAS kts (float
if 0.1 < cmdspd < 1.0:
acspd = mach2tas(cmdspd, acalt)
else:
acspd = cas2tas(cmdspd * kts, acalt)
sw = self.create_ac(acid, actype, aclat, aclon, achdg, acalt, acspd)
return sw
def selhdg(self, arglist): # HDG command
# Select heading command: HDG acid, hdg
if len(arglist) < 2:
return False #Error/Display helptext
print "en voorbij de check"
# unwrap arguments
idx = arglist[0] # aircraft index
hdg = arglist[1] # float
# Give autopilot commands
self.ahdg[idx] = hdg
self.swlnav[idx] = False
# Everything went ok!
return True