def update_fms(self, qdr, dist):
# Shift waypoints for aircraft i where necessary
for i in bs.traf.actwp.Reached(qdr, dist, bs.traf.actwp.flyby,
bs.traf.actwp.flyturn,bs.traf.actwp.turnrad):
# Save current wp speed for use on next leg when we pass this waypoint
# VNAV speeds are always FROM-speed, so we accelerate/decellerate at the waypoint
# where this speed is specified, so we need to save it for use now
# before getting the new data for the next waypoint
# Get speed for next leg from the waypoint we now
bs.traf.actwp.spd[i] = bs.traf.actwp.nextspd[i]
bs.traf.actwp.spdcon[i] = bs.traf.actwp.nextspd[i]
# Use turnradius of passing wp for bank angle
if bs.traf.actwp.flyturn[i] and bs.traf.actwp.turnrad[i]>0.:
if bs.traf.actwp.turnspd[i]>0.:
turnspd = bs.traf.actwp.turnspd[i]
else:
turnspd = bs.traf.tas[i]
bs.traf.aphi[i] = atan(turnspd*turnspd/(bs.traf.actwp.turnrad[i]*nm*g0)) # [rad]
else:
bs.traf.aphi[i] = 0.0 #[rad] or leave untouched???
# Get next wp (lnavon = False if no more waypoints)
lat, lon, alt, bs.traf.actwp.nextspd[i], bs.traf.actwp.xtoalt[i], toalt, \
bs.traf.actwp.xtorta[i], bs.traf.actwp.torta[i], \
lnavon, flyby, flyturn, turnrad, turnspd,\
bs.traf.actwp.next_qdr[i] = \
self.route[i].getnextwp() # note: xtoalt,toalt in [m]
# End of route/no more waypoints: switch off LNAV
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 not bs.traf.swlnav[i]:
bs.traf.actwp.spd[i] = -999.
# VNAV spd mode: use speed of this waypoint as commanded speed
# while passing waypoint and save next speed for passing next wp
# Speed is now from speed! Next speed is ready in wpdata
if bs.traf.swvnavspd[i] and bs.traf.actwp.spd[i]> 0.0:
bs.traf.selspd[i] = bs.traf.actwp.spd[i]
# Update qdr and turndist for this new waypoint for ComputeVNAV
qdr[i], 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 works, is there a next leg direction or not?
if bs.traf.actwp.next_qdr[i] < -900.:
local_next_qdr = qdr[i]
else:
local_next_qdr = bs.traf.actwp.next_qdr[i]
bs.traf.actwp.flyturn[i] = flyturn
bs.traf.actwp.turnrad[i] = turnrad
bs.traf.actwp.turnspd[i] = turnspd
# Calculate turn dist (and radius which we do not use) now for scalar variable [i]
bs.traf.actwp.turndist[i], dummy = \
bs.traf.actwp.calcturn(bs.traf.tas[i], bs.traf.bank[i],
qdr[i], local_next_qdr,turnrad) # update turn distance for VNAV
# VNAV = FMS ALT/SPD mode incl. RTA
self.ComputeVNAV(i, toalt, bs.traf.actwp.xtoalt[i], bs.traf.actwp.torta[i],
bs.traf.actwp.xtorta[i])
# Check deceleration distance before next wp in case of turns
bs.traf.actwp.deceldist[i] = np.where(flyturn,
(bs.traf.tas[i] - cas2tas(turnspd, bs.traf.alt[i])) * 0.5 *
(bs.traf.tas[i] - cas2tas(turnspd, bs.traf.alt[i])) / max(0.001, bs.traf.ax[i])
, 0.0)
# Continuous guidance when speed constraint on active leg
# If still an RTA in the route and currently no speed constraint
for iac in np.where((bs.traf.actwp.torta > -99.)*(bs.traf.actwp.spdcon<0.0))[0]:
iwp = bs.traf.ap.route[iac].iactwp
if bs.traf.ap.route[iac].wprta[iwp]>-99.:
# For all a/c flying to an RTA waypoint, recalculate speed more often
dist2go4rta = geo.kwikdist(bs.traf.lat[iac],bs.traf.lon[iac], \
bs.traf.actwp.lat[iac],bs.traf.actwp.lon[iac])*nm \
+ bs.traf.ap.route[iac].wpxtorta[iwp] # last term zero for active wp rta
# Set bs.traf.actwp.spd to rta speed, if necessary
self.setspeedforRTA(iac,bs.traf.actwp.torta[iac],dist2go4rta)
# If VNAV speed is on (by default coupled to VNAV), use it for speed guidance
if bs.traf.swvnavspd[iac]:
bs.traf.selspd[iac] = bs.traf.actwp.spd[iac]
def update_fms(self, qdr, dist):
# Shift waypoints for aircraft i where necessary
for i in bs.traf.actwp.Reached(qdr, dist, bs.traf.actwp.flyby,
bs.traf.actwp.flyturn,bs.traf.actwp.turnrad):
# Save current wp speed for use on next leg when we pass this waypoint
# VNAV speeds are always FROM-speed, so we accelerate/decellerate at the waypoint
# where this speed is specified, so we need to save it for use now
# before getting the new data for the next waypoint
# Get speed for next leg from the waypoint we pass now
bs.traf.actwp.spd[i] = bs.traf.actwp.nextspd[i]
bs.traf.actwp.spdcon[i] = bs.traf.actwp.nextspd[i]
# Use turnradius of passing wp for bank angle
if bs.traf.actwp.flyturn[i] and bs.traf.actwp.turnrad[i]>0.:
if bs.traf.actwp.turnspd[i]>0.:
turnspd = bs.traf.actwp.turnspd[i]
else:
turnspd = bs.traf.tas[i]
bs.traf.aphi[i] = atan(turnspd*turnspd/(bs.traf.actwp.turnrad[i]*nm*g0)) # [rad]
else:
bs.traf.aphi[i] = 0.0 #[rad] or leave untouched???
# Get next wp (lnavon = False if no more waypoints)
lat, lon, alt, bs.traf.actwp.nextspd[i], bs.traf.actwp.xtoalt[i], toalt, \
bs.traf.actwp.xtorta[i], bs.traf.actwp.torta[i], \
lnavon, flyby, flyturn, turnrad, turnspd,\
bs.traf.actwp.next_qdr[i] = \
self.route[i].getnextwp() # note: xtoalt,toalt in [m]
# End of route/no more waypoints: switch off LNAV using the lnavon
# switch returned by getnextwp
if not lnavon and bs.traf.swlnav[i]:
bs.traf.swlnav[i] = False
# Last wp: copy last wp values for alt and speed in autopilot
if bs.traf.swvnavspd[i] and bs.traf.actwp.nextspd[i]>= 0.0:
bs.traf.selspd[i] = bs.traf.actwp.nextspd[i]
# In case of no LNAV, do not allow VNAV mode on its own
bs.traf.swvnav[i] = bs.traf.swvnav[i] and bs.traf.swlnav[i]
bs.traf.actwp.lat[i] = lat # [deg]
bs.traf.actwp.lon[i] = lon # [deg]
# 1.0 in case of fly by, else fly over
bs.traf.actwp.flyby[i] = int(flyby)
# User has entered an altitude for this waypoint
if alt >= -0.01:
bs.traf.actwp.nextaltco[i] = alt # [m]
if not bs.traf.swlnav[i]:
bs.traf.actwp.spd[i] = -999.
# VNAV spd mode: use speed of this waypoint as commanded speed
# while passing waypoint and save next speed for passing next wp
# Speed is now from speed! Next speed is ready in wpdata
if bs.traf.swvnavspd[i] and bs.traf.actwp.spd[i]> 0.0:
bs.traf.selspd[i] = bs.traf.actwp.spd[i]
# Update qdr and turndist for this new waypoint for ComputeVNAV
qdr[i], distnmi = geo.qdrdist(bs.traf.lat[i], bs.traf.lon[i],
bs.traf.actwp.lat[i], bs.traf.actwp.lon[i])
dist[i] = distnmi*nm
# Update turndist so ComputeVNAV works, is there a next leg direction or not?
if bs.traf.actwp.next_qdr[i] < -900.:
local_next_qdr = qdr[i]
else:
local_next_qdr = bs.traf.actwp.next_qdr[i]
# Get flyturn switches and data
bs.traf.actwp.flyturn[i] = flyturn
bs.traf.actwp.turnrad[i] = turnrad
# Pass on whether currently flyturn mode:
# at beginning of leg,c copy tonextwp to lastwp
# set next turn False
bs.traf.actwp.turnfromlastwp[i] = bs.traf.actwp.turntonextwp[i]
bs.traf.actwp.turntonextwp[i] = False
# Keep both turning speeds: turn to leg and turn from leg
bs.traf.actwp.oldturnspd[i] = bs.traf.actwp.turnspd[i] # old turnspd, turning by this waypoint
if bs.traf.actwp.flyturn[i]:
bs.traf.actwp.turnspd[i] = turnspd # new turnspd, turning by next waypoint
else:
bs.traf.actwp.turnspd[i] = -990.
# Calculate turn dist (and radius which we do not use) now for scalar variable [i]
bs.traf.actwp.turndist[i], dummy = \
bs.traf.actwp.calcturn(bs.traf.tas[i], bs.traf.bank[i],
qdr[i], local_next_qdr,turnrad) # update turn distance for VNAV
# Reduce turn dist for reduced turnspd
if bs.traf.actwp.flyturn[i] and bs.traf.actwp.turnrad[i]<0.0 and bs.traf.actwp.turnspd[i]>0.:
turntas = cas2tas(bs.traf.actwp.turnspd[i], bs.traf.alt[i])
bs.traf.actwp.turndist[i] = bs.traf.actwp.turndist[i]*turntas*turntas/(bs.traf.tas[i]*bs.traf.tas[i])
# VNAV = FMS ALT/SPD mode incl. RTA
self.ComputeVNAV(i, toalt, bs.traf.actwp.xtoalt[i], bs.traf.actwp.torta[i],
bs.traf.actwp.xtorta[i])
# End of per waypoint i switching loop
# Update qdr2wp with up-to-date qdr, now that we have checked passing wp
self.qdr2wp = qdr%360.
# Continuous guidance when speed constraint on active leg is in update-method
# If still an RTA in the route and currently no speed constraint
for iac in np.where((bs.traf.actwp.torta > -99.)*(bs.traf.actwp.spdcon<0.0))[0]:
iwp = bs.traf.ap.route[iac].iactwp
if bs.traf.ap.route[iac].wprta[iwp]>-99.:
# For all a/c flying to an RTA waypoint, recalculate speed more often
dist2go4rta = geo.kwikdist(bs.traf.lat[iac],bs.traf.lon[iac], \
bs.traf.actwp.lat[iac],bs.traf.actwp.lon[iac])*nm \
+ bs.traf.ap.route[iac].wpxtorta[iwp] # last term zero for active wp rta
# Set bs.traf.actwp.spd to rta speed, if necessary
self.setspeedforRTA(iac,bs.traf.actwp.torta[iac],dist2go4rta)
# If VNAV speed is on (by default coupled to VNAV), use it for speed guidance
if bs.traf.swvnavspd[iac]:
bs.traf.selspd[iac] = bs.traf.actwp.spd[iac]