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 selspd(self, idx, casmach): # SPD command
""" Select speed command: SPD acid, casmach (= CASkts/Mach) """
if idx<0 or idx>=bs.traf.ntraf:
return False,"SPD: Aircraft does not exist"
# convert input speed to cas and mach depending on the magnitude of input
if 0.0<= casmach <= 2.0:
bs.traf.aspd[idx] = mach2cas(casmach, bs.traf.alt[idx])
bs.traf.ama[idx] = casmach
else:
bs.traf.aspd[idx] = casmach
bs.traf.ama[idx] = cas2mach(casmach, bs.traf.alt[idx])
# Switch off VNAV: SPD command overrides
bs.traf.swvnav[idx] = False
return True
def selspdcmd(self, idx, casmach): # SPD command
""" Select speed command: SPD acid, casmach (= CASkts/Mach) """
if idx < 0 or idx >= bs.traf.ntraf:
return False, "SPD: Aircraft does not exist"
# Depending on or position relative to crossover altitude,
# we will maintain CAS or Mach when altitude changes
# We will convert values when needed
if bs.traf.abco[idx] and casmach > 2.0:
bs.traf.selspd[idx] = cas2mach(casmach, bs.traf.alt[idx])
elif bs.traf.belco[idx] and casmach <= 2.0:
bs.traf.selspd[idx] = mach2cas(casmach, bs.traf.alt[idx])
else: # User uses correct Mach/CAS
bs.traf.selspd[idx] = casmach
# Switch off VNAV: SPD command overrides
bs.traf.swvnav[idx] = False
return True
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(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 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_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, 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
