def limits(self, intent_v_tas, intent_vs, intent_h, ax):
""" apply limits on indent speed, vertical speed, and altitude (called in pilot module)"""
super(PerfNAP, self).limits(intent_v_tas, intent_vs, intent_h)
# if isinstance(self.vmin, np.ndarray):
# pass
# else:
# self.update()
allow_h = np.where(intent_h > self.hmax, self.hmax, intent_h)
intent_v_cas = aero.vtas2cas(intent_v_tas, allow_h)
allow_v_cas = np.where(intent_v_cas < self.vmin, self.vmin, intent_v_cas)
allow_v_cas = np.where(intent_v_cas > self.vmax, self.vmax, allow_v_cas)
allow_v_tas = aero.vcas2tas(allow_v_cas, allow_h)
vs_max_with_acc = (1 - ax / self.axmax) * self.vsmax
allow_vs = np.where(intent_vs > self.vsmax, vs_max_with_acc, intent_vs)
allow_vs = np.where(intent_vs < self.vsmin, self.vsmin, allow_vs)
# print(intent_h, allow_h, self.hmax)
# print(intent_v_cas, allow_v_cas, self.vmin, self.vmax)
# print(self.coeff.limits_rotor['EC35'])
return allow_v_tas, allow_vs, allow_h
def FlightEnvelope(self):
# check for the flight envelope
bs.traf.delalt = bs.traf.apalt - bs.traf.alt # [m]
bs.traf.perf.limits()
#print self.aspd[0]/kts
# Update desired sates with values within the flight envelope
# To do: add const Mach const CAS mode
self.spd = np.where(bs.traf.limspd_flag,
vcas2tas(bs.traf.limspd, bs.traf.alt), self.spd)
# Autopilot selected altitude [m]
self.alt = np.where(bs.traf.limalt > -900., bs.traf.limalt, self.alt)
# Autopilot selected vertical speed (V/S)
self.vs = np.where(bs.traf.limvs > -9000., bs.traf.limvs, self.vs)
# To be discussed: Following change in VNAV mode only?
# below crossover altitude: CAS=const, above crossover altitude: MA = const
# climb/descend above crossover: Ma = const, else CAS = const
# ama is fixed when above crossover
bs.traf.ama = np.where(bs.traf.abco * (bs.traf.ama == 0.),
vcas2mach(bs.traf.aspd, bs.traf.alt),
bs.traf.ama)
# ama is deleted when below crossover
bs.traf.ama = np.where(bs.traf.belco, 0.0, bs.traf.ama)
def applylimits(self):
# check for the flight envelope
if bs.settings.performance_model == 'openap':
self.aporasas.tas, self.aporasas.vs, self.aporasas.alt = \
self.perf.limits(self.aporasas.tas, self.aporasas.vs, \
self.aporasas.alt, self.ax)
else:
self.perf.limits(
) # Sets limspd_flag and limspd when it needs to be limited
# Update desired sates with values within the flight envelope
# When CAS is limited, it needs to be converted to TAS as only this TAS is used later on!
self.aporasas.tas = np.where(self.limspd_flag,
vcas2tas(self.limspd, self.alt),
self.aporasas.tas)
# Autopilot selected altitude [m]
self.aporasas.alt = np.where(self.limalt_flag, bs.traf.limalt,
self.aporasas.alt)
# Autopilot selected vertical speed (V/S)
self.aporasas.vs = np.where(self.limvs_flag, self.limvs,
self.aporasas.vs)
def limits(self, intent_v_tas, intent_vs, intent_h, ax):
""" apply limits on indent speed, vertical speed, and altitude (called in pilot module)"""
super(OpenAP, self).limits(intent_v_tas, intent_vs, intent_h)
allow_h = np.where(intent_h > self.hmax, self.hmax, intent_h)
intent_v_cas = aero.vtas2cas(intent_v_tas, allow_h)
allow_v_cas = np.where((intent_v_cas < self.vmin), self.vmin,
intent_v_cas)
allow_v_cas = np.where(intent_v_cas > self.vmax, self.vmax,
allow_v_cas)
allow_v_tas = aero.vcas2tas(allow_v_cas, allow_h)
vs_max_with_acc = (1 - ax / self.axmax) * self.vsmax
allow_vs = np.where((intent_vs > 0) & (intent_vs > self.vsmax),
vs_max_with_acc,
intent_vs) # for climb with vs larger than vsmax
allow_vs = np.where(
(intent_vs < 0) & (intent_vs < self.vsmin), vs_max_with_acc,
allow_vs) # for descent with vs smaller than vsmin (negative)
allow_vs = np.where(
(self.phase == ph.GD) & (bs.traf.tas < self.vminto), 0,
allow_vs) # takeoff aircraft
return allow_v_tas, allow_vs, allow_h
def limits(self, intent_v_tas, intent_vs, intent_h, ax):
""" apply limits on indent speed, vertical speed, and altitude (called in pilot module)"""
super(OpenAP, self).limits(intent_v_tas, intent_vs, intent_h)
# if isinstance(self.vmin, np.ndarray):
# pass
# else:
# self.update()
allow_h = np.where(intent_h > self.hmax, self.hmax, intent_h)
intent_v_cas = aero.vtas2cas(intent_v_tas, allow_h)
allow_v_cas = np.where(intent_v_cas < self.vmin, self.vmin,
intent_v_cas)
allow_v_cas = np.where(intent_v_cas > self.vmax, self.vmax,
allow_v_cas)
allow_v_tas = aero.vcas2tas(allow_v_cas, allow_h)
vs_max_with_acc = (1 - ax / self.axmax) * self.vsmax
allow_vs = np.where(intent_vs > self.vsmax, vs_max_with_acc, intent_vs)
allow_vs = np.where(intent_vs < self.vsmin, self.vsmin, allow_vs)
# print(intent_h, allow_h, self.hmax)
# print(intent_v_cas, allow_v_cas, self.vmin, self.vmax)
# print(self.coeff.limits_rotor['EC35'])
return allow_v_tas, allow_vs, allow_h
def currentlimits(self, id=None):
"""Get current kinematic performance envelop.
Args:
id (int or 1D-array): Aircraft ID(s). Defualt to None (all aircraft).
Returns:
floats or 1D-arrays: Min TAS, Max TAS, Min VS, Max VS
"""
vtasmin = aero.vcas2tas(self.vmin, bs.traf.alt)
vtasmax = np.minimum(aero.vcas2tas(self.vmax, bs.traf.alt),
aero.vmach2tas(self.mmo, bs.traf.alt))
if id is not None:
return vtasmin[id], vtasmax[id], self.vsmin[id], self.vsmax[id]
else:
return vtasmin, vtasmax, self.vsmin, self.vsmax
def limits(self, intent_v_tas, intent_vs, intent_h, ax):
"""apply limits on indent speed, vertical speed, and altitude (called in pilot module)
Args:
intent_v_tas (float or 1D-array): intent true airspeed
intent_vs (float or 1D-array): intent vertical speed
intent_h (float or 1D-array): intent altitude
ax (float or 1D-array): acceleration
Returns:
floats or 1D-arrays: Allowed TAS, Allowed vetical rate, Allowed altitude
"""
allow_h = np.where(intent_h > self.hmax, self.hmax, intent_h)
intent_v_cas = aero.vtas2cas(intent_v_tas, allow_h)
allow_v_cas = np.where((intent_v_cas < self.vmin), self.vmin, intent_v_cas)
allow_v_cas = np.where(intent_v_cas > self.vmax, self.vmax, allow_v_cas)
allow_v_tas = aero.vcas2tas(allow_v_cas, allow_h)
allow_v_tas = np.where(
aero.vtas2mach(allow_v_tas, allow_h) > self.mmo,
aero.vmach2tas(self.mmo, allow_h),
allow_v_tas,
) # maximum cannot exceed MMO
vs_max_with_acc = (1 - ax / self.axmax) * self.vsmax
allow_vs = np.where(
(intent_vs > 0) & (intent_vs > self.vsmax), vs_max_with_acc, intent_vs
) # for climb with vs larger than vsmax
allow_vs = np.where(
(intent_vs < 0) & (intent_vs < self.vsmin), vs_max_with_acc, allow_vs
) # for descent with vs smaller than vsmin (negative)
allow_vs = np.where(
(self.phase == ph.GD) & (bs.traf.tas < self.vminto), 0, allow_vs
) # takeoff aircraft
# corect rotercraft speed limits
ir = np.where(self.lifttype == coeff.LIFT_ROTOR)[0]
allow_v_tas[ir] = np.where(
(intent_v_tas[ir] < self.vmin[ir]), self.vmin[ir], intent_v_tas[ir]
)
allow_v_tas[ir] = np.where(
(intent_v_tas[ir] > self.vmax[ir]), self.vmax[ir], allow_v_tas[ir]
)
allow_vs[ir] = np.where(
(intent_vs[ir] < self.vsmin[ir]), self.vsmin[ir], intent_vs[ir]
)
allow_vs[ir] = np.where(
(intent_vs[ir] > self.vsmax[ir]), self.vsmax[ir], allow_vs[ir]
)
return allow_v_tas, allow_vs, allow_h
def FlightEnvelope(self):
# check for the flight envelope
bs.traf.delalt = bs.traf.selalt - bs.traf.alt # [m]
bs.traf.perf.limits() # Sets limspd_flag and limspd when it needs to be limited
# Update desired sates with values within the flight envelope
# When CAs is limited, it needs to be converted to TAS as only this TAS is used later on!
self.tas = np.where(bs.traf.limspd_flag, vcas2tas(bs.traf.limspd, bs.traf.alt), self.tas)
# Autopilot selected altitude [m]
self.alt = np.where(bs.traf.limalt_flag, bs.traf.limalt, self.alt)
# Autopilot selected vertical speed (V/S)
self.vs = np.where(bs.traf.limvs_flag, bs.traf.limvs, self.vs)
def limits(self, indent_v, indent_vs, indent_h):
""" apply limits on indent speed, vertical speed, and altitude """
super(PerfNAP, self).limits(indent_v, indent_vs, indent_h)
indent_v = aero.vtas2cas(indent_v, indent_h)
allow_v = np.where(indent_v < self.vmin, self.vmin, indent_v)
allow_v = np.where(indent_v > self.vmax, self.vmax, indent_v)
allow_v = aero.vcas2tas(indent_v, indent_h)
allow_vs = np.where(indent_vs < self.vsmin, self.vsmin, indent_vs)
allow_vs = np.where(indent_vs > self.vsmax, self.vsmax, indent_vs)
allow_h = np.where(indent_h > self.hmaxalt, self.hmaxalt, indent_h)
return allow_v, allow_vs, allow_h
def limits(self, indent_v, indent_vs, indent_h):
""" apply limits on indent speed, vertical speed, and altitude """
super(PerfNAP, self).limits(indent_v, indent_vs, indent_h)
indent_v = aero.vtas2cas(indent_v, indent_h)
allow_v = np.where(indent_v < self.vmin, self.vmin, indent_v)
allow_v = np.where(indent_v > self.vmax, self.vmax, indent_v)
allow_v = aero.vcas2tas(indent_v, indent_h)
allow_vs = np.where(indent_vs < self.vsmin, self.vsmin, indent_vs)
allow_vs = np.where(indent_vs > self.vsmax, self.vsmax, indent_vs)
allow_h = np.where(indent_h > self.hmaxalt, self.hmaxalt, indent_h)
return allow_v, allow_vs, allow_h
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 FlightEnvelope(self):
# check for the flight envelope
bs.traf.delalt = bs.traf.selalt - bs.traf.alt # [m]
bs.traf.perf.limits(
) # Sets limspd_flag and limspd when it needs to be limited
# Update desired sates with values within the flight envelope
# When CAs is limited, it needs to be converted to TAS as only this TAS is used later on!
self.tas = np.where(bs.traf.limspd_flag,
vcas2tas(bs.traf.limspd, bs.traf.alt), self.tas)
# Autopilot selected altitude [m]
self.alt = np.where(bs.traf.limalt_flag, bs.traf.limalt, self.alt)
# Autopilot selected vertical speed (V/S)
self.vs = np.where(bs.traf.limvs_flag, bs.traf.limvs, self.vs)
def applylimits(self):
# check for the flight envelope
if settings.performance_model == 'openap':
self.tas, self.vs, self.alt = bs.traf.perf.limits(self.tas, self.vs, self.alt, bs.traf.ax)
else:
bs.traf.perf.limits() # Sets limspd_flag and limspd when it needs to be limited
# Update desired sates with values within the flight envelope
# When CAs is limited, it needs to be converted to TAS as only this TAS is used later on!
self.tas = np.where(bs.traf.limspd_flag, vcas2tas(bs.traf.limspd, bs.traf.alt), self.tas)
# Autopilot selected altitude [m]
self.alt = np.where(bs.traf.limalt_flag, bs.traf.limalt, self.alt)
# Autopilot selected vertical speed (V/S)
self.vs = np.where(bs.traf.limvs_flag, bs.traf.limvs, self.vs)
def limits(self, intent_v, intent_vs, intent_h, ax):
"""FLIGHT ENVELPOE"""
# summarize minimum speeds - ac in ground mode might be pushing back
self.vmin = (self.phase == 1) * self.vmto + (self.phase == 2) * self.vmic + (self.phase == 3) * self.vmcr + \
(self.phase == 4) * self.vmap + (self.phase == 5) * self.vmld + (self.phase == 6) * -10.
# maximum altitude: hmax/act = MIN[hmo, hmax+gt*(dtemp-ctc1)+gw*(mmax-mact)]
# or hmo if hmx ==0 ()
# at the moment just ISA atmosphere, dtemp = 0
c1 = self.dtemp - self.ctct1
# if c1<0: c1 = 0
# values above 0 remain, values below are replaced through 0
c1m = np.array(c1 < 0) * 0.00000000000001
c1def = np.maximum(c1, c1m)
self.hact = self.hmax + self.gt * c1def + self.gw * (self.mmax -
self.mass)
# if hmax in OPF File ==0: hmaxact = hmo, else minimum(hmo, hmact)
self.hmaxact = (self.hmax == 0) * self.hmo + (
self.hmax != 0) * np.minimum(self.hmo, self.hact)
# forwarding to tools
self.limspd, self.limspd_flag, self.limalt, \
self.limalt_flag, self.limvs, self.limvs_flag = calclimits(
vtas2cas(intent_v, bs.traf.alt), bs.traf.gs,
self.vmto, self.vmin, self.vmo, self.mmo,
bs.traf.M, bs.traf.alt, self.hmaxact,
intent_h, intent_vs, self.maxthr,
self.thrust, self.D, bs.traf.tas,
self.mass, self.ESF, self.phase)
# Update desired sates with values within the flight envelope
# When CAS is limited, it needs to be converted to TAS as only this TAS is used later on!
allowed_tas = np.where(self.limspd_flag,
vcas2tas(self.limspd, bs.traf.alt), intent_v)
# Autopilot selected altitude [m]
allowed_alt = np.where(self.limalt_flag, self.limalt, intent_h)
# Autopilot selected vertical speed (V/S)
allowed_vs = np.where(self.limvs_flag, self.limvs, intent_vs)
return allowed_tas, allowed_vs, allowed_alt
def limits(self, intent_v_tas, intent_vs, intent_h, ax):
""" apply limits on indent speed, vertical speed, and altitude (called in pilot module)"""
super(OpenAP, self).limits(intent_v_tas, intent_vs, intent_h)
allow_h = np.where(intent_h > self.hmax, self.hmax, intent_h)
intent_v_cas = aero.vtas2cas(intent_v_tas, allow_h)
allow_v_cas = np.where((intent_v_cas < self.vmin), self.vmin, intent_v_cas)
allow_v_cas = np.where(intent_v_cas > self.vmax, self.vmax, allow_v_cas)
allow_v_tas = aero.vcas2tas(allow_v_cas, allow_h)
vs_max_with_acc = (1 - ax / self.axmax) * self.vsmax
allow_vs = np.where((intent_vs > 0) & (intent_vs>self.vsmax), vs_max_with_acc, intent_vs) # for climb with vs larger than vsmax
allow_vs = np.where((intent_vs < 0) & (intent_vs<self.vsmin), vs_max_with_acc, allow_vs) # for descent with vs smaller than vsmin (negative)
allow_vs = np.where((self.phase==ph.GD) & (bs.traf.tas < self.vminto), 0, allow_vs) # takeoff aircraft
return allow_v_tas, allow_vs, allow_h
def limits(self, indent_v, indent_vs, indent_h):
""" apply limits on indent speed, vertical speed, and altitude """
super(PerfNAP, self).limits(indent_v, indent_vs, indent_h)
# if isinstance(self.vmin, np.ndarray):
# pass
# else:
# self.update()
indent_v = aero.vtas2cas(indent_v, indent_h)
allow_v = np.where(indent_v < self.vmin, self.vmin, indent_v)
allow_v = np.where(indent_v > self.vmax, self.vmax, indent_v)
allow_v = aero.vcas2tas(indent_v, indent_h)
allow_vs = np.where(indent_vs < self.vsmin, self.vsmin, indent_vs)
allow_vs = np.where(indent_vs > self.vsmax, self.vsmax, indent_vs)
allow_h = np.where(indent_h > self.hmaxalt, self.hmaxalt, indent_h)
return allow_v, allow_vs, allow_h
def limits(self, intent_v, intent_vs, intent_h, ax):
"""Flight envelope""" # Connect this with function limits in performance.py
# combine minimum speeds and flight phases. Phases initial climb, cruise
# and approach use the same CLmax and thus the same function for Vmin
self.vmto = self.vm_to * np.sqrt(self.mass / bs.traf.rho)
self.vmic = np.sqrt(2 * self.mass * g0 /
(bs.traf.rho * self.clmaxcr * self.Sref))
self.vmcr = self.vmic
self.vmap = self.vmic
self.vmld = self.vm_ld * np.sqrt(self.mass / bs.traf.rho)
# summarize and convert to cas
# note: aircraft on ground may be pushed back
self.vmin = (self.phase==1)*vtas2cas(self.vmto, bs.traf.alt) + \
((self.phase==2) + (self.phase==3) + (self.phase==4))*vtas2cas(self.vmcr, bs.traf.alt) + \
(self.phase==5)*vtas2cas(self.vmld, bs.traf.alt) + (self.phase==6)*-10.0
# forwarding to tools
self.limspd, self.limspd_flag, self.limalt, \
self.limalt_flag, self.limvs, self.limvs_flag = calclimits(
vtas2cas(intent_v, bs.traf.alt), bs.traf.gs, \
self.vmto, self.vmin, self.vmo, self.mmo, \
bs.traf.M, bs.traf.alt, self.hmaxact, \
intent_h, intent_vs, self.maxthr, \
self.thrust_pilot, self.D, bs.traf.tas, \
self.mass, self.ESF, self.phase)
# Update desired sates with values within the flight envelope
# When CAS is limited, it needs to be converted to TAS as only this TAS is used later on!
allowed_tas = np.where(self.limspd_flag,
vcas2tas(self.limspd, bs.traf.alt), intent_v)
# Autopilot selected altitude [m]
allowed_alt = np.where(self.limalt_flag, self.limalt, intent_h)
# Autopilot selected vertical speed (V/S)
allowed_vs = np.where(self.limvs_flag, self.limvs, intent_vs)
return allowed_tas, allowed_vs, allowed_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])
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]
# 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/decel distance for next leg or turn
# Actual distance it takes to decelerate
# Normally next leg speed (actwp.spd) but in case we fly turns with a specified turn speed
# use the turn speed
swturnspd = bs.traf.actwp.flyturn*(bs.traf.actwp.turnspd>0.001)
nexttas = np.where(swturnspd,
vcas2tas(bs.traf.actwp.turnspd,bs.traf.alt),
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)
dxspdchg = swturnspd*bs.traf.actwp.turndist + np.sign(tasdiff)/np.maximum(0.01,np.abs(bs.traf.ax)) * \
(bs.traf.tas+0.5*tasdiff)
# Check also whether VNAVSPD is on, if not, SPD SEL has override for next leg
# and same for turn logic
usespdcon = (dist2wp < dxspdchg)*(bs.traf.actwp.spdcon > -990.) * \
bs.traf.swvnavspd*bs.traf.swvnav
bs.traf.selspd = np.where(usespdcon, np.where(swturnspd,bs.traf.actwp.turnspd,
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 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):
# 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
print(t_leg)
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(' ')
return t_total
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 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
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) # 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]
#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, 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))
# dt = dv/a ; dx = v*dt + 0.5*a*dt2 => dx = dv/a * (v + 0.5*dv)
ax = bs.traf.perf.acceleration()
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
nexttas = vcasormach2tas(bs.traf.actwp.nextspd,bs.traf.alt)
tasdiff = (nexttas - bs.traf.tas)*(bs.traf.actwp.spd>=0.) # [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(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
usenextspdcon = (dist2wp < dxspdconchg)*(bs.traf.actwp.nextspd> -990.) * \
bs.traf.swvnavspd*bs.traf.swvnav*bs.traf.swlnav
useturnspd = np.logical_or(bs.traf.actwp.turntonextwp,(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.), -999.,
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)
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])
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 ========================
# Note that the code below is vectorized, with traffic arrays, so for all aircraft
# ComputeVNAV and inside waypoint loop of update_fms, it was scalar (per a/c with index i)
# VNAV guidance logic (using the variables prepared by ComputeVNAV when activating waypoint)
#
# Central question is:
# - Can we please we start to descend or to climb?
#
# Well, when Top of Descent (ToD) switch is on, descend as late as possible,
# But when Top of Climb switch is on or off, climb as soon as possible, only difference is steepness used in ComputeVNAV
# to calculate bs.traf.actwp.vs
# Only use this logic if there is a valid next altitude constraint (nextaltco)
startdescorclimb = (bs.traf.actwp.nextaltco>=-0.1) * \
np.logical_or(self.swtod * (bs.traf.alt>bs.traf.actwp.nextaltco) * (self.dist2wp < self.dist2vs),
bs.traf.alt<bs.traf.actwp.nextaltco)
#print("self.dist2vs =",self.dist2vs)
# 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, startdescorclimb,
self.dist2wp <= np.maximum(0.1 * nm, 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?
# Now done in ComputeVNAV
# See ComputeVNAV for bs.traf.actwp.vs calculation
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)
# for VNAV use fixed V/S and change start of descent
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.nextturnspd > 0.0,
vcas2tas(bs.traf.actwp.nextturnspd, bs.traf.alt),
-1.0 + 0. * bs.traf.tas)
# Switch is now whether the aircraft has any turn waypoints
swturnspd = bs.traf.actwp.nextturnidx > 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.perf.vmax,
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)
qdrturn, dist2turn = geo.qdrdist(bs.traf.lat, bs.traf.lon,
bs.traf.actwp.nextturnlat,
bs.traf.actwp.nextturnlon)
self.qdrturn = qdrturn
dist2turn = dist2turn * nm
# Where we don't have a turn waypoint, as in turn idx is negative, then put distance
# as Earth circumference.
self.dist2turn = np.where(bs.traf.actwp.nextturnidx > 0, dist2turn,
40075000)
# 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.dist2turn < (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 = bs.traf.swlnav * 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.nextturnspd,
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)
self.inturn = np.logical_or(useturnspd, inoldturn)
#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 show_vminmax(self):
return {
'vmin': aero.vcas2tas(self.vmin, bs.traf.alt),
'vmax': aero.vcas2tas(self.vmax, bs.traf.alt)
}
