def test_delwpt(start):
global starttime, timer, wp2reached
timelimit = 240
if start:
# Create a waypoint which does not lie on the aircraft path
# And create a next waypoint to reach after that
stack.stack("CRE KLM10 B747 52 4 000 FL99 150")
stack.stack("DEFWPT WPDEL1 52.0 4.1")
stack.stack("DEFWPT WPDEL2 52.1 4.2")
stack.stack("DEFWPT WPDEL3 52.0 4.3")
stack.stack("KLM10 ADDWPT WPDEL1")
stack.stack("KLM10 ADDWPT WPDEL2")
stack.stack("KLM10 ADDWPT WPDEL3")
stack.stack("KLM10 DELWPT WPDEL2")
starttime = timer
wp2reached = False
else:
_, d2 = geo.qdrdist(52.1 ,4.2 ,traf.lat[0],traf.lon[0])
if d2 < 1:
wp2reached = True
# When the aircraft reaches waypoint 3 within the time limit return success
_, d = geo.qdrdist(52.0 ,4.3 ,traf.lat[0],traf.lon[0])
wp3reached = d < 1 # d is measured in nm
if timer-starttime>timelimit or wp3reached:
return wp3reached and not wp2reached
def test_direct(start):
global starttime, timer, wp1reached
timelimit = 350
if start:
# Create a waypoint which does not lie on the aircraft path
# And create the waypoint to direct to
stack.stack("CRE KLM10 B747 52 4 000 FL99 150")
stack.stack("DEFWPT WPDIR1 52.2 4.1")
stack.stack("DEFWPT WPDIR2 52.2 3.9")
stack.stack("KLM10 ADDWPT WPDIR1")
stack.stack("KLM10 ADDWPT WPDIR2")
stack.stack("KLM10 DIRECT WPDIR2")
starttime = timer
wp1reached = False
else:
# If close to WP1, save that WP1 is reached
_, d1 = geo.qdrdist(52.2 , 4.1 ,traf.lat[0],traf.lon[0])
if d1 < 1:
wp1reached = True
# When the aircraft reaches waypoint 2 within the time limit, return success
_, d2 = geo.qdrdist(52.2 , 3.9 ,traf.lat[0],traf.lon[0])
closeenough = d2 < 1 # d is measured in nm
if timer-starttime>timelimit or closeenough:
return closeenough and not wp1reached
def ll2xy(self, lat, lon):
if not self.swnavdisp:
# RADAR mode:
# Convert lat/lon to pixel x,y
# Normal case
if self.lon1 > self.lon0:
x = self.width * (lon - self.lon0) / (self.lon1 - self.lon0)
# Wrap around:
else:
dellon = 180. - self.lon0 + self.lon1 + 180.
xlon = lon + (lon < 0.) * 360.
x = (xlon - self.lon0) / dellon * self.width
y = self.height * (self.lat1 - lat) / (self.lat1 - self.lat0)
else:
# NAVDISP mode:
qdr, dist = geo.qdrdist(self.ndlat, self.ndlon, lat, lon)
alpha = np.radians(qdr - self.ndcrs)
base = 30. * (self.lat1 - self.lat0)
x = dist * np.sin(alpha) / base * self.height + self.width / 2
y = -dist * np.cos(alpha) / base * self.height + self.height / 2
return np.rint(x), np.rint(y)
def calcfp(self):
"""Do flight plan calculations"""
# self.delwpt("T/D")
# self.delwpt("T/C")
# Direction to waypoint
self.nwp = len(self.wpname)
# Create flight plan calculation table
self.wpdirfrom = self.nwp*[0.]
self.wpdistto = self.nwp*[0.]
self.wpialt = self.nwp*[-1]
self.wptoalt = self.nwp*[-999.]
self.wpxtoalt = self.nwp*[1.]
# No waypoints: make empty variables to be safe and return: nothing to do
if self.nwp==0:
return
# Calculate lateral leg data
# LNAV: Calculate leg distances and directions
for i in range(0, self.nwp - 1):
qdr,dist = geo.qdrdist(self.wplat[i] ,self.wplon[i],
self.wplat[i+1],self.wplon[i+1])
self.wpdirfrom[i] = qdr
self.wpdistto[i+1] = dist #[nm] distto is in nautical miles
if self.nwp>1:
self.wpdirfrom[-1] = self.wpdirfrom[-2]
# Calclate longitudinal leg data
# VNAV: calc next altitude constraint: index, altitude and distance to it
ialt = -1
toalt = -999.
xtoalt = 0.
for i in range(self.nwp-1,-1,-1):
# waypoint with altitude constraint (dest of al specified)
if self.wptype[i]==Route.dest:
ialt = i
toalt = 0.
xtoalt = 0. # [m]
elif self.wpalt[i] >= 0:
ialt = i
toalt = self.wpalt[i]
xtoalt = 0. # [m]
# waypoint with no altitude constraint:keep counting
else:
if i!=self.nwp-1:
xtoalt += self.wpdistto[i+1]*nm # [m] xtoalt is in meters!
else:
xtoalt = 0.0
self.wpialt[i] = ialt
self.wptoalt[i] = toalt #[m]
self.wpxtoalt[i] = xtoalt #[m]
def getnextqdr(self):
# get qdr for next leg
if -1 < self.iactwp < self.nwp - 1:
nextqdr, dist = geo.qdrdist(\
self.wplat[self.iactwp], self.wplon[self.iactwp],\
self.wplat[self.iactwp+1],self.wplon[self.iactwp+1])
else:
nextqdr = -999.
return nextqdr
def direct(self, idx, wpnam):
"""Set active point to a waypoint by name"""
name = wpnam.upper().strip()
if name != "" and self.wpname.count(name) > 0:
wpidx = self.wpname.index(name)
self.iactwp = wpidx
bs.traf.actwp.lat[idx] = self.wplat[wpidx]
bs.traf.actwp.lon[idx] = self.wplon[wpidx]
bs.traf.actwp.flyby[idx] = self.wpflyby[wpidx]
self.calcfp()
bs.traf.ap.ComputeVNAV(idx, self.wptoalt[wpidx], self.wpxtoalt[wpidx])
# If there is a speed specified, process it
if self.wpspd[wpidx]>0.:
# Set target speed for autopilot
if self.wpalt[wpidx] < 0.0:
alt = bs.traf.alt[idx]
else:
alt = self.wpalt[wpidx]
# Check for valid Mach or CAS
if self.wpspd[wpidx] <2.0:
cas = mach2cas(self.wpspd[wpidx], alt)
else:
cas = self.wpspd[wpidx]
# Save it for next leg
bs.traf.actwp.spd[idx] = cas
# When already in VNAV: fly it
if bs.traf.swvnav[idx]:
bs.traf.selspd[idx]=cas
# No speed specified for next leg
else:
bs.traf.actwp.spd[idx] = -999.
qdr, dist = geo.qdrdist(bs.traf.lat[idx], bs.traf.lon[idx],
bs.traf.actwp.lat[idx], bs.traf.actwp.lon[idx])
turnrad = bs.traf.tas[idx]*bs.traf.tas[idx]/tan(radians(25.)) / g0 / nm # [nm]default bank angle 25 deg
bs.traf.actwp.turndist[idx] = (bs.traf.actwp.flyby[idx] > 0.5) * \
turnrad*abs(tan(0.5*radians(max(5., abs(degto180(qdr -
self.wpdirfrom[self.iactwp])))))) # [nm]
bs.traf.swlnav[idx] = True
return True
else:
return False, "Waypoint " + wpnam + " not found"
def test_addwpt(start):
global starttime, timer
timelimit = 120
if start:
# Create a waypoint which does not lie on the aircraft path
stack.stack("CRE KLM10 B747 52 4 000 FL99 150")
stack.stack("KLM10 ADDWPT 52 4.1")
starttime = timer
else:
# When the aircraft reaches the waypoint, return success
_, d = geo.qdrdist(52,4.1,traf.lat[0],traf.lon[0])
closeenough = d < 0.2 # d is measured in nm
if timer-starttime>timelimit or closeenough:
return closeenough
def test_wind(start):
global starttime, timer
timelimit = 20
if start:
# Create a very strong headwind
cas = 250
tas = vcas2tas(cas*kts,10000*ft)/kts
stack.stack("CRE KLM10 B747 52 4 000 FL100 "+str(int(cas)))
# Mind that wind is defined in the direction that it is coming from
stack.stack("WIND 52 4 FL100 000 "+str(int(tas)))
starttime = timer
elif timer-starttime>timelimit:
# If the aircraft did not move
_, d = geo.qdrdist(52,4,traf.lat[0],traf.lon[0])
return d < 0.1 # d is measured in nm
def test_after(start):
global starttime, timer
timelimit = 240
lowertimelimit = 120
if start:
# Create a waypoint which does not lie on the aircraft path
# And create a next waypoint to reach after that
stack.stack("CRE KLM10 B747 52 4 000 FL99 150")
stack.stack("DEFWPT TESTWP 52 4.1")
stack.stack("KLM10 ADDWPT TESTWP")
stack.stack("KLM10 AFTER TESTWP ADDWPT 51.9 4.1")
starttime = timer
else:
# When the aircraft reaches the waypoint, return success
_, d = geo.qdrdist(51.9,4.1,traf.lat[0],traf.lon[0])
closeenough = d < 0.2 # d is measured in nm
if timer-starttime>timelimit or closeenough:
return closeenough and timer-starttime>lowertimelimit
def test_before(start):
global starttime, timer
timelimit = 240
if start:
# Create a waypoint which does not lie on the aircraft path
# And create a next waypoint to reach after that
stack.stack("CRE KLM10 B747 52 4 000 FL99 150")
stack.stack("DEFWPT WP1 52.0 4.1")
stack.stack("DEFWPT WP2 52.1 4.2")
stack.stack("DEFWPT WP3 52.0 4.3")
stack.stack("KLM10 ADDWPT WP1")
stack.stack("KLM10 AFTER WP1 ADDWPT WP3")
stack.stack("KLM10 BEFORE WP3 ADDWPT WP2")
starttime = timer
else:
# When the aircraft reaches waypoint 3 within the time limit return success
_, d = geo.qdrdist(52.1 ,4.2 ,traf.lat[0],traf.lon[0])
closeenough = d < 4 # d is measured in nm
if timer-starttime>timelimit or closeenough:
return closeenough
def minTLOS(asas, traf, i, i_other, x1, y1, x, y):
""" This function calculates the aggregated TLOS for all resolution points """
# Get speeds of other AC in range
x_other = traf.gseast[i_other]
y_other = traf.gsnorth[i_other]
# Get relative bearing [deg] and distance [nm]
qdr, dist = geo.qdrdist(traf.lat[i], traf.lon[i], traf.lat[i_other], traf.lon[i_other])
# Convert to SI
qdr = np.deg2rad(qdr)
dist *= nm
# For vectorization, store lengths as W and L
W = np.shape(x)[0]
L = np.shape(x_other)[0]
# Relative speed-components
du = np.dot(x_other.reshape((L,1)),np.ones((1,W))) - np.dot(np.ones((L,1)),x.reshape((1,W)))
dv = np.dot(y_other.reshape((L,1)),np.ones((1,W))) - np.dot(np.ones((L,1)),y.reshape((1,W)))
# Relative speed + zero check
vrel2 = du * du + dv * dv
vrel2 = np.where(np.abs(vrel2) < 1e-6, 1e-6, vrel2) # limit lower absolute value
# X and Y distance
dx = np.dot(np.reshape(dist*np.sin(qdr),(L,1)),np.ones((1,W)))
dy = np.dot(np.reshape(dist*np.cos(qdr),(L,1)),np.ones((1,W)))
# Time to CPA
tcpa = -(du * dx + dv * dy) / vrel2
# CPA distance
dcpa2 = np.square(np.dot(dist.reshape((L,1)),np.ones((1,W)))) - np.square(tcpa) * vrel2
# Calculate time to LOS
R2 = asas.R * asas.R
swhorconf = dcpa2 < R2
dxinhor = np.sqrt(np.maximum(0,R2-dcpa2))
dtinhor = dxinhor / np.sqrt(vrel2)
tinhor = np.where(swhorconf, tcpa-dtinhor, 0.)
tinhor = np.where(tinhor > 0, tinhor, 1e6)
# Get index of best solution
idx = np.argmax(np.sum(tinhor,0))
return idx
def update(self):
# FMS LNAV mode:
# qdr[deg],distinnm[nm]
qdr, distinnm = geo.qdrdist(bs.traf.lat, bs.traf.lon,
bs.traf.actwp.lat, bs.traf.actwp.lon) # [deg][nm])
self.qdr2wp = qdr
self.dist2wp = distinnm*nm # Conversion to meters
# FMS route update and possibly waypoint shift. Note: qdr, dist2wp will be updated accordingly in case of wp switch
self.update_fms(qdr, self.dist2wp) # Updates self.qdr2wp when necessary
#================= Continuous FMS guidance ========================
# Waypoint switching in the loop above was scalar (per a/c with index i)
# Code below is vectorized, with arrays for all aircraft
# Do VNAV start of descent check
#dy = (bs.traf.actwp.lat - bs.traf.lat) #[deg lat = 60 nm]
#dx = (bs.traf.actwp.lon - bs.traf.lon) * bs.traf.coslat #[corrected deg lon = 60 nm]
#self.dist2wp = 60. * nm * np.sqrt(dx * dx + dy * dy) # [m]
# VNAV logic: descend as late as possible, climb as soon as possible
startdescent = (self.dist2wp < self.dist2vs) + (bs.traf.actwp.nextaltco > bs.traf.alt)
# If not lnav:Climb/descend if doing so before lnav/vnav was switched off
# (because there are no more waypoints). This is needed
# to continue descending when you get into a conflict
# while descending to the destination (the last waypoint)
# Use 0.1 nm (185.2 m) circle in case turndist might be zero
self.swvnavvs = bs.traf.swvnav * np.where(bs.traf.swlnav, startdescent,
self.dist2wp <= np.maximum(185.2,bs.traf.actwp.turndist))
#Recalculate V/S based on current altitude and distance to next alt constraint
# How much time do we have before we need to descend?
t2go2alt = np.maximum(0.,(self.dist2wp + bs.traf.actwp.xtoalt - bs.traf.actwp.turndist)) \
/ np.maximum(0.5,bs.traf.gs)
# use steepness to calculate V/S unless we need to descend faster
bs.traf.actwp.vs = np.maximum(self.steepness*bs.traf.gs, \
np.abs((bs.traf.actwp.nextaltco-bs.traf.alt)) \
/np.maximum(1.0,t2go2alt))
self.vnavvs = np.where(self.swvnavvs, bs.traf.actwp.vs, self.vnavvs)
#was: self.vnavvs = np.where(self.swvnavvs, self.steepness * bs.traf.gs, self.vnavvs)
# self.vs = np.where(self.swvnavvs, self.vnavvs, self.vsdef * bs.traf.limvs_flag)
selvs = np.where(abs(bs.traf.selvs) > 0.1, bs.traf.selvs, self.vsdef) # m/s
self.vs = np.where(self.swvnavvs, self.vnavvs, selvs)
self.alt = np.where(self.swvnavvs, bs.traf.actwp.nextaltco, bs.traf.selalt)
# When descending or climbing in VNAV also update altitude command of select/hold mode
bs.traf.selalt = np.where(self.swvnavvs,bs.traf.actwp.nextaltco,bs.traf.selalt)
# LNAV commanded track angle
self.trk = np.where(bs.traf.swlnav, self.qdr2wp, self.trk)
# FMS speed guidance: anticipate accel/decel distance for next leg or turn
# Calculate actual distance it takes to decelerate/accelerate based on two cases: turning speed (decel)
# Normally next leg speed (actwp.spd) but in case we fly turns with a specified turn speed
# use the turn speed
# Is turn speed specified and are we not already slow enough? We only decelerate for turns, not accel.
turntas = np.where(bs.traf.actwp.turnspd>0.0, vcas2tas(bs.traf.actwp.turnspd, bs.traf.alt),
-1.0+0.*bs.traf.tas)
swturnspd = bs.traf.actwp.flyturn*(turntas>0.0)*(bs.traf.actwp.turnspd>0.0)
turntasdiff = np.maximum(0.,(bs.traf.tas - turntas)*(turntas>0.0))
# t = (v1-v0)/a ; x = v0*t+1/2*a*t*t => dx = (v1*v1-v0*v0)/ (2a)
dxturnspdchg = distaccel(turntas,bs.traf.tas, bs.traf.perf.axmax)
# dxturnspdchg = 0.5*np.abs(turntas*turntas-bs.traf.tas*bs.traf.tas)/(np.sign(turntas-bs.traf.tas)*np.maximum(0.01,np.abs(ax)))
# dxturnspdchg = np.where(swturnspd, np.abs(turntasdiff)/np.maximum(0.01,ax)*(bs.traf.tas+0.5*np.abs(turntasdiff)),
# 0.0*bs.traf.tas)
# Decelerate or accelerate for next required speed because of speed constraint or RTA speed
# Note that because nextspd comes from the stack, and can be either a mach number or
# a calibrated airspeed, it can only be converted from Mach / CAS [kts] to TAS [m/s]
# once the altitude is known.
nexttas = vcasormach2tas(bs.traf.actwp.nextspd, bs.traf.alt)
# tasdiff = (nexttas - bs.traf.tas)*(bs.traf.actwp.spd>=0.) # [m/s]
# t = (v1-v0)/a ; x = v0*t+1/2*a*t*t => dx = (v1*v1-v0*v0)/ (2a)
dxspdconchg = distaccel(bs.traf.tas, nexttas, bs.traf.perf.axmax)
# Check also whether VNAVSPD is on, if not, SPD SEL has override for next leg
# and same for turn logic
usenextspdcon = (self.dist2wp < dxspdconchg)*(bs.traf.actwp.nextspd>-990.) * \
bs.traf.swvnavspd*bs.traf.swvnav*bs.traf.swlnav
useturnspd = np.logical_or(bs.traf.actwp.turntonextwp,\
(self.dist2wp < dxturnspdchg+bs.traf.actwp.turndist) * \
swturnspd*bs.traf.swvnavspd*bs.traf.swvnav*bs.traf.swlnav)
# Hold turn mode can only be switched on here, cannot be switched off here (happeps upon passing wp)
bs.traf.actwp.turntonextwp = np.logical_or(bs.traf.actwp.turntonextwp,useturnspd)
# Which CAS/Mach do we have to keep? VNAV, last turn or next turn?
oncurrentleg = (abs(degto180(bs.traf.trk - qdr)) < 2.0) # [deg]
inoldturn = (bs.traf.actwp.oldturnspd > 0.) * np.logical_not(oncurrentleg)
# Avoid using old turning speeds when turning of this leg to the next leg
# by disabling (old) turningspd when on leg
bs.traf.actwp.oldturnspd = np.where(oncurrentleg*(bs.traf.actwp.oldturnspd>0.), -998.,
bs.traf.actwp.oldturnspd)
# turnfromlastwp can only be switched off here, not on (latter happens upon passing wp)
bs.traf.actwp.turnfromlastwp = np.logical_and(bs.traf.actwp.turnfromlastwp,inoldturn)
# Select speed: turn sped, next speed constraint, or current speed constraint
bs.traf.selspd = np.where(useturnspd,bs.traf.actwp.turnspd,
np.where(usenextspdcon, bs.traf.actwp.nextspd,
np.where((bs.traf.actwp.spdcon>=0)*bs.traf.swvnavspd,bs.traf.actwp.spd,
bs.traf.selspd)))
# Temporary override when still in old turn
bs.traf.selspd = np.where(inoldturn*(bs.traf.actwp.oldturnspd>0.)*bs.traf.swvnavspd*bs.traf.swvnav*bs.traf.swlnav,
bs.traf.actwp.oldturnspd,bs.traf.selspd)
#debug if inoldturn[0]:
#debug print("inoldturn bs.traf.trk =",bs.traf.trk[0],"qdr =",qdr)
#debug elif usenextspdcon[0]:
#debug print("usenextspdcon")
#debug elif useturnspd[0]:
#debug print("useturnspd")
#debug elif bs.traf.actwp.spdcon>0:
#debug print("using current speed constraint")
#debug else:
#debug print("no speed given")
# Below crossover altitude: CAS=const, above crossover altitude: Mach = const
self.tas = vcasormach2tas(bs.traf.selspd, bs.traf.alt)
def update(self):
''' Periodic update function for metrics calculation. '''
self.sectorsd = np.zeros(len(self.sectors))
self.sectorconv = np.zeros(len(self.sectors))
self.sectoreff = []
if not traf.ntraf or not self.sectors:
return
# Check convergence using CD with large RPZ and tlook
confpairs, lospairs, inconf, tcpamax, qdr, dist, dcpa, tcpa, tLOS = \
traf.cd.detect(traf, traf, np.ones(traf.ntraf) * 20 * nm, traf.cd.hpz, np.ones(traf.ntraf) * 3600)
if confpairs:
own, intr = zip(*confpairs)
ownidx = traf.id2idx(own)
mask = traf.alt[ownidx] > 70 * ft
ownidx = np.array(ownidx)[mask]
dcpa = np.array(dcpa)[mask]
tcpa = np.array(tcpa)[mask]
else:
ownidx = np.array([])
sendeff = False
for idx, (sector,
previnside) in enumerate(zip(self.sectors, self.acinside)):
inside = areafilter.checkInside(sector, traf.lat, traf.lon,
traf.alt)
sectoreff = []
# Detect aircraft leaving and entering the sector
previds = set(previnside.acid)
ids = set(np.array(traf.id)[inside])
arrived = list(ids - previds)
left = previds - ids
# Split aircraft that left the sector in deleted and not deleted
left_intraf = left.intersection(traf.id)
left_del = list(
left - left_intraf) # Aircraft id's prev inside but deleted
left_intraf = list(left_intraf) # Aircraft id's that left sector
# New a/c in sector arr listed by index in arridx
arridx = traf.id2idx(arrived)
leftidx = traf.id2idx(left_intraf)
# Retrieve the current distance flown for arriving and leaving aircraft
arrdist = traf.distflown[arridx]
arrlat = traf.lat[arridx]
arrlon = traf.lon[arridx]
# Get all a/c ids that left from the set delac
leftlat, leftlon, leftdist = self.delac.get(left_del)
leftlat = np.append(leftlat, traf.lat[leftidx])
leftlon = np.append(leftlon, traf.lon[leftidx])
leftdist = np.append(leftdist, traf.distflown[leftidx])
leftlat0, leftlon0, leftdist0 = previnside.get(left_del +
left_intraf)
self.delac.delete(left_del)
if len(left) > 0:
# Exclude aircraft where origin = destination for sector efficiency,
# so require that distance start-end > 10 nm
q, d = geo.qdrdist(leftlat0, leftlon0, leftlat, leftlon)
mask = d > 10
sectoreff = list(
(leftdist[mask] - leftdist0[mask]) / d[mask] / nm)
names = np.array(left_del + left_intraf)[mask]
for name, eff in zip(names, sectoreff):
self.feff.write('{}, {}, {}\n'.format(sim.simt, name, eff))
sendeff = True
# print('{} aircraft left sector {}, distance flown (acid:dist):'.format(len(left), sector))
# for a, d0, d1, e in zip(left, leftdist0, leftdist, sectoreff):
# print('Aircraft {} flew {} meters (eff = {})'.format(a, round(d1-d0), e))
# Update inside data for this sector
previnside.delete(left)
previnside.extend(arrived, arrlat, arrlon, arrdist)
self.sectoreff.append(sectoreff)
self.sectorsd[idx] = np.count_nonzero(inside)
insidx = np.where(np.logical_and(inside, inconf))
pairsinside = np.isin(ownidx, insidx)
if len(pairsinside):
tnorm = np.array(tcpa)[pairsinside] / 300.0
dcpanorm = np.array(dcpa)[pairsinside] / (5.0 * nm)
self.sectorconv[idx] = np.sum(
np.sqrt(2.0 / tnorm * tnorm + dcpanorm * dcpanorm))
else:
self.sectorconv[idx] = 0
self.fconv.write('{}, {}\n'.format(sim.simt, self.sectorconv[idx]))
self.fsd.write('{}, {}\n'.format(sim.simt, self.sectorsd[idx]))
if sendeff:
self.effplot.send()
def newcalcfp(self):
"""Do flight plan calculations"""
# Remove old top of descents and old top of climbs
while self.wpname.count("T/D") > 0:
self.delwpt("T/D")
while self.wpname.count("T/C") > 0:
self.delwpt("T/C")
# Remove old actual position waypoints
while self.wpname.count("A/C") > 0:
self.delwpt("A/C")
# Insert actual position as A/C waypoint
idx = self.iactwp
self.insertcalcwp(idx, "A/C")
self.wplat[idx] = bs.traf.lat[self.iac] # deg
self.wplon[idx] = bs.traf.lon[self.iac] # deg
self.wpalt[idx] = bs.traf.alt[self.iac] # m
self.wpspd[idx] = bs.traf.tas[self.iac] # m/s
# Calculate distance to last waypoint in route
nwp = len(self.wpname)
dist2go = [0.0]
for i in range(nwp - 2, -1, -1):
qdr, dist = geo.qdrdist(self.wplat[i], self.wplon[i],
self.wplat[i + 1], self.wplon[i + 1])
dist2go = [dist2go[0] + dist] + dist2go
# Make VNAV WP list with only waypoints with altitude constraints
# This list we will use to find where to insert t/c and t/d
alt = []
x = []
name = []
for i in range(nwp):
if self.wpalt[i] > -1.:
alt.append(self.wpalt[i])
x.append(dist2go[i])
name.append(self.wpname[i] +
" ") # space for check first 2 chars later
# Find where to insert cruise segment (if any)
# Find longest segment without altitude constraints
desslope = clbslope = 1.
crzalt = bs.traf.crzalt[self.iac]
if crzalt > 0.:
ilong = -1
dxlong = 0.0
nvwp = len(alt)
for i in range(nvwp - 1):
if x[i] - x[i + 1] > dxlong:
ilong = i
dxlong = x[i] - x[i + 1]
# VNAV parameters to insert T/Cs and T/Ds
crzdist = 20. * nm # minimally required distance at cruise level
clbslope = 3000. * ft / (10. * nm) # 1:3 rule for now
desslope = clbslope # 1:3 rule for now
# Can we get a sufficient distance at cruise altitude?
if max(alt[ilong], alt[ilong + 1]) < crzalt:
dxclimb = (crzalt - alt[ilong]) * clbslope
dxdesc = (crzalt - alt[ilong + 1]) * desslope
if x[ilong] - x[ilong + 1] > dxclimb + crzdist + dxdesc:
# Insert T/C (top of climb) at cruise level
name.insert(ilong + 1, "T/C")
alt.insert(ilong + 1, crzalt)
x.insert(ilong + 1, x[ilong] + dxclimb)
# Insert T/D (top of descent) at cruise level
name.insert(ilong + 2, "T/D")
alt.insert(ilong + 2, crzalt)
x.insert(ilong + 2, x[ilong + 1] - dxdesc)
# Compare angles to rates:
epsh = 50. * ft # Nothing to be done for small altitude changes
epsx = 1. * nm # [m] Nothing to be done at this short range
i = 0
while i < len(alt) - 1:
if name[i][:2] == "T/":
continue
dy = alt[i + 1] - alt[i] # alt change (pos = climb)
dx = x[i] - x[i + 1] # distance (positive)
dxdes = abs(dy) / desslope
dxclb = abs(dy) / clbslope
if dy < epsh and dx + epsx > dxdes: # insert T/D?
name.insert(i + 1, "T/D")
alt.insert(i + 1, alt[i])
x.insert(i + 1, x[i + 1] - dxdes)
i += 1
elif dy > epsh and dx + epsx > dxclb: # insert T/C?
name.insert(i + 1, "T/C")
alt.insert(i + 1, alt[i + 1])
x.insert(i + 1, x[i] + dxclb)
i += 2
else:
i += 1
# Now insert T/Cs and T/Ds in actual flight plan
nvwp = len(alt)
for i in range(nvwp, -1, -1):
# Copy all new waypoints (which are all named T/C or T/D)
if name[i][:2] == "T/":
# Find place in flight plan to insert T/C or T/D
j = nvwp - 1
while dist2go[j] < x[i] and j > 1:
j = j - 1
# Interpolation factor for position on leg
f = (x[i] - dist2go[j + 1]) / (dist2go[j] - dist2go[j + 1])
lat = f * self.wplat[j] + (1. - f) * self.wplat[j + 1]
lon = f * self.wplon[j] + (1. - f) * self.wplon[j + 1]
self.wpname.insert(j, name[i])
self.wptype.insert(j, Route.calcwp)
self.wplat.insert(j, lat)
self.wplon.insert(j, lon)
self.wpalt.insert(j, alt[i])
self.wpspd.insert(j, -999.)
def __init__(self,name,cmd,cmdargs):
self.name = name.upper()
self.tupdate = sim.simt-999.
self.flow = 0
self.incircle = True
self.segdir = None # Segment direction in degrees
self.hdg = None # When runway is used as separate airport
# Is location a circle segment?
if swcircle and self.name[:4]=="SEGM":
self.type = "seg"
self.lat,self.lon,brg = getseg(self.name)
pass
else:
success, posobj = txt2pos(name,ctrlat,ctrlon)
if success:
# for runway type, get heading as default optional argument for command line
if posobj.type == "rwy":
aptname, rwyname = name.split('/RW')
rwyname = rwyname.lstrip('Y')
try:
self.hdg = navdb.rwythresholds[aptname][rwyname][2]
except:
self.hdg = None
pass
else:
rwyhdg = None
self.lat,self.lon = posobj.lat, posobj.lon
self.type = posobj.type
# If outside circle change lat,lon to edge of circle
self.incircle = incircle(self.lat,self.lon)
if not self.incircle:
self.type = "seg"
self.segdir,dist = qdrdist(ctrlat,ctrlon,posobj.lat,posobj.lon)
segname = "SEGM"+str(int(round(self.segdir)))
self.lat,self.lon,hdg = getseg(segname)
else:
print("ERROR: trafgen class Source called for "+name+". Position not found")
self.lat,self.lon = 0.0,0.0
# Aircraft types
self.actypes = ["*"]
# Runways
self.runways = [] # name
self.rwylat = []
self.rwylon = []
self.rwyhdg = []
self.rwyline = [] # number of aircraft waiting in line
self.rwytotime = [] # time of last takeoff
self.dtakeoff = 90. # sec take-off interval on one runway, default 90 sec
# Destinations
self.dest = []
self.destlat = []
self.destlon = []
self.desttype = [] # "apt","wpt","seg"
self.desthdg = [] # if dest is a runway (for flights within FIR) or circle segment (source outside FIR)
self.destactypes = [] # Types for this destinations ([]=use defaults for this source)
#Names of drawing objects runways
self.polys = [] # Names of current runway polygons to remove when runways change
# Start values
self.startaltmin = -999. # [ft] minimum starting altitude
self.startaltmax = -999. # [ft] maximum starting altitude
self.startspdmin = -999. # [m/s] minimum starting speed
self.startspdmax = -999. # [m/s] maximum starting speed
self.starthdgmin = -999. # [deg] Valid values -360 - 360 degrees (to also have an interval possible around 360)
self.starthdgmax = -999. # [deg] Valid values -360 - 360 degrees (to also have an interval possible around 360)
return
def calcfp(
self
): # Current Flight Plan calculations, which actualize based on flight condition
"""Do flight plan calculations"""
# self.delwpt("T/D")
# self.delwpt("T/C")
# Direction to waypoint
self.nwp = len(self.wpname)
# Create flight plan calculation table
self.wpdirfrom = self.nwp * [0.]
self.wpdistto = self.nwp * [0.]
self.wpialt = self.nwp * [-1]
self.wptoalt = self.nwp * [-999.]
self.wpxtoalt = self.nwp * [1.] # Avoid division by zero
self.wpirta = self.nwp * [-1]
self.wptorta = self.nwp * [-999.]
self.wpxtorta = self.nwp * [1.] #[m] Avoid division by zero
# No waypoints: make empty variables to be safe and return: nothing to do
if self.nwp == 0:
return
# Calculate lateral leg data
# LNAV: Calculate leg distances and directions
for i in range(0, self.nwp - 1):
qdr, dist = geo.qdrdist(self.wplat[i], self.wplon[i],
self.wplat[i + 1], self.wplon[i + 1])
self.wpdirfrom[i] = qdr
self.wpdistto[i + 1] = dist #[nm] distto is in nautical miles
if self.nwp > 1:
self.wpdirfrom[-1] = self.wpdirfrom[-2]
# Calculate longitudinal leg data
# VNAV: calc next altitude constraint: index, altitude and distance to it
ialt = -1 # index to waypoint with next altitude constraint
toalt = -999. # value of next altitude constraint
xtoalt = 0. # distance to next altitude constraint from this wp
for i in range(self.nwp - 1, -1, -1):
# waypoint with altitude constraint (dest of al specified)
if self.wptype[i] == Route.dest:
ialt = i
toalt = 0.
xtoalt = 0. # [m]
elif self.wpalt[i] >= 0:
ialt = i
toalt = self.wpalt[i]
xtoalt = 0. # [m]
# waypoint with no altitude constraint:keep counting
else:
if i != self.nwp - 1:
xtoalt = xtoalt + self.wpdistto[
i + 1] * nm # [m] xtoalt is in meters!
else:
xtoalt = 0.0
self.wpialt[i] = ialt
self.wptoalt[i] = toalt #[m]
self.wpxtoalt[i] = xtoalt #[m]
# RTA: calc next rta constraint: index, altitude and distance to it
# If any RTA.
if any(array(self.wprta) >= 0.0):
#print("Yes, I found RTAs")
irta = -1 # index of wp
torta = -999. # next rta value
xtorta = 0. # distance to next rta
for i in range(self.nwp - 1, -1, -1):
# waypoint with rta: reset counter, update rts
if self.wprta[i] >= 0:
irta = i
torta = self.wprta[i]
xtorta = 0. # [m]
# waypoint with no altitude constraint:keep counting
else:
if i != self.nwp - 1:
# No speed or rta constraint: add to xtorta
if self.wpspd[i] <= 0.0:
xtorta = xtorta + self.wpdistto[
i + 1] * nm # [m] xtoalt is in meters!
else:
# speed constraint on this leg: shift torta to account for this
# altitude unknown
if self.wptoalt[i] > 0.:
alt = toalt
else:
# TODO: current a/c altitude would be better guess, but not accessible here
# as we do not know aircraft index for this route
alt = 10000. * ft # default to minimize errors, when no alt constraints are present
legtas = casormach2tas(self.wpspd[i], alt)
#TODO: account for wind at this position vy adding wind vectors to waypoints?
# xtorta stays the same! This leg will not be available for RTA scheduling, so distance
# is not in xtorta. Therefore we need to subtract legtime to ignore this leg for the RTA
# scheduling
legtime = self.wpdistto[i + 1] / legtas
torta = torta - legtime
else:
xtorta = 0.0
torta = -999.0
self.wpirta[i] = irta
self.wptorta[i] = torta # [s]
self.wpxtorta[i] = xtorta # [m]
def update_fms(self, qdr, dist, dt=bs.settings.fms_dt):
# 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, \
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]:
# Valid speed and LNAV and VNAV ap modes are on
# Depending on crossover altitude we fix CAS or Mach
if bs.traf.abco[i] and spd > 1.0:
bs.traf.actwp.spd[i] = cas2mach(spd, bs.traf.alt[i])
elif bs.traf.belco[i] and 0. < spd <= 1.0:
bs.traf.actwp.spd[i] = mach2cas(spd, bs.traf.alt[i])
else:
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 wokrs, 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
self.ComputeVNAV(i, toalt, bs.traf.actwp.xtoalt[i])
def update(self):
if self.ncond==0:
return
# Update indices based on list of id's
acidxlst = np.array(bs.traf.id2idx(self.id))
if len(acidxlst)>0:
idelcond = sorted(list(np.where(acidxlst<0)[0]))
for i in idelcond[::-1]:
del (self.id[i])
self.condtype = np.delete(self.condtype, i)
self.target = np.delete(self.target, i)
self.lastdif = np.delete(self.lastdif, i)
del self.posdata[i]
del self.cmd[i]
self.ncond = len(self.id)
if self.ncond==0:
return
acidxlst = np.array(bs.traf.id2idx(self.id))
# Check condition types
actdist = np.ones(self.ncond)*999e9 # Invalid number which never triggers anything is extremely large
for j in range(self.ncond):
if self.condtype[j] == postype:
qdr,dist = qdrdist(bs.traf.lat[acidxlst[j]],bs.traf.lon[acidxlst[j]],self.posdata[j][0],self.posdata[j][1])
actdist[j] = dist # [nm]
# Get relevant actual value using index list as index to numpy arrays
self.actual = (self.condtype == alttype) * bs.traf.alt[acidxlst] + \
(self.condtype == spdtype) * bs.traf.cas[acidxlst] + \
(self.condtype == postype) * actdist
# Compare sign of actual difference with sign of last difference
actdif = self.target - self.actual
# Make sorted arrya of indices of true conditions and their conditional commands
idxtrue = sorted(list(np.where(actdif*self.lastdif <= 0.0)[0]))# Sign changed
self.lastdif = actdif
if idxtrue==None or len(idxtrue)==0:
return
# Execute commands found to have true condition
for i in idxtrue:
if i>=0:
stack.stack(self.cmd[i])
# debug
# stack.stack(" ECHO Conditional command issued: "+self.cmd[i])
# Delete executed commands to clean up arrays and lists
# from highest index to lowest for consistency
for i in idxtrue[::-1]:
if i>=0:
del self.id[i]
self.condtype = np.delete(self.condtype,i)
self.target = np.delete(self.target,i)
self.lastdif = np.delete(self.lastdif,i)
del self.posdata[i]
del self.cmd[i]
# Adjust number of conditions
self.ncond = len(self.id)
if self.ncond!=len(self.cmd):
print ("self.ncond=",self.ncond)
print ("self.cmd=",self.cmd)
print ("traffic/conditional.py: self.delcondition: invalid condition array size")
return
def calcfp(self):
"""Do flight plan calculations"""
# self.delwpt("T/D")
# self.delwpt("T/C")
# Direction to waypoint
self.nwp = len(self.wpname)
# Create flight plan calculation table
self.wpdirfrom = self.nwp * [0.]
self.wpdistto = self.nwp * [0.]
self.wpialt = self.nwp * [-1]
self.wptoalt = self.nwp * [-999.]
self.wpxtoalt = self.nwp * [1.]
# No waypoints: make empty variables to be safe and return: nothing to do
if self.nwp == 0:
return
# Calculate lateral leg data
# LNAV: Calculate leg distances and directions
for i in range(0, self.nwp - 1):
qdr,dist = geo.qdrdist(self.wplat[i] ,self.wplon[i], \
self.wplat[i+1],self.wplon[i+1])
self.wpdirfrom[i] = qdr
self.wpdistto[i + 1] = dist #[nm] distto is in nautical miles
if self.nwp > 1:
self.wpdirfrom[-1] = self.wpdirfrom[-2]
# Calclate longitudinal leg data
# VNAV: calc next altitude constraint: index, altitude and distance to it
ialt = -1
toalt = -999.
xtoalt = 0.
for i in range(self.nwp - 1, -1, -1):
# waypoint with altitude constraint (dest of al specified)
if self.wptype[i] == self.dest:
ialt = i
toalt = 0.
xtoalt = 0. # [m]
elif self.wpalt[i] >= 0:
ialt = i
toalt = self.wpalt[i]
xtoalt = 0. # [m]
# waypoint with no altitude constraint:keep counting
else:
if i != self.nwp - 1:
xtoalt = xtoalt + self.wpdistto[
i + 1] * nm # [m] xtoalt is in meters!
else:
xtoalt = 0.0
self.wpialt[i] = ialt
self.wptoalt[i] = toalt #[m]
self.wpxtoalt[i] = xtoalt #[m]
return
def update(self):
# FMS LNAV mode:
# qdr[deg],distinnm[nm]
qdr, distinnm = geo.qdrdist(bs.traf.lat, bs.traf.lon,
bs.traf.actwp.lat,
bs.traf.actwp.lon) # [deg][nm])
dist = distinnm * nm # Conversion to meters
# FMS route update
self.update_fms(qdr, dist)
#================= Continuous FMS guidance ========================
# Waypoint switching in the loop above was scalar (per a/c with index i)
# Code below is vectorized, with arrays for all aircraft
# Do VNAV start of descent check
dy = (bs.traf.actwp.lat - bs.traf.lat) #[deg lat = 60 nm]
dx = (bs.traf.actwp.lon -
bs.traf.lon) * bs.traf.coslat #[corrected deg lon = 60 nm]
dist2wp = 60. * nm * np.sqrt(dx * dx + dy * dy) # [m]
#print("dist2wp =",dist2wp," self.dist2vs =",self.dist2vs)
# VNAV logic: descend as late as possible, climb as soon as possible
startdescent = (dist2wp < self.dist2vs) + (bs.traf.actwp.nextaltco >
bs.traf.alt)
# If not lnav:Climb/descend if doing so before lnav/vnav was switched off
# (because there are no more waypoints). This is needed
# to continue descending when you get into a conflict
# while descending to the destination (the last waypoint)
# Use 0.1 nm (185.2 m) circle in case turndist might be zero
self.swvnavvs = bs.traf.swvnav * np.where(
bs.traf.swlnav, startdescent,
dist <= np.maximum(185.2, bs.traf.actwp.turndist))
#Recalculate V/S based on current altitude and distance to next alt constraint
# How much time do we have before we need to descend?
t2go2alt = np.maximum(0.,(dist2wp + bs.traf.actwp.xtoalt - bs.traf.actwp.turndist)) \
/ np.maximum(0.5,bs.traf.gs)
# use steepness to calculate V/S unless we need to descend faster
bs.traf.actwp.vs = np.maximum(self.steepness*bs.traf.gs, \
np.abs((bs.traf.actwp.nextaltco-bs.traf.alt)) \
/np.maximum(1.0,t2go2alt))
self.vnavvs = np.where(self.swvnavvs, bs.traf.actwp.vs, self.vnavvs)
#was: self.vnavvs = np.where(self.swvnavvs, self.steepness * bs.traf.gs, self.vnavvs)
# self.vs = np.where(self.swvnavvs, self.vnavvs, bs.traf.apvsdef * bs.traf.limvs_flag)
selvs = np.where(
abs(bs.traf.selvs) > 0.1, bs.traf.selvs, bs.traf.apvsdef) # m/s
self.vs = np.where(self.swvnavvs, self.vnavvs, selvs)
self.alt = np.where(self.swvnavvs, bs.traf.actwp.nextaltco,
bs.traf.selalt)
# When descending or climbing in VNAV also update altitude command of select/hold mode
bs.traf.selalt = np.where(self.swvnavvs, bs.traf.actwp.nextaltco,
bs.traf.selalt)
# LNAV commanded track angle
self.trk = np.where(bs.traf.swlnav, qdr, self.trk)
# FMS speed guidance: anticipate accel distance
# Actual distance it takes to decelerate
nexttas = vcasormach2tas(bs.traf.actwp.spd, bs.traf.alt)
tasdiff = nexttas - bs.traf.tas # [m/s]
dtspdchg = np.abs(tasdiff) / np.maximum(0.01, np.abs(bs.traf.ax)) #[s]
dxspdchg = 0.5 * np.sign(tasdiff) * np.abs(
bs.traf.ax) * dtspdchg * dtspdchg + bs.traf.tas * dtspdchg #[m]
# Chekc also whether VNAVSPD is on, if not, SPD SEL has override
usespdcon = (dist2wp < dxspdchg) * (
bs.traf.actwp.spd > -990.) * bs.traf.swvnavspd * bs.traf.swvnav
bs.traf.selspd = np.where(usespdcon, bs.traf.actwp.spd, bs.traf.selspd)
# Below crossover altitude: CAS=const, above crossover altitude: Mach = const
self.tas = vcasormach2tas(bs.traf.selspd, bs.traf.alt)
def update(self,gain):
# Time step update of drain
# 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:
# Calculate starting position using origin
if len(self.orig)>0:
# Add origin
iorig = int(random.random() * len(self.orig))
else:
iorig = -1
if iorig>=0:
incirc = self.origincirc[iorig]
lat,lon = self.origlat[iorig],self.origlon[iorig]
hdg,dist = qdrdist(lat,lon,self.lat,self.lon)
else:
print("Warning update drain",self.name,"called with no origins present!")
hdg = random.random()*360.
print("using random segment",int(hdg+180)%360)
incirc = False
if not incirc:
lat,lon = kwikpos(ctrlat,ctrlon,(hdg+180)%360,radius)
elif self.origtype=="seg":
lat,lon,brg = getseg(self.name)
hdg = (brg+180)%360
else:
hdg = random.random()*360.
if incirc and (self.origtype[iorig]=="apt" or self.origtype[iorig]=="rwy"):
alttxt,spdtxt = str(0),str(0)
else:
alttxt,spdtxt = "FL"+str(random.randint(200,300)), str(random.randint(250,350))
if iorig>=0:
acid = randacname(self.orig[iorig], self.name)
else:
acid = randacname("LFPG", self.name)
if len(self.origactypes)>0:
actype = random.choice(self.origactypes[iorig])
else:
actype = random.choice(self.actypes)
stack.stack("CRE " + ",".join([acid,actype,str(lat), str(lon),
str(int(hdg%360)),alttxt,spdtxt]))
if iorig>=0:
if self.orig[iorig][:4]!="SEGM":
stack.stack(acid + " ORIG " + self.orig[iorig])
else:
stack.stack(acid + " ORIG " + str(self.origlat[iorig]) + " " +\
str(self.origlat[iorig]))
if not (self.name[:4]=="SEGM"):
stack.stack(acid + " DEST " + self.name)
else:
stack.stack(acid + " ADDWPT " + str(self.lat) + " " + str(self.lon))
if alttxt=="0" and spdtxt =="0":
stack.stack(acid+" SPD 250")
stack.stack(acid+" ALT FL100")
#stack.stack(acid+" LNAV ON")
else:
stack.stack(acid + " LNAV ON")
def __init__(self,name,cmd,cmdargs):
self.name = name.upper()
self.tupdate = sim.simt-999.
self.flow = 0
self.incircle = True
self.segdir = None # Segment direction in degrees
# Is location a circle segment?
if self.name[:4]=="SEGM":
self.type = "seg"
self.lat,self.lon,brg = getseg(self.name) # For SEGMnnn to SEGMnnn for crossing flights optional
pass
else:
success, posobj = txt2pos(name,ctrlat,ctrlon)
if success:
# for runway type, get heading as default optional argument for command line
if posobj.type == "rwy":
aptname, rwyname = name.split('/RW')
rwyname = rwyname.lstrip('Y')
try:
rwyhdg = navdb.rwythresholds[aptname][rwyname][2]
except:
rwyhdg = None
pass
else:
rwyhdg = None
self.lat,self.lon = posobj.lat, posobj.lon
self.type = posobj.type
# If outside circle change lat,lon to edge of circle
self.incircle = incircle(self.lat,self.lon)
if not self.incircle:
self.type = "seg"
self.segdir,dist = qdrdist(ctrlat,ctrlon,posobj.lat,posobj.lon)
segname = "SEGM"+str(int(round(self.segdir)))
self.lat,self.lon,hdg = getseg(segname)
else:
print("ERROR: trafgen class Drain called for "+name+". Position not found")
self.lat,self.lon = 0.0,0.0
# Aircraft types
self.actypes = ["*"]
# Runways
self.runways = [] # name
self.rwylat = []
self.rwylon = []
self.rwyhdg = []
self.rwyline = [] # number of aircraft waiting in line
#Origins
self.orig = []
self.origlat = []
self.origlon = []
self.origtype = [] # "apt","wpt","seg"
self.orighdg = [] # if orig is a runway (for flights within FIR) or circle segment (drain outside FIR)
self.origactypes = [] # Types for this originations ([]=use defaults for this drain)
self.origincirc = []
#Names of drawing objects runways
self.polys = []
return
def update_fms(self, qdr, dist):
# Check which aircraft i have reached their active waypoint
# Shift waypoints for aircraft i where necessary
# Reached function return list of indices where reached logic is True
for i in bs.traf.actwp.Reached(qdr, dist, bs.traf.actwp.flyby,
bs.traf.actwp.flyturn,bs.traf.actwp.turnrad,bs.traf.actwp.swlastwp):
# Save current wp speed for use on next leg when we pass this waypoint
# VNAV speeds are always FROM-speeds, 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]
# Execute stack commands for the still active waypoint, which we pass
self.route[i].runactwpstack()
# If specified, use the given turn radius of passing wp for bank angle
if bs.traf.actwp.flyturn[i]:
if bs.traf.actwp.turnspd[i]>=0.:
turnspd = bs.traf.actwp.turnspd[i]
else:
turnspd = bs.traf.tas[i]
if bs.traf.actwp.turnrad[i] > 0.:
self.turnphi[i] = atan(turnspd*turnspd/(bs.traf.actwp.turnrad[i]*nm*g0)) # [rad]
else:
self.turnphi[i] = 0.0 # [rad] or leave untouched???
else:
self.turnphi[i] = 0.0 #[rad] or leave untouched???
# Get next wp, if there still is one
if not bs.traf.actwp.swlastwp[i]:
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], bs.traf.actwp.swlastwp[i] = \
self.route[i].getnextwp() # note: xtoalt,toalt in [m]
# Prevent trying to activate the next waypoint when it was already the last waypoint
else:
bs.traf.swlnav[i] = False
bs.traf.swvnav[i] = False
bs.traf.swvnavspd[i] = False
continue # Go to next a/c which reached its active waypoint
# 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] = -997.
# 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
self.dist2wp[i] = distnmi
bs.traf.actwp.curlegdir[i] = qdr[i]
bs.traf.actwp.curleglen[i] = distnmi
# 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], self.bankdef[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] and bs.traf.actwp.spd[iac]>=0.0:
bs.traf.selspd[iac] = bs.traf.actwp.spd[iac]
def update(self):
# FMS LNAV mode:
# qdr[deg],distinnm[nm]
qdr, distinnm = geo.qdrdist(bs.traf.lat, bs.traf.lon,
bs.traf.actwp.lat, bs.traf.actwp.lon) # [deg][nm])
dist = distinnm*nm # Conversion to meters
# FMS route update
self.update_fms(qdr, dist)
#================= Continuous FMS guidance ========================
# Waypoint switching in the loop above was scalar (per a/c with index i)
# Code below is vectorized, with arrays for all aircraft
# Do VNAV start of descent check
dy = (bs.traf.actwp.lat - bs.traf.lat) #[deg lat = 60 nm]
dx = (bs.traf.actwp.lon - bs.traf.lon) * bs.traf.coslat #[corrected deg lon = 60 nm]
dist2wp = 60. * nm * np.sqrt(dx * dx + dy * dy) # [m]
#print("dist2wp =",dist2wp," self.dist2vs =",self.dist2vs)
# VNAV logic: descend as late as possible, climb as soon as possible
startdescent = (dist2wp < self.dist2vs) + (bs.traf.actwp.nextaltco > bs.traf.alt)
# If not lnav:Climb/descend if doing so before lnav/vnav was switched off
# (because there are no more waypoints). This is needed
# to continue descending when you get into a conflict
# while descending to the destination (the last waypoint)
# Use 0.1 nm (185.2 m) circle in case turndist might be zero
self.swvnavvs = bs.traf.swvnav * np.where(bs.traf.swlnav, startdescent,
dist <= np.maximum(185.2,bs.traf.actwp.turndist))
#Recalculate V/S based on current altitude and distance to next alt constraint
# How much time do we have before we need to descend?
t2go2alt = np.maximum(0.,(dist2wp + bs.traf.actwp.xtoalt - bs.traf.actwp.turndist)) \
/ np.maximum(0.5,bs.traf.gs)
# use steepness to calculate V/S unless we need to descend faster
bs.traf.actwp.vs = np.maximum(self.steepness*bs.traf.gs, \
np.abs((bs.traf.actwp.nextaltco-bs.traf.alt)) \
/np.maximum(1.0,t2go2alt))
self.vnavvs = np.where(self.swvnavvs, bs.traf.actwp.vs, self.vnavvs)
#was: self.vnavvs = np.where(self.swvnavvs, self.steepness * bs.traf.gs, self.vnavvs)
# self.vs = np.where(self.swvnavvs, self.vnavvs, bs.traf.apvsdef * bs.traf.limvs_flag)
selvs = np.where(abs(bs.traf.selvs) > 0.1, bs.traf.selvs, bs.traf.apvsdef) # m/s
self.vs = np.where(self.swvnavvs, self.vnavvs, selvs)
self.alt = np.where(self.swvnavvs, bs.traf.actwp.nextaltco, bs.traf.selalt)
# When descending or climbing in VNAV also update altitude command of select/hold mode
bs.traf.selalt = np.where(self.swvnavvs,bs.traf.actwp.nextaltco,bs.traf.selalt)
# LNAV commanded track angle
self.trk = np.where(bs.traf.swlnav, qdr, self.trk)
# FMS speed guidance: anticipate accel distance
# Actual distance it takes to decelerate
nexttas = vcasormach2tas(bs.traf.actwp.spd,bs.traf.alt)
tasdiff = nexttas - bs.traf.tas # [m/s]
dtspdchg = np.abs(tasdiff)/np.maximum(0.01,np.abs(bs.traf.ax)) #[s]
dxspdchg = 0.5*np.sign(tasdiff)*np.abs(bs.traf.ax)*dtspdchg*dtspdchg + bs.traf.tas*dtspdchg #[m]
# Chekc also whether VNAVSPD is on, if not, SPD SEL has override
usespdcon = (dist2wp < dxspdchg)*(bs.traf.actwp.spd > -990.)*bs.traf.swvnavspd*bs.traf.swvnav
bs.traf.selspd = np.where(usespdcon, bs.traf.actwp.spd, bs.traf.selspd)
# Below crossover altitude: CAS=const, above crossover altitude: Mach = const
self.tas = vcasormach2tas(bs.traf.selspd, bs.traf.alt)
def atdistcmd(self, acidx, lat, lon, targdist, cmdtxt):
qdr, actdist = qdrdist(bs.traf.lat[acidx], bs.traf.lon[acidx], lat, lon)
self.addcondition(acidx, postype, targdist, actdist, cmdtxt, (lat,lon))
return True
def ETA(acidx):
# Assuming no wind.
# Assume complete route is available including orig --> destination
# print(np.where(traf.ap.route[acidx].wptype))
# Acceleration value depends on flight phase and aircraft type in BADA
# The longitudinal acceleration is constant for all airborne flight
# phases in the bada model.
ax = traf.perf.acceleration()
iactwp = traf.ap.route[acidx].iactwp
nwp = len(traf.ap.route[acidx].wpname)
# Construct a tables with relevant route parameters before the 4D prediction
dist2vs = [0]*nwp
Vtas = [0]*nwp
Vcas = traf.ap.route[acidx].wpspd.copy()
dsturn = [0]*nwp
qdr = [0]*nwp
s = [0]*nwp
turndist = [0]*nwp
lat = traf.ap.route[acidx].wplat.copy()
lon = traf.ap.route[acidx].wplon.copy()
altatwp = traf.ap.route[acidx].wpalt.copy()
wpialt = traf.ap.route[acidx].wpialt.copy()
for wpidx in range(iactwp-1, nwp):
# If the next waypoint is the active waypoint, use aircraft data
# Else use previous waypoint data
if wpidx == iactwp-1:
Vtas[wpidx] = traf.gs[acidx] # [m/s]
Vcas[wpidx] = traf.cas[acidx]
lat[wpidx] = traf.lat[acidx] # [deg]
lon[wpidx] = traf.lon[acidx] # [deg]
altatwp[wpidx] = traf.alt[acidx] # [m]
else:
lat[wpidx] = traf.ap.route[acidx].wplat[wpidx] # [deg]
lon[wpidx] = traf.ap.route[acidx].wplon[wpidx] #[deg]
qdr, s[wpidx] = qdrdist(lat[wpidx-1], lon[wpidx-1], lat[wpidx], lon[wpidx]) # [nm]
s[wpidx] *= nm # [m]
# check for valid speed, if no speed is given take the speed
# already in memory from previous waypoint.
if traf.ap.route[acidx].wpspd[wpidx] < 0:
Vtas[wpidx] = Vtas[wpidx-1]
Vcas[wpidx] = Vcas[wpidx-1]
else:
Vtas[wpidx] = vcas2tas(traf.ap.route[acidx].wpspd[wpidx],
traf.ap.route[acidx].wpalt[wpidx]).item() # [m/s]
# Compute distance correction for flyby turns.
if wpidx < nwp - 2:
#bearing of leg --> Second leg.
nextqdr, nexts = qdrdist(lat[wpidx], lon[wpidx], lat[wpidx+1], lon[wpidx+1]) #[deg, nm]
nexts *= nm # [m]
dist2turn, turnrad = traf.actwp.calcturn(Vtas[wpidx], traf.bank[acidx], qdr, nextqdr) # [m], [m]
delhdg = np.abs(degto180(qdr%360-nextqdr%360)) # [deg] Heading change
distofturn = np.pi*turnrad*delhdg/360 # [m] half of distance on turn radius
dsturn[wpidx] = dist2turn - distofturn
turndist[wpidx] = dist2turn
# Now loop to fill the VNAV constraints. A second loop is necessary
# Because the complete speed profile and turn profile is required.
curalt = traf.alt[acidx]
for wpidx in range(iactwp, nwp):
# If Next altitude is not the altitude constraint, check if the
# aircraft should already decent. Otherwise aircraft stays at
# current altitude. Otherwise expected altitude is filled in.
if wpidx < wpialt[wpidx]:
# Compute whether decent should start or speed is still same
dist2vs[wpidx] = turndist[wpidx] + \
np.abs(curalt - traf.ap.route[acidx].wptoalt[wpidx]) / traf.ap.steepness
# If the dist2vs is smaller everything should be fine
if traf.ap.route[acidx].wpxtoalt[wpidx] > dist2vs[wpidx]:
dist2vs[wpidx] = 0
altatwp[wpidx] = curalt
else:
dist2vs[wpidx] = dist2vs[wpidx] - traf.ap.route[acidx].wpxtoalt[wpidx]
if curalt > traf.ap.route[acidx].wptoalt[wpidx]:
sign = -1
else:
sign = 1
curalt = curalt + dist2vs[wpidx] * traf.ap.steepness * sign
altatwp[wpidx] = curalt
# If there is an altitude constraint on the current waypoint compute
# dist2vs for that waypoint.
else:
dist2vs[wpidx] = np.abs(curalt - traf.ap.route[acidx].wptoalt[wpidx]) / traf.ap.steepness
if dist2vs[wpidx] > traf.ap.route[acidx].wpxtoalt[wpidx]:
dist2vs[wpidx] = traf.ap.route[acidx].wpxtoalt[wpidx]
# Start computing the actual 4D trajectory
t_total = 0
for wpidx in range(iactwp, nwp-1):
# If V1 != V2 the aircraft will first immediately accelerate
# according to the performance model. To compute the time for the
# leg, constant acceleration is assumed
Vcas1 = Vcas[wpidx-1]
Vtas1 = Vtas[wpidx-1]
Vcas2 = Vcas[wpidx]
Vtas2 = Vtas[wpidx]
s_leg = s[wpidx] - dsturn[wpidx-1] - dsturn[wpidx]
if Vcas1!=Vcas2:
axsign = 1 + -2*(Vcas1>Vcas2) # [-]
t_acc = (Vcas2-Vcas1)/ax * axsign # [s]
s_ax = t_acc * (Vtas2+Vtas1)/2 # [m] Constant acceleration
else:
s_ax = 0 # [m]
t_acc = 0
s_nom = s_leg - s_ax - dist2vs[wpidx] # [m] Distance of normal flight
# Check if acceleration distance and start of descent overlap
if s_nom < 0:
t_vs = 0
t_nom = 0
if dist2vs[wpidx] <= s_leg:
s1 = 0
s2 = s_ax
s3 = s_leg - s_ax
else:
s1 = s_leg - dist2vs[wpidx]
s2 = dist2vs[wpidx] + s_ax - s_leg
s3 = s_leg - s_ax
t1 = abc(0.5*vcas2tas(ax, altatwp[wpidx-1]), Vtas1, s1)
Vcas_vs = Vcas1 - t1 * ax
t2 = (Vcas2-Vcas_vs)/ax
alt2 = altatwp[wpidx-1] - s2 * traf.ap.steepness
Vtas_vs = vcas2tas(Vcas_vs, alt2)
t3 = s3/((Vtas_vs + Vtas2)/2)
t_leg = t1 + t2 + t3
else:
Vtas_nom = vcas2tas(Vcas2, altatwp[wpidx-1])
t_nom = s_nom/Vtas_nom
#Assume same speed for now.
t_vs = dist2vs[wpidx]/(Vtas_nom + Vtas2)/2
if dist2vs[wpidx] < 1:
t_vs = 0
t_leg = t_nom + t_acc + t_vs
t_total+=t_leg
# Debug print statements
# print('wptype ', traf.ap.route[acidx].wptype[traf.ap.route[acidx].iactwp])
# print('DEBUG')
# print('Vtas ', Vtas)
# print('Vcas ', Vcas)
# print('dsturn ', dsturn)
# print('s ', s)
# print('turndist ', turndist)
# print('lat ', lat)
# print('lon ', lon)
# print('altatwp ', altatwp)
# print('wpialt ', traf.ap.route[acidx].wpialt)
# print('wptoalt ', traf.ap.route[acidx].wptoalt)
# print('wpxtoalt ', traf.ap.route[acidx].wpxtoalt)
# print('altatwp ', altatwp)
# print('dist2vs ', dist2vs)
# print('eta', sim.simt + t_total)
# print(' ')
# print(t_total)
return t_total[0] + sim.simt
def update(self, simt):
# Scheduling: when dt has passed or restart
if self.t0 + self.dt < simt or simt < self.t0:
self.t0 = simt
# FMS LNAV mode:
qdr, dist = geo.qdrdist(bs.traf.lat, bs.traf.lon,
bs.traf.actwp.lat, bs.traf.actwp.lon) # [deg][nm])
# Shift waypoints for aircraft i where necessary
for i in bs.traf.actwp.Reached(qdr, dist):
# Save current wp speed
oldspd = bs.traf.actwp.spd[i]
# Get next wp (lnavon = False if no more waypoints)
lat, lon, alt, spd, xtoalt, toalt, 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
bs.traf.actwp.lon[i] = lon
bs.traf.actwp.flyby[i] = int(flyby) # 1.0 in case of fly by, else fly over
# User has entered an altitude for this waypoint
if alt >= 0.:
bs.traf.actwp.alt[i] = alt
if spd > 0. and bs.traf.swlnav[i] and bs.traf.swvnav[i]:
# Valid speed and LNAV and VNAV ap modes are on
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
if bs.traf.swvnav[i] and oldspd > 0.0:
destalt = alt if alt > 0.0 else bs.traf.alt[i]
if oldspd<2.0:
bs.traf.aspd[i] = mach2cas(oldspd, destalt)
bs.traf.ama[i] = oldspd
else:
bs.traf.aspd[i] = oldspd
bs.traf.ama[i] = cas2mach(oldspd, destalt)
# VNAV = FMS ALT/SPD mode
self.ComputeVNAV(i, toalt, xtoalt)
#=============== End of Waypoint switching loop ===================
#================= Continuous FMS guidance ========================
# Do VNAV start of descent check
dy = (bs.traf.actwp.lat - bs.traf.lat)
dx = (bs.traf.actwp.lon - bs.traf.lon) * bs.traf.coslat
dist2wp = 60. * nm * np.sqrt(dx * dx + dy * dy)
# VNAV logic: descend as late as possible, climb as soon as possible
startdescent = bs.traf.swvnav * ((dist2wp < self.dist2vs)+(bs.traf.actwp.alt > bs.traf.alt))
# If not lnav:Climb/descend if doing so before lnav/vnav was switched off
# (because there are no more waypoints). This is needed
# to continue descending when you get into a conflict
# while descending to the destination (the last waypoint)
# Use 100 nm (185.2 m) circle in case turndist might be zero
self.swvnavvs = np.where(bs.traf.swlnav, startdescent, dist <= np.maximum(185.2,bs.traf.actwp.turndist))
#Recalculate V/S based on current altitude and distance
t2go = dist2wp/np.maximum(0.5,bs.traf.gs)
bs.traf.actwp.vs = (bs.traf.actwp.alt-bs.traf.alt)/np.maximum(1.0,t2go)
self.vnavvs = np.where(self.swvnavvs, bs.traf.actwp.vs, self.vnavvs)
#was: self.vnavvs = np.where(self.swvnavvs, self.steepness * bs.traf.gs, self.vnavvs)
# self.vs = np.where(self.swvnavvs, self.vnavvs, bs.traf.avsdef * bs.traf.limvs_flag)
avs = np.where(abs(bs.traf.avs) > 0.1, bs.traf.avs, bs.traf.avsdef) # m/s
self.vs = np.where(self.swvnavvs, self.vnavvs, avs * bs.traf.limvs_flag)
self.alt = np.where(self.swvnavvs, bs.traf.actwp.alt, bs.traf.apalt)
# When descending or climbing in VNAV also update altitude command of select/hold mode
bs.traf.apalt = np.where(self.swvnavvs,bs.traf.actwp.alt,bs.traf.apalt)
# LNAV commanded track angle
self.trk = np.where(bs.traf.swlnav, qdr, self.trk)
# Below crossover altitude: CAS=const, above crossover altitude: MA = const
self.tas = vcas2tas(bs.traf.aspd, bs.traf.alt) * bs.traf.belco + vmach2tas(bs.traf.ama, bs.traf.alt) * bs.traf.abco
def direct(self, idx, wpnam):
#print("Hello from direct")
"""Set active point to a waypoint by name"""
name = wpnam.upper().strip()
if name != "" and self.wpname.count(name) > 0:
wpidx = self.wpname.index(name)
self.iactwp = wpidx
bs.traf.actwp.lat[idx] = self.wplat[wpidx]
bs.traf.actwp.lon[idx] = self.wplon[wpidx]
bs.traf.actwp.flyby[idx] = self.wpflyby[wpidx]
bs.traf.actwp.flyturn[idx] = self.wpflyturn[wpidx]
bs.traf.actwp.turnrad[idx] = self.wpturnrad[wpidx]
bs.traf.actwp.turnspd[idx] = self.wpturnspd[wpidx]
# Do calculation for VNAV
self.calcfp()
bs.traf.actwp.xtoalt[idx] = self.wpxtoalt[wpidx]
bs.traf.actwp.nextaltco[idx] = self.wptoalt[wpidx]
bs.traf.actwp.torta[idx] = self.wptorta[
wpidx] # available for active RTA-guidance
bs.traf.actwp.xtorta[idx] = self.wpxtorta[
wpidx] # available for active RTA-guidance
#VNAV calculations like V/S and speed for RTA
bs.traf.ap.ComputeVNAV(idx, self.wptoalt[wpidx], self.wpxtoalt[wpidx],\
self.wptorta[wpidx],self.wpxtorta[wpidx])
# If there is a speed specified, process it
if self.wpspd[wpidx] > 0.:
# Set target speed for autopilot
if self.wpalt[wpidx] < 0.0:
alt = bs.traf.alt[idx]
else:
alt = self.wpalt[wpidx]
# Check for valid Mach or CAS
if self.wpspd[wpidx] < 2.0:
cas = mach2cas(self.wpspd[wpidx], alt)
else:
cas = self.wpspd[wpidx]
# Save it for next leg
bs.traf.actwp.nextspd[idx] = cas
# No speed specified for next leg
else:
bs.traf.actwp.nextspd[idx] = -999.
qdr, dist = geo.qdrdist(bs.traf.lat[idx], bs.traf.lon[idx],
bs.traf.actwp.lat[idx],
bs.traf.actwp.lon[idx])
if self.wpflyturn[wpidx] or self.wpturnrad[wpidx] < 0.:
turnrad = self.wpturnrad[wpidx]
else:
turnrad = bs.traf.tas[idx] * bs.traf.tas[idx] / tan(
radians(25.)) / g0 / nm # [nm]default bank angle 25 deg
bs.traf.actwp.turndist[idx] = (bs.traf.actwp.flyby[idx] > 0.5) * \
turnrad*abs(tan(0.5*radians(max(5., abs(degto180(qdr -
self.wpdirfrom[self.iactwp])))))) # [nm]
bs.traf.swlnav[idx] = True
return True
else:
return False, "Waypoint " + wpnam + " not found"
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]
bs.traf.actwp.flyturn[i] = flyturn
bs.traf.actwp.turnrad[i] = turnrad
# 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
bs.traf.actwp.turnspd[i] = turnspd # new turnspd, turning by next waypoint
# 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])
# End of per waypoint i switching loop
# 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]
return
def distcalc(lat0, lon0, lat1, lon1):
try:
qdr, dist = geo.qdrdist(lat0, lon0, lat1, lon1)
return True, "QDR = %.2f deg, Dist = %.3f nm" % (qdr % 360.0, dist)
except:
return False, "Error in dist calculation."
def resolve(self, conf, ownship, intruder):
''' Main resolution module '''
# --> Resolution setting options (through METHOD stack command):
# 1 - MVP, GEO, vertical = METHOD BOTH 3D
# 2 - MVP, GEO, no vertical = METHOD BOTH 2D
# 3 - MVP, no GEO, vertical = METHOD MVP 3D
# 4 - MVP, no GEO, no vertical = METHOD MVP 2D
# 5 - no MVP, GEO, vertical = METHOD GEO 3D
# 6 - no MVP, GEO, no vertical = METHOD GEO 2D
# ------------------------------------------------------------------
# resolve() process:
# 1 -> if mvp is active, compute pairwise summed horizontal MVP solution
# (this will give newtrk, newgs, newvs, newalt for all aircraft)
# 2 -> check which aircraft will breach given the solution (from 1)
# (if mvp is inactive, return True for all aircraft)
# 3 -> reset newtrk, newgs, newvs, newalt to current aircraft state for aircraft with breach = True
# 4 -> loop through all pairwise conflicts where ownship breach == True (from 2)
# 4.a -> if mvp is active, compute dv_mvp for pairwise conflict
# - if mvp does not breach, proceed directly to (4.d)
# 4.b -> if geo is active, compute dv_geo for pairwise conflict
# 4.c -> using a decision criterion, choose mvp or geo solution
# (non-existing maneuvers will be automatically dismissed)
# 4.d -> if vertical is active, compute dv_ver
# - if vertical is not active, proceed directly to (4.f)
# 4.e -> using a decision criterion, choose horizontal (from 6) or vertical (from 7)
# (not computed maneuvers will be automatically dismissed)
# 4.f -> add chosen dv to already existing dv for ownship (pairwise summed)
# 5 -> compute average of horizontal maneuver: pairwise-summed average
# 6 -> add dv of all aircraft to newtrk, newgs, newvs, newalt from (1)
# (dv for breaching aircraft is zero by default)
# 7 -> cap velocities and return newtrk, newgs, newvs, newalt
# Initialize resomethod log for all conflicts in the current timestep
self.resomethod = np.full(len(conf.confpairs), 'MVP')
# Reset number of conflicts
traf_nconf = np.zeros(traf.ntraf, dtype=int)
# Create newvs, newalt
newvs = np.zeros(ownship.ntraf)
if np.any(self.layers):
newalt = np.array(self.layers[self.layidx])
else:
newalt = np.array(ownship.alt)
# Initialize switch to keep track of vertical maneuvers
traf_swalt = np.zeros(ownship.ntraf, dtype=np.bool)
# Get array containing active limits for each aircraft
gsmin, gsmax, trkmin, trkmax, vsmin, vsmax = ap_geo.get_limits()
# Create empty dv array for all aircraft
dv_traf = np.zeros((ownship.ntraf, 3))
# STEP 1: If MVP is active, determine pairwise summed horizontal solution and check for breaches =====
if self.mvp_active:
newtrk, newgs, _, _ = super().resolve(conf, ownship, intruder)
# STEP 2: Check for breaches =========================================================================
eps = 1e-10 # small increment for limit to prevent floating point errors leading to wrong result
breachgs = np.logical_or(newgs < (gsmin - eps), newgs >
(gsmax + eps))
breachtrk = np.logical_or(
degto180(newtrk - trkmin) < 0,
degto180(newtrk - trkmax) > 0)
# Bool array indicating which aircraft will breach a geovector limit through a CR maneuver
swbreach = (breachgs | breachtrk) & conf.inconf
# STEP 3: Reset newtrk, newgs, newvs, newalt for the breaching aircraft ==============================
newtrk[swbreach] = np.array(ownship.trk[swbreach])
newgs[swbreach] = np.array(ownship.gs[swbreach])
# If MVP is not active, set each aircraft to be breaching
# This ensures that all conflicts get a solution from the GEO module
# Also create newtrk, newgs
else:
newtrk = np.array(ownship.trk)
newgs = np.array(ownship.gs)
swbreach = np.ones(ownship.ntraf, dtype=np.bool)
# STEP 4: Loop through all conflicts in the current timestep =========================================
nconf = 0 # keeping track of conflict number to store resolution method
for ((ac1, ac2), qdr, dist, tcpa, tLOS) in zip(conf.confpairs, conf.qdr, \
conf.dist, conf.tcpa, conf.tLOS):
# Retrieve ownship and intruder index
idx1 = ownship.id.index(ac1)
idx2 = intruder.id.index(ac2)
# CR is switched off for aircraft in a geofence conflict: do not resolve
try:
if ap_geo.APgeo().geoconf[idx1]:
self.resomethod[nconf] = 'FCE'
nconf += 1
continue
except AttributeError: # if geofence autopilot is not implemented
pass
# Check if horizontal pairwise summed MVP solution will breach at all
if not swbreach[idx1]:
nconf += 1 # update conflict counter
continue # go to next iteration in for loop
# Check if the aircraft is already performing a vertical maneuver
if traf_swalt[idx1]:
self.resomethod[nconf] = 'IND'
nconf += 1
continue
# Retrieve horizontal geovector limits for the ownship
gsl = gsmin[idx1]
gsu = gsmax[idx1]
trkl = trkmin[idx1]
trku = trkmax[idx1]
# STEP 4a: MVP ======================================================================================
if self.mvp_active:
dv_mvp, _ = super().MVP(ownship, intruder, conf, qdr, dist,
tcpa, tLOS, idx1, idx2)
# Set mvp vs to zero
dv_mvp[2] = 0.
#Check if MVP solution leads to a geovector breach
gs_mvp = ownship.gs[idx1] + np.sqrt(dv_mvp[0]**2 +
dv_mvp[1]**2)
trk_mvp = ownship.trk[idx1] + (
np.rad2deg(np.arctan2(dv_mvp[0], dv_mvp[1])) % 360)
mvp_breach = True if gs_mvp < gsl or gs_mvp > gsu or \
degto180(trk_mvp - trkl) < 0 or degto180(trk_mvp - trku) > 0 else False
else: #if MVP not active
dv_mvp, mvp_breach = np.full(3, np.nan), True
# STEP 4b: GEO ======================================================================================
# Only compute the alternative maneuver if the MVP solution will breach
if mvp_breach:
if self.geo_active:
# Retrieve intruder rpz and horizontal ownship geovector limits
rpz = conf.rpz[idx2] * self.resofach
# GEO maneuver only exists if the pair is not in LoS
if dist > rpz:
dv_geo, type_geo = self.GEO(ownship, intruder, gsl, gsu, trkl, trku, qdr, \
dist, rpz, idx1, idx2)
# Check if GEO solution leads to a geovector breach
geo_breach = True if type_geo == 'INS' else False
elif not self.mvp_active: # if already in LoS and MVP not active, do nothing
dv_geo, geo_breach, type_geo = np.zeros(
3), False, 'LOS'
else: # if already in LoS but MVP maneuver can be used to escape
dv_geo, geo_breach, type_geo = np.full(
3, np.nan), False, 'LOS'
else: #if GEO not active
dv_geo, geo_breach, type_geo = np.full(
3, np.nan), True, 'undefined'
# STEP 4c: Choose horizontal maneuver ================================================================
p_mvp = p_horz if mvp_breach else 1 #penalty factor for mvp maneuver
p_geo = p_horz if geo_breach else 1 #penalty factor for geo maneuver
criterion_mvp = p_mvp * np.sqrt(dv_mvp[0]**2 + dv_mvp[1]**2)
criterion_geo = p_geo * np.sqrt(dv_geo[0]**2 + dv_geo[1]**2)
criteria = np.array([criterion_mvp, criterion_geo])
# The maneuver with the smallest existing horizontal state change magnitude will be chosen
do_geo = True if np.nanargmin(criteria) == 1 else False
if do_geo:
dv, self.resomethod[nconf] = dv_geo, type_geo
p_hor = p_vert if geo_breach else 1
else:
dv, self.resomethod[nconf], p_hor = dv_mvp, 'MVP', p_vert
# If the MVP solution for this pairwise conflict will not breach, choose MVP
else:
dv, self.resomethod[nconf], p_hor = dv_mvp, 'MVP', 1
# STEP 4d: vertical =================================================================================
# Compute vertical maneuver if active and choose between horizontal and vertical
alt_cur = newalt[idx1]
if self.ver_active:
# Vertical maneuver is only allowed if the aircraft has priority
if self.row_priority(ownship, intruder, idx1, idx2):
dv_ver, alt_res, layidx = self.vertical(
ownship, intruder, idx1, idx2, conf.hpz[idx2], tLOS)
# Retrieve vertical geovector limits for the ownship
vsl, vsu = vsmin[idx1], vsmax[idx1]
# Check if vertical solution leads to a geovector breach and set penalty
ver_breach = True if dv_ver[2] < vsl or dv_ver[
2] > vsu else False
p_ver = p_vert if ver_breach else 1
# Determine if traffic is in lookahead disc on current (_cur) and resolution (_res) alt
# Lookahead disc equals ownship ground speed * lookahead time with height = hpz
# Furthermore, number of instant LoS will be determined for both altitudes
traf_qdr, traf_dist = qdrdist(ownship.lat[idx1],
ownship.lon[idx1],
intruder.lat, intruder.lon)
traf_qdr, traf_dist = np.radians(traf_qdr), traf_dist * nm
traf_dh_cur = alt_cur - intruder.alt
traf_dh_res = alt_res - intruder.alt
# inside horizontally for both the lookahead disc and the PZ
ins_hor = traf_dist < (ownship.gs[idx1] *
conf.dtlookahead[idx1])
los_hor = traf_dist < conf.rpz[
idx1] # inside ownship PZ = LoS
los_hor[
idx1] = False # for LoS count -> exclude the ownship
# inside vertically on current and resolution altitude
ins_ver_cur = np.abs(traf_dh_cur) < conf.hpz[idx1]
ins_ver_res = np.abs(traf_dh_res) < conf.hpz[idx1]
# if both are True: traffic is inside
ins_cur = np.logical_and(ins_hor, ins_ver_cur)
ins_res = np.logical_and(ins_hor, ins_ver_res)
los_cur = np.logical_and(los_hor, ins_ver_cur)
los_res = np.logical_and(los_hor, ins_ver_res)
# number of instant losses of separation
nlos_cur = los_cur.sum()
nlos_res = los_res.sum()
# Determine if traffic is converging with ownship
# Aircraft are converging if distance between them will get smaller in the future
drel = np.array([
np.sin(traf_qdr) * traf_dist,
np.cos(traf_qdr) * traf_dist, traf_dh_cur
]).T
vall = np.array(
[intruder.gseast, intruder.gsnorth, intruder.vs]).T
vown = np.array([
ownship.gseast[idx1], ownship.gsnorth[idx1],
ownship.vs[idx1]
]).T
vrel = vall - vown
dnew = drel + vrel # look one second ahead
traf_newdist = np.sqrt(dnew[:, 0]**2 + dnew[:, 1]**2 +
dnew[:, 2]**2)
conv_cur = traf_newdist[ins_cur] < traf_dist[ins_cur]
conv_res = traf_newdist[ins_res] < traf_dist[ins_res]
# Count number of converging traffic
# (works since ownship is not converging with itself)
nconv_cur = conv_cur.sum()
nconv_res = conv_res.sum()
# Compute decision criterion for both current and resolution altitude
criterion_cur = p_hor * (nconv_cur + 100 * nlos_cur
) # x100 penalty for instant LoS
criterion_res = p_ver * (nconv_res + 100 * nlos_res
) # " "
# STEP 4e: Choose between horizontal and vertical maneuver and set dv accordingly ====================
do_ver = True if criterion_res < criterion_cur else False
if do_ver:
dv, self.resomethod[nconf] = dv_ver, 'VER'
newalt[idx1] = alt_res
self.layidx[idx1] = layidx
traf_swalt[idx1] = True
# Reset previously computed horizontal maneuvers
# These are not necessary any more
dv_traf[idx1, 0:2] = 0.
for n in range(
nconf
): # loop through previously solved pairwise conflicts
if ownship.id.index(
conf.confpairs[n]
[0]) == idx1: # check if ownship = idx1
self.resomethod[n] = 'IND'
# STEP 4f: Finally append chosen dv to complete dv array =============================================
dv_traf[idx1] += dv
traf_nconf[idx1] += 1
# Update conflict counter
nconf += 1
# STEP 5: Compute average of horizontal pairwise-summed state changes =================================
dv_traf[:,
0] = np.where(dv_traf[:, 0] == 0., 0., dv_traf[:, 0] /
traf_nconf) # average eartherly gs state-change
dv_traf[:,
1] = np.where(dv_traf[:, 1] == 0., 0., dv_traf[:, 1] /
traf_nconf) # average northerly gs state-change
# STEP 6: Update velocity vector of all aircraft =====================================================
newtrk = np.radians(newtrk)
newgseast = newgs * np.sin(newtrk)
newgsnorth = newgs * np.cos(newtrk)
v_traf = np.array([newgseast, newgsnorth, newvs])
v_traf += dv_traf.T
# Convert velocity vector to cyclindrical coordinate system
newtrk = (np.arctan2(v_traf[0, :], v_traf[1, :]) * 180 / np.pi) % 360
newgs = (np.sqrt(v_traf[0, :]**2 + v_traf[1, :]**2))
newvs = v_traf[2, :]
# STEP 7: Cap the velocity and vertical speed and return =============================================
newgscapped = np.maximum(ownship.perf.vmin,
np.minimum(ownship.perf.vmax, newgs))
newvscapped = np.maximum(ownship.perf.vsmin,
np.minimum(ownship.perf.vsmax, newvs))
return newtrk, newgscapped, newvscapped, newalt
def update(self):
self.sectorsd = np.zeros(len(self.sectors))
self.sectorconv = np.zeros(len(self.sectors))
self.sectoreff = []
if not traf.ntraf or not self.sectors:
return
# Check convergence using CD with large RPZ and tlook
confpairs, lospairs, inconf, tcpamax, qdr, dist, dcpa, tcpa, tLOS = \
traf.asas.cd.detect(traf, traf, 20 * nm, traf.asas.dh, 3600)
if confpairs:
own, intr = zip(*confpairs)
ownidx = traf.id2idx(own)
mask = traf.alt[ownidx] > 70 * ft
ownidx = np.array(ownidx)[mask]
dcpa = np.array(dcpa)[mask]
tcpa = np.array(tcpa)[mask]
else:
ownidx = np.array([])
sendeff = False
for idx, (sector, previnside) in enumerate(zip(self.sectors, self.acinside)):
inside = areafilter.checkInside(sector, traf.lat, traf.lon, traf.alt)
sectoreff = []
# Detect aircraft leaving and entering the sector
previds = set(previnside.acid)
ids = set(np.array(traf.id)[inside])
arrived = list(ids - previds)
left = previds - ids
# Split left aircraft in deleted and not deleted
left_intraf = left.intersection(traf.id)
left_del = list(left - left_intraf)
left_intraf = list(left_intraf)
arridx = traf.id2idx(arrived)
leftidx = traf.id2idx(left_intraf)
# Retrieve the current distance flown for arriving and leaving aircraft
arrdist = traf.distflown[arridx]
arrlat = traf.lat[arridx]
arrlon = traf.lon[arridx]
leftlat, leftlon, leftdist = self.delac.get(left_del)
leftlat = np.append(leftlat, traf.lat[leftidx])
leftlon = np.append(leftlon, traf.lon[leftidx])
leftdist = np.append(leftdist, traf.distflown[leftidx])
leftlat0, leftlon0, leftdist0 = previnside.get(left_del + left_intraf)
self.delac.delete(left_del)
if len(left) > 0:
q, d = geo.qdrdist(leftlat0, leftlon0, leftlat, leftlon)
# Exclude aircraft where origin = destination
mask = d > 10
sectoreff = list((leftdist[mask] - leftdist0[mask]) / d[mask] / nm)
names = np.array(left_del + left_intraf)[mask]
for name, eff in zip(names, sectoreff):
self.feff.write('{}, {}, {}\n'.format(sim.simt, name, eff))
sendeff = True
# print('{} aircraft left sector {}, distance flown (acid:dist):'.format(len(left), sector))
# for a, d0, d1, e in zip(left, leftdist0, leftdist, sectoreff):
# print('Aircraft {} flew {} meters (eff = {})'.format(a, round(d1-d0), e))
# Update inside data for this sector
previnside.delete(left)
previnside.extend(arrived, arrlat, arrlon, arrdist)
self.sectoreff.append(sectoreff)
self.sectorsd[idx] = np.count_nonzero(inside)
insidx = np.where(np.logical_and(inside, inconf))
pairsinside = np.isin(ownidx, insidx)
if len(pairsinside):
tnorm = np.array(tcpa)[pairsinside] / 300.0
dcpanorm = np.array(dcpa)[pairsinside] / (5.0 * nm)
self.sectorconv[idx] = np.sum(
np.sqrt(2.0 / tnorm * tnorm + dcpanorm * dcpanorm))
else:
self.sectorconv[idx] = 0
self.fconv.write('{}, {}\n'.format(sim.simt, self.sectorconv[idx]))
self.fsd.write('{}, {}\n'.format(sim.simt, self.sectorsd[idx]))
if sendeff:
self.effplot.send()
def __init__(self,name,cmd,cmdargs):
self.name = name.upper()
self.tupdate = sim.simt-999.
self.flow = 0
self.incircle = True
self.segdir = None # Segment direction in degrees
self.hdg = None # When runway is used as separate airport
# Is location a circle segment?
if swcircle and self.name[:4]=="SEGM":
self.type = "seg"
self.lat,self.lon,brg = getseg(self.name)
pass
else:
success, posobj = txt2pos(name,ctrlat,ctrlon)
if success:
# for runway type, get heading as default optional argument for command line
if posobj.type == "rwy":
aptname, rwyname = name.split('/RW')
rwyname = rwyname.lstrip('Y')
try:
self.hdg = navdb.rwythresholds[aptname][rwyname][2]
except:
self.hdg = None
pass
else:
rwyhdg = None
self.lat,self.lon = posobj.lat, posobj.lon
self.type = posobj.type
# If outside circle change lat,lon to edge of circle
self.incircle = incircle(self.lat,self.lon)
if not self.incircle:
self.type = "seg"
self.segdir,dist = qdrdist(ctrlat,ctrlon,posobj.lat,posobj.lon)
segname = "SEGM"+str(int(round(self.segdir)))
self.lat,self.lon,hdg = getseg(segname)
else:
print("ERROR: trafgen class Source called for "+name+". Position not found")
self.lat,self.lon = 0.0,0.0
# Aircraft types
self.actypes = ["*"]
# Runways
self.runways = [] # name
self.rwylat = []
self.rwylon = []
self.rwyhdg = []
self.rwyline = [] # number of aircraft waiting in line
self.rwytotime = [] # time of last takeoff
# If source type is a runway, add it as the only runway
if self.type=="rwy":
self.runways = [rwyname]
self.rwylat = [self.lat]
self.rwylon = [self.lon]
self.rwyhdg = [int(rwyname.rstrip("LCR").lstrip("0"))*10.]
self.rwyline = [] # number of aircraft waiting in line
self.rwytotime = [-999] # time of last takeoff
self.dtakeoff = 90. # sec take-off interval on one runway, default 90 sec
# Destinations
self.dest = []
self.destlat = []
self.destlon = []
self.desttype = [] # "apt","wpt","seg"
self.desthdg = [] # if dest is a runway (for flights within FIR) or circle segment (source outside FIR)
self.destactypes = [] # Types for this destinations ([]=use defaults for this source)
#Names of drawing objects runways
self.polys = [] # Names of current runway polygons to remove when runways change
# Start values
self.startaltmin = -999. # [ft] minimum starting altitude
self.startaltmax = -999. # [ft] maximum starting altitude
self.startspdmin = -999. # [m/s] minimum starting speed
self.startspdmax = -999. # [m/s] maximum starting speed
self.starthdgmin = -999. # [deg] Valid values -360 - 360 degrees (to also have an interval possible around 360)
self.starthdgmax = -999. # [deg] Valid values -360 - 360 degrees (to also have an interval possible around 360)
return
def __init__(self,name,cmd,cmdargs):
self.name = name.upper()
self.tupdate = sim.simt-999.
self.flow = 0
self.incircle = True
self.segdir = None # Segment direction in degrees
# Is location a circle segment?
if self.name[:4]=="SEGM":
self.type = "seg"
self.lat,self.lon,brg = getseg(self.name) # For SEGMnnn to SEGMnnn for crossing flights optional
pass
else:
success, posobj = txt2pos(name,ctrlat,ctrlon)
if success:
# for runway type, get heading as default optional argument for command line
if posobj.type == "rwy":
aptname, rwyname = name.split('/RW')
rwyname = rwyname.lstrip('Y')
try:
rwyhdg = navdb.rwythresholds[aptname][rwyname][2]
except:
rwyhdg = None
pass
else:
rwyhdg = None
self.lat,self.lon = posobj.lat, posobj.lon
self.type = posobj.type
# If outside circle change lat,lon to edge of circle
self.incircle = incircle(self.lat,self.lon)
if not self.incircle:
self.type = "seg"
self.segdir,dist = qdrdist(ctrlat,ctrlon,posobj.lat,posobj.lon)
segname = "SEGM"+str(int(round(self.segdir)))
self.lat,self.lon,hdg = getseg(segname)
else:
print("ERROR: trafgen class Drain called for "+name+". Position not found")
self.lat,self.lon = 0.0,0.0
# Aircraft types
self.actypes = ["*"]
# Runways
self.runways = [] # name
self.rwylat = []
self.rwylon = []
self.rwyhdg = []
self.rwyline = [] # number of aircraft waiting in line
#Origins
self.orig = []
self.origlat = []
self.origlon = []
self.origtype = [] # "apt","wpt","seg"
self.orighdg = [] # if orig is a runway (for flights within FIR) or circle segment (drain outside FIR)
self.origactypes = [] # Types for this originations ([]=use defaults for this drain)
self.origincirc = []
#Names of drawing objects runways
self.polys = []
return
def update(self,gain):
# Time step update of drain
# 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:
# Calculate starting position using origin
if len(self.orig)>0:
# Add origin
iorig = int(random.random() * len(self.orig))
else:
iorig = -1
if iorig>=0:
incirc = self.origincirc[iorig]
lat,lon = self.origlat[iorig],self.origlon[iorig]
hdg,dist = qdrdist(lat,lon,self.lat,self.lon)
else:
print("Warning update drain",self.name,"called with no origins present!")
hdg = random.random()*360.
print("using random segment",int(hdg+180)%360)
incirc = False
if not incirc:
lat,lon = kwikpos(ctrlat,ctrlon,(hdg+180)%360,radius)
elif self.origtype=="seg":
lat,lon,brg = getseg(self.name)
hdg = (brg+180)%360
else:
hdg = random.random()*360.
if incirc and (self.origtype[iorig]=="apt" or self.origtype[iorig]=="rwy"):
alttxt,spdtxt = str(0),str(0)
else:
alttxt,spdtxt = "FL"+str(random.randint(200,300)), str(random.randint(250,350))
if iorig>=0:
acid = randacname(self.orig[iorig], self.name)
else:
acid = randacname("LFPG", self.name)
if len(self.origactypes)>0:
actype = random.choice(self.origactypes[iorig])
else:
actype = random.choice(self.actypes)
stack.stack("CRE " + ",".join([acid,actype,str(lat), str(lon),
str(int(hdg%360)),alttxt,spdtxt]))
if iorig>=0:
if self.orig[iorig][:4]!="SEGM":
stack.stack(acid + " ORIG " + self.orig[iorig])
else:
stack.stack(acid + " ORIG " + str(self.origlat[iorig]) + " " +\
str(self.origlon[iorig]))
if not (self.name[:4]=="SEGM"):
stack.stack(acid + " DEST " + self.name)
else:
stack.stack(acid + " ADDWPT " + str(self.lat) + " " + str(self.lon))
if alttxt=="0" and spdtxt =="0":
stack.stack(acid+" SPD 250")
stack.stack(acid+" ALT FL100")
#stack.stack(acid+" LNAV ON")
else:
stack.stack(acid + " LNAV ON")
def newcalcfp(self):
"""Do flight plan calculations"""
# Remove old top of descents and old top of climbs
while self.wpname.count("T/D")>0:
self.delwpt("T/D")
while self.wpname.count("T/C")>0:
self.delwpt("T/C")
# Remove old actual position waypoints
while self.wpname.count("A/C")>0:
self.delwpt("A/C")
# Insert actual position as A/C waypoint
idx = self.iactwp
self.insertcalcwp(idx,"A/C")
self.wplat[idx] = bs.traf.lat[self.iac] # deg
self.wplon[idx] = bs.traf.lon[self.iac] # deg
self.wpalt[idx] = bs.traf.alt[self.iac] # m
self.wpspd[idx] = bs.traf.tas[self.iac] # m/s
# Calculate distance to last waypoint in route
nwp = len(self.wpname)
dist2go = [0.0]
for i in range(nwp - 2, -1, -1):
qdr, dist = geo.qdrdist(self.wplat[i], self.wplon[i],
self.wplat[i + 1], self.wplon[i + 1])
dist2go = [dist2go[0] + dist] + dist2go
# Make VNAV WP list with only waypoints with altitude constraints
# This list we will use to find where to insert t/c and t/d
alt = []
x = []
name = []
for i in range(nwp):
if self.wpalt[i]>-1.:
alt.append(self.wpalt[i])
x.append(dist2go[i])
name.append(self.wpname[i]+" ") # space for check first 2 chars later
# Find where to insert cruise segment (if any)
# Find longest segment without altitude constraints
desslope = clbslope = 1.
crzalt = bs.traf.crzalt[self.iac]
if crzalt>0.:
ilong = -1
dxlong = 0.0
nvwp = len(alt)
for i in range(nvwp-1):
if x[i]-x[i+1]> dxlong:
ilong = i
dxlong = x[i]-x[i+1]
# VNAV parameters to insert T/Cs and T/Ds
crzdist = 20.*nm # minimally required distance at cruise level
clbslope = 3000.*ft/(10.*nm) # 1:3 rule for now
desslope = clbslope # 1:3 rule for now
# Can we get a sufficient distance at cruise altitude?
if max(alt[ilong],alt[ilong+1]) < crzalt :
dxclimb = (crzalt-alt[ilong])*clbslope
dxdesc = (crzalt-alt[ilong+1])*desslope
if x[ilong] - x[ilong+1] > dxclimb + crzdist + dxdesc:
# Insert T/C (top of climb) at cruise level
name.insert(ilong+1,"T/C")
alt.insert(ilong+1,crzalt)
x.insert(ilong+1,x[ilong]+dxclimb)
# Insert T/D (top of descent) at cruise level
name.insert(ilong+2,"T/D")
alt.insert(ilong+2,crzalt)
x.insert(ilong+2,x[ilong+1]-dxdesc)
# Compare angles to rates:
epsh = 50.*ft # Nothing to be done for small altitude changes
epsx = 1.*nm # [m] Nothing to be done at this short range
i = 0
while i<len(alt)-1:
if name[i][:2]=="T/":
continue
dy = alt[i+1]-alt[i] # alt change (pos = climb)
dx = x[i]-x[i+1] # distance (positive)
dxdes = abs(dy)/desslope
dxclb = abs(dy)/clbslope
if dy<epsh and dx + epsx > dxdes: # insert T/D?
name.insert(i+1,"T/D")
alt.insert(i+1,alt[i])
x.insert(i+1,x[i+1]-dxdes)
i += 1
elif dy>epsh and dx + epsx > dxclb: # insert T/C?
name.insert(i+1,"T/C")
alt.insert(i+1,alt[i+1])
x.insert(i+1,x[i]+dxclb)
i += 2
else:
i += 1
# Now insert T/Cs and T/Ds in actual flight plan
nvwp = len(alt)
for i in range(nvwp,-1,-1):
# Copy all new waypoints (which are all named T/C or T/D)
if name[i][:2]=="T/":
# Find place in flight plan to insert T/C or T/D
j = nvwp-1
while dist2go[j]<x[i] and j>1:
j=j-1
# Interpolation factor for position on leg
f = (x[i]-dist2go[j+1])/(dist2go[j]-dist2go[j+1])
lat = f*self.wplat[j]+(1.-f)*self.wplat[j+1]
lon = f*self.wplon[j]+(1.-f)*self.wplon[j+1]
self.wpname.insert(j,name[i])
self.wptype.insert(j,Route.calcwp)
self.wplat.insert(j,lat)
self.wplon.insert(j,lon)
self.wpalt.insert(j,alt[i])
self.wpspd.insert(j,-999.)
def update(self, simt):
# Scheduling: when dt has passed or restart
if self.t0 + self.dt < simt or simt < self.t0 or simt<self.dt:
self.t0 = simt
# FMS LNAV mode:
# qdr[deg],distinnm[nm]
qdr, distinnm = geo.qdrdist(bs.traf.lat, bs.traf.lon,
bs.traf.actwp.lat, bs.traf.actwp.lon) # [deg][nm])
dist = distinnm*nm # Conversion to meters
# Shift waypoints for aircraft i where necessary
for i in bs.traf.actwp.Reached(qdr,dist,bs.traf.actwp.flyby):
# Save current wp speed
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, \
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]
bs.traf.actwp.flyby[i] = int(flyby) # 1.0 in case of fly by, else fly over
# User has entered an altitude for this waypoint
if alt >= -0.01:
bs.traf.actwp.nextaltco[i] = alt #[m]
if spd > 0. and bs.traf.swlnav[i] and bs.traf.swvnav[i]:
# Valid speed and LNAV and VNAV ap modes are on
# Depending on crossover altitude we fix CAS or Mach
if bs.traf.abco[i] and spd>2.0:
bs.traf.actwp.spd[i] = cas2mach(spd,bs.traf.alt[i])
elif bs.traf.belco[i] and spd<=2.0:
bs.traf.actwp.spd[i] = mach2cas(spd,bs.traf.alt[i])
else:
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
# VNAV = FMS ALT/SPD mode
self.ComputeVNAV(i, toalt, bs.traf.actwp.xtoalt[i])
#=============== End of Waypoint switching loop ===================
#================= Continuous FMS guidance ========================
# Waypoint switching in the loop above was scalar (per a/c with index i)
# Code below is vectorized, with arrays for all aircraft
# Do VNAV start of descent check
dy = (bs.traf.actwp.lat - bs.traf.lat) #[deg lat = 60 nm]
dx = (bs.traf.actwp.lon - bs.traf.lon) * bs.traf.coslat #[corrected deg lon = 60 nm]
dist2wp = 60. * nm * np.sqrt(dx * dx + dy * dy) # [m]
# VNAV logic: descend as late as possible, climb as soon as possible
startdescent = (dist2wp < self.dist2vs) + (bs.traf.actwp.nextaltco > bs.traf.alt)
# If not lnav:Climb/descend if doing so before lnav/vnav was switched off
# (because there are no more waypoints). This is needed
# to continue descending when you get into a conflict
# while descending to the destination (the last waypoint)
# Use 0.1 nm (185.2 m) circle in case turndist might be zero
self.swvnavvs = bs.traf.swvnav * np.where(bs.traf.swlnav, startdescent,
dist <= np.maximum(185.2,bs.traf.actwp.turndist))
#Recalculate V/S based on current altitude and distance to next alt constraint
# How much time do we have before we need to descend?
t2go2alt = np.maximum(0.,(dist2wp + bs.traf.actwp.xtoalt - bs.traf.actwp.turndist)) \
/ np.maximum(0.5,bs.traf.gs)
# use steepness to calculate V/S unless we need to descend faster
bs.traf.actwp.vs = np.maximum(self.steepness*bs.traf.gs, \
np.abs((bs.traf.actwp.nextaltco-bs.traf.alt)) \
/np.maximum(1.0,t2go2alt))
self.vnavvs = np.where(self.swvnavvs, bs.traf.actwp.vs, self.vnavvs)
#was: self.vnavvs = np.where(self.swvnavvs, self.steepness * bs.traf.gs, self.vnavvs)
# self.vs = np.where(self.swvnavvs, self.vnavvs, bs.traf.apvsdef * bs.traf.limvs_flag)
selvs = np.where(abs(bs.traf.selvs) > 0.1, bs.traf.selvs, bs.traf.apvsdef) # m/s
self.vs = np.where(self.swvnavvs, self.vnavvs, selvs)
self.alt = np.where(self.swvnavvs, bs.traf.actwp.nextaltco, bs.traf.selalt)
# When descending or climbing in VNAV also update altitude command of select/hold mode
bs.traf.selalt = np.where(self.swvnavvs,bs.traf.actwp.nextaltco,bs.traf.selalt)
# LNAV commanded track angle
self.trk = np.where(bs.traf.swlnav, qdr, self.trk)
# FMS speed guidance: anticipate accel distance
# Actual distance it takes to decelerate
nexttas = vcasormach2tas(bs.traf.actwp.spd,bs.traf.alt)
tasdiff = nexttas - bs.traf.tas # [m/s]
dtspdchg = np.abs(tasdiff)/np.maximum(0.01,np.abs(bs.traf.ax)) #[s]
dxspdchg = 0.5*np.sign(tasdiff)*np.abs(bs.traf.ax)*dtspdchg*dtspdchg + bs.traf.tas*dtspdchg #[m]
usespdcon = (dist2wp < dxspdchg)*(bs.traf.actwp.spd > 0.)*bs.traf.swvnav
bs.traf.selspd = np.where(usespdcon, bs.traf.actwp.spd, bs.traf.selspd)
# Below crossover altitude: CAS=const, above crossover altitude: Mach = const
self.tas = vcasormach2tas(bs.traf.selspd, bs.traf.alt)
def update(self, simt):
# Scheduling: when dt has passed or restart
if self.t0 + self.dt < simt or simt < self.t0 or simt<self.dt:
self.t0 = simt
# FMS LNAV mode:
# qdr[deg],distinnm[nm]
qdr, distinnm = geo.qdrdist(bs.traf.lat, bs.traf.lon,
bs.traf.actwp.lat, bs.traf.actwp.lon) # [deg][nm])
dist = distinnm*nm # Conversion to meters
# Shift waypoints for aircraft i where necessary
for i in bs.traf.actwp.Reached(qdr,dist,bs.traf.actwp.flyby):
# Save current wp speed
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, \
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]
bs.traf.actwp.flyby[i] = int(flyby) # 1.0 in case of fly by, else fly over
# 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]:
# Valid speed and LNAV and VNAV ap modes are on
# Depending on crossover altitude we fix CAS or Mach
if bs.traf.abco[i] and spd>1.0:
bs.traf.actwp.spd[i] = cas2mach(spd,bs.traf.alt[i])
elif bs.traf.belco[i] and 0. < spd<=1.0:
bs.traf.actwp.spd[i] = mach2cas(spd,bs.traf.alt[i])
else:
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
# VNAV = FMS ALT/SPD mode
self.ComputeVNAV(i, toalt, bs.traf.actwp.xtoalt[i])
#=============== End of Waypoint switching loop ===================
#================= Continuous FMS guidance ========================
# Waypoint switching in the loop above was scalar (per a/c with index i)
# Code below is vectorized, with arrays for all aircraft
# Do VNAV start of descent check
dy = (bs.traf.actwp.lat - bs.traf.lat) #[deg lat = 60 nm]
dx = (bs.traf.actwp.lon - bs.traf.lon) * bs.traf.coslat #[corrected deg lon = 60 nm]
dist2wp = 60. * nm * np.sqrt(dx * dx + dy * dy) # [m]
# VNAV logic: descend as late as possible, climb as soon as possible
startdescent = (dist2wp < self.dist2vs) + (bs.traf.actwp.nextaltco > bs.traf.alt)
# If not lnav:Climb/descend if doing so before lnav/vnav was switched off
# (because there are no more waypoints). This is needed
# to continue descending when you get into a conflict
# while descending to the destination (the last waypoint)
# Use 0.1 nm (185.2 m) circle in case turndist might be zero
self.swvnavvs = bs.traf.swvnav * np.where(bs.traf.swlnav, startdescent,
dist <= np.maximum(185.2,bs.traf.actwp.turndist))
#Recalculate V/S based on current altitude and distance to next alt constraint
# How much time do we have before we need to descend?
t2go2alt = np.maximum(0.,(dist2wp + bs.traf.actwp.xtoalt - bs.traf.actwp.turndist)) \
/ np.maximum(0.5,bs.traf.gs)
# use steepness to calculate V/S unless we need to descend faster
bs.traf.actwp.vs = np.maximum(self.steepness*bs.traf.gs, \
np.abs((bs.traf.actwp.nextaltco-bs.traf.alt)) \
/np.maximum(1.0,t2go2alt))
self.vnavvs = np.where(self.swvnavvs, bs.traf.actwp.vs, self.vnavvs)
#was: self.vnavvs = np.where(self.swvnavvs, self.steepness * bs.traf.gs, self.vnavvs)
# self.vs = np.where(self.swvnavvs, self.vnavvs, bs.traf.apvsdef * bs.traf.limvs_flag)
selvs = np.where(abs(bs.traf.selvs) > 0.1, bs.traf.selvs, bs.traf.apvsdef) # m/s
self.vs = np.where(self.swvnavvs, self.vnavvs, selvs)
self.alt = np.where(self.swvnavvs, bs.traf.actwp.nextaltco, bs.traf.selalt)
# When descending or climbing in VNAV also update altitude command of select/hold mode
bs.traf.selalt = np.where(self.swvnavvs,bs.traf.actwp.nextaltco,bs.traf.selalt)
# LNAV commanded track angle
self.trk = np.where(bs.traf.swlnav, qdr, self.trk)
# FMS speed guidance: anticipate accel distance
# Actual distance it takes to decelerate
nexttas = vcasormach2tas(bs.traf.actwp.spd,bs.traf.alt)
tasdiff = nexttas - bs.traf.tas # [m/s]
dtspdchg = np.abs(tasdiff)/np.maximum(0.01,np.abs(bs.traf.ax)) #[s]
dxspdchg = 0.5*np.sign(tasdiff)*np.abs(bs.traf.ax)*dtspdchg*dtspdchg + bs.traf.tas*dtspdchg #[m]
usespdcon = (dist2wp < dxspdchg)*(bs.traf.actwp.spd > -990.)*bs.traf.swvnav
bs.traf.selspd = np.where(usespdcon, bs.traf.actwp.spd, bs.traf.selspd)
# Below crossover altitude: CAS=const, above crossover altitude: Mach = const
self.tas = vcasormach2tas(bs.traf.selspd, bs.traf.alt)
def update(self):
# FMS LNAV mode:
# qdr[deg],distinnm[nm]
qdr, distinnm = geo.qdrdist(bs.traf.lat, bs.traf.lon,
bs.traf.actwp.lat, bs.traf.actwp.lon) # [deg][nm])
dist2wp = distinnm*nm # Conversion to meters
# FMS route update and possibly waypoint shift. Note: qdr, dist2wp will be updated accordingly in case of wp switch
self.update_fms(qdr, dist2wp)
#================= Continuous FMS guidance ========================
# Waypoint switching in the loop above was scalar (per a/c with index i)
# Code below is vectorized, with arrays for all aircraft
# Do VNAV start of descent check
#dy = (bs.traf.actwp.lat - bs.traf.lat) #[deg lat = 60 nm]
#dx = (bs.traf.actwp.lon - bs.traf.lon) * bs.traf.coslat #[corrected deg lon = 60 nm]
#dist2wp = 60. * nm * np.sqrt(dx * dx + dy * dy) # [m]
# VNAV logic: descend as late as possible, climb as soon as possible
startdescent = (dist2wp < self.dist2vs) + (bs.traf.actwp.nextaltco > bs.traf.alt)
# If not lnav:Climb/descend if doing so before lnav/vnav was switched off
# (because there are no more waypoints). This is needed
# to continue descending when you get into a conflict
# while descending to the destination (the last waypoint)
# Use 0.1 nm (185.2 m) circle in case turndist might be zero
self.swvnavvs = bs.traf.swvnav * np.where(bs.traf.swlnav, startdescent,
dist2wp <= np.maximum(185.2,bs.traf.actwp.turndist))
#Recalculate V/S based on current altitude and distance to next alt constraint
# How much time do we have before we need to descend?
t2go2alt = np.maximum(0.,(dist2wp + bs.traf.actwp.xtoalt - bs.traf.actwp.turndist)) \
/ np.maximum(0.5,bs.traf.gs)
# use steepness to calculate V/S unless we need to descend faster
bs.traf.actwp.vs = np.maximum(self.steepness*bs.traf.gs, \
np.abs((bs.traf.actwp.nextaltco-bs.traf.alt)) \
/np.maximum(1.0,t2go2alt))
self.vnavvs = np.where(self.swvnavvs, bs.traf.actwp.vs, self.vnavvs)
#was: self.vnavvs = np.where(self.swvnavvs, self.steepness * bs.traf.gs, self.vnavvs)
# self.vs = np.where(self.swvnavvs, self.vnavvs, bs.traf.apvsdef * bs.traf.limvs_flag)
selvs = np.where(abs(bs.traf.selvs) > 0.1, bs.traf.selvs, bs.traf.apvsdef) # m/s
self.vs = np.where(self.swvnavvs, self.vnavvs, selvs)
self.alt = np.where(self.swvnavvs, bs.traf.actwp.nextaltco, bs.traf.selalt)
# When descending or climbing in VNAV also update altitude command of select/hold mode
bs.traf.selalt = np.where(self.swvnavvs,bs.traf.actwp.nextaltco,bs.traf.selalt)
# LNAV commanded track angle
self.trk = np.where(bs.traf.swlnav, qdr, self.trk)
# FMS speed guidance: anticipate accel/decel distance for next leg or turn
# Calculate actual distance it takes to decelerate/accelerate based on two cases: turning speed (decel)
# Normally next leg speed (actwp.spd) but in case we fly turns with a specified turn speed
# use the turn speed
# Is turn speed specified and are we not already slow enough? We only decelerate for turns, not accel.
turntas = np.where(bs.traf.actwp.turnspd>0.0, vcas2tas(bs.traf.actwp.turnspd, bs.traf.alt),
-1.0+0.*bs.traf.tas)
swturnspd = bs.traf.actwp.flyturn*(turntas>0.0)*(bs.traf.tas>turntas)*(bs.traf.actwp.turnspd>0.0)
turntasdiff = (bs.traf.tas - turntas)*(turntas>0.0)
# dt = dv/a ; dx = v*dt + 0.5*a*dt2 => dx = dv/a * (v + 0.5*dv)
dxturnspdchg = np.where(swturnspd, turntasdiff/np.maximum(0.01,np.abs(bs.traf.ax)*(bs.traf.tas+0.5*np.abs(turntasdiff))),
0.0*bs.traf.tas)
# Decelerate or accelerate for next required speed because of speed constraint or RTA speed
nexttas = vcasormach2tas(bs.traf.actwp.spd,bs.traf.alt)
tasdiff = nexttas - bs.traf.tas # [m/s]
# dt = dv/a dx = v*dt + 0.5*a*dt2 => dx = dv/a * (v + 0.5*dv)
dxspdconchg = np.abs(tasdiff)/np.maximum(0.01, np.abs(bs.traf.ax)) * (bs.traf.tas+0.5*np.abs(tasdiff))
# Check also whether VNAVSPD is on, if not, SPD SEL has override for next leg
# and same for turn logic
usespdcon = (dist2wp < dxspdconchg)*(bs.traf.actwp.spdcon> -990.) * \
bs.traf.swvnavspd*bs.traf.swvnav*bs.traf.swlnav
useturnspd = (dist2wp < dxturnspdchg)*swturnspd * \
bs.traf.swvnavspd*bs.traf.swvnav*bs.traf.swlnav
# Which CAS/Mach do we have to keep? VNAV, last turn or next turn?
oncurrentleg = (abs(bs.traf.trk - qdr) < 2.0)
inoldturn = (bs.traf.actwp.oldturnspd > 0.) * np.logical_not(oncurrentleg) # [deg]
# Avoid using old turning speeds when turning of this leg ot the next leg
# by disabling (old) turningspd when on leg
bs.traf.actwp.oldturnspd = np.where(oncurrentleg*bs.traf.actwp.oldturnspd, -999.,
bs.traf.actwp.oldturnspd)
# Select speed
bs.traf.selspd = np.where(useturnspd,bs.traf.actwp.turnspd,np.where(usespdcon, bs.traf.actwp.spd, bs.traf.selspd))
# Temporary override when still in old turn
bs.traf.selspd = np.where(inoldturn*(bs.traf.actwp.oldturnspd)*bs.traf.swvnavspd*bs.traf.swvnav*bs.traf.swlnav,
bs.traf.actwp.oldturnspd,bs.traf.selspd)
# Below crossover altitude: CAS=const, above crossover altitude: Mach = const
self.tas = vcasormach2tas(bs.traf.selspd, bs.traf.alt)
return
