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 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 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