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 creconfs(self,
acid,
actype,
targetidx,
dpsi,
dcpa,
tlosh,
dH=None,
tlosv=None,
spd=None):
''' Create an aircraft in conflict with target aircraft.
Arguments:
- acid: callsign of new aircraft
- actype: aircraft type of new aircraft
- targetidx: id (callsign) of target aircraft
- dpsi: Conflict angle (angle between tracks of ownship and intruder) (deg)
- cpa: Predicted distance at closest point of approach (NM)
- tlosh: Horizontal time to loss of separation ((hh:mm:)sec)
- dH: Vertical distance (ft)
- tlosv: Vertical time to loss of separation
- spd: Speed of new aircraft (CAS/Mach, kts/-)
'''
latref = self.lat[targetidx] # deg
lonref = self.lon[targetidx] # deg
altref = self.alt[targetidx] # m
trkref = radians(self.trk[targetidx])
gsref = self.gs[targetidx] # m/s
tasref = self.tas[targetidx] # m/s
vsref = self.vs[targetidx] # m/s
cpa = dcpa * nm
pzr = bs.settings.asas_pzr * nm
pzh = bs.settings.asas_pzh * ft
trk = trkref + radians(dpsi)
if dH is None:
acalt = altref
acvs = 0.0
else:
acalt = altref + dH
tlosv = tlosh if tlosv is None else tlosv
acvs = vsref - np.sign(dH) * (abs(dH) - pzh) / tlosv
if spd:
# CAS or Mach provided: convert to groundspeed, assuming that
# wind at intruder position is similar to wind at ownship position
tas = tasref if spd is None else casormach2tas(spd, acalt)
tasn, tase = tas * cos(trk), tas * sin(trk)
wn, we = self.wind.getdata(latref, lonref, acalt)
gsn, gse = tasn + wn, tase + we
else:
# Groundspeed is the same as ownship
gsn, gse = gsref * cos(trk), gsref * sin(trk)
# Horizontal relative velocity vector
vreln, vrele = gsref * cos(trkref) - gsn, gsref * sin(trkref) - gse
# Relative velocity magnitude
vrel = sqrt(vreln * vreln + vrele * vrele)
# Relative travel distance to closest point of approach
drelcpa = tlosh * vrel + (0 if cpa > pzr else sqrt(pzr * pzr -
cpa * cpa))
# Initial intruder distance
dist = sqrt(drelcpa * drelcpa + cpa * cpa)
# Rotation matrix diagonal and cross elements for distance vector
rd = drelcpa / dist
rx = cpa / dist
# Rotate relative velocity vector to obtain intruder bearing
brn = degrees(atan2(-rx * vreln + rd * vrele, rd * vreln + rx * vrele))
# Calculate intruder lat/lon
aclat, aclon = geo.kwikpos(latref, lonref, brn, dist / nm)
# convert groundspeed to CAS, and track to heading using actual
# intruder position
wn, we = self.wind.getdata(aclat, aclon, acalt)
tasn, tase = gsn - wn, gse - we
acspd = tas2cas(sqrt(tasn * tasn + tase * tase), acalt)
achdg = degrees(atan2(tase, tasn))
# Create and, when necessary, set vertical speed
self.cre(acid, actype, aclat, aclon, achdg, acalt, acspd)
self.ap.selaltcmd(len(self.lat) - 1, altref, acvs)
self.vs[-1] = acvs
