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 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, 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, 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.nextaltco[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
# 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)
#=============== 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.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 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 to next alt constraint
t2go2alt = np.maximum(0.,(dist2wp + bs.traf.actwp.xtoalt - bs.traf.actwp.turndist*nm)) \
/ np.maximum(0.5,bs.traf.gs)
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))
dxspdchg = 0.5 * np.sign(tasdiff) * np.abs(
bs.traf.ax) * dtspdchg * dtspdchg + bs.traf.tas * dtspdchg
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, 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 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
oldspd = bs.traf.actwp.spd[i] # Save speed as specified for the waypoint we pass
# 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
# Update qdr and turndist for this new waypoint for ComputeVNAV
dummy, qdr[i] = 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])
#=============== 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]
#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]
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])
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 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)
