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