Exemplo n.º 1
0
class Traffic:
    """
    Traffic class definition    : Traffic data

    Methods:
        Traffic()            :  constructor
        reset()              :  Reset traffic database w.r.t a/c data

        create(acid,actype,aclat,aclon,achdg,acalt,acspd) : create aircraft
        delete(acid)         : delete an aircraft from traffic data
        deletall()           : delete all traffic
        update(sim)          : do a numerical integration step
        id2idx(name)         : return index in traffic database of given call sign
        selhdg(i,hdg)        : set autopilot heading and activate heading select mode
        selspd(i,spd)        : set autopilot CAS/Mach and activate heading select mode

        engchange(i,engtype) : change engine type of an aircraft

        changeTrailColor(color,idx)     : change colour of trail of aircraft idx

        setNoise(A)          : Add turbulence

    Members: see create

    Created by  : Jacco M. Hoekstra
    """

    def __init__(self, navdb):
        # ASAS object
        self.asas = ASAS()

        # All traffic data is initialized in the reset function
        self.reset(navdb)

    def reset(self, navdb):
        #  model-specific parameters.
        # Default: BlueSky internal performance model.
        # Insert your BADA files to the folder "BlueSky/data/coefficients/BADA"
        # for working with EUROCONTROL`s Base of Aircraft Data revision 3.12

        self.perf = Perf(self)

        self.ntraf = 0

        # Traffic list & arrays definition

        # !!!IMPORTANT NOTE!!!
        # Any variables added here should also be added in the Traffic
        # methods self.create() (append) and self.delete() (delete)
        # which can be found directly below __init__

        # Traffic basic flight data

        # Traffic basic flight data
        self.id     = []  # identifier (string)
        self.type   = []  # aircaft type (string)
        self.lat    = np.array([])  # latitude [deg]
        self.lon    = np.array([])  # longitude [deg]
        self.trk    = np.array([])  # track angle [deg]
        self.tas    = np.array([])  # true airspeed [m/s]
        self.gs     = np.array([])  # ground speed [m/s]
        self.cas    = np.array([])  # calibrated airspeed [m/s]
        self.M      = np.array([])  # mach number
        self.alt    = np.array([])  # altitude [m]
        self.fll    = np.array([])  # flight level [ft/100]
        self.vs     = np.array([])  # vertical speed [m/s]
        self.p      = np.array([])  # atmospheric air pressure [N/m2]
        self.rho    = np.array([])  # atmospheric air density [kg/m3]
        self.Temp   = np.array([])  # atmospheric air temperature [K]
        self.dtemp  = np.array([])  # delta t for non-ISA conditions

        # Traffic performance data
        self.avsdef = np.array([])  # [m/s]default vertical speed of autopilot
        self.aphi   = np.array([])  # [rad] bank angle setting of autopilot
        self.ax     = np.array([])  # [m/s2] absolute value of longitudinal accelleration
        self.bank   = np.array([])  # nominal bank angle, [radian]
        self.bphase = np.array([])  # standard bank angles per phase
        self.hdgsel = np.array([])  # determines whether aircraft is turning

        # Help variables to save computation time
        self.coslat = np.array([])  # Cosine of latitude for flat-earth aproximations

        # Crossover altitude
        self.abco   = np.array([])
        self.belco  = np.array([])

        # Traffic autopilot settings
        self.ahdg   = []  # selected heading [deg]
        self.aspd   = []  # selected spd(CAS) [m/s]
        self.aptas  = []  # just for initializing
        self.ama    = []  # selected spd above crossover altitude (Mach) [-]
        self.apalt  = []  # selected alt[m]
        self.apfll  = []  # selected fl [ft/100]
        self.avs    = []  # selected vertical speed [m/s]

        # limit settings
        self.limspd   = []  # limit speed
        self.limalt   = []  # limit altitude
        self.limvs    = []  # limit vertical speed due to thrust limitation

        # Traffic navigation information
        self.orig   = []  # Four letter code of origin airport
        self.dest   = []  # Four letter code of destination airport

        # LNAV route navigation
        self.swlnav = np.array([])  # Lateral (HDG) based on nav?
        self.swvnav = np.array([])  # Vertical/longitudinal (ALT+SPD) based on nav info

        self.actwplat  = np.array([])  # Active WP latitude
        self.actwplon  = np.array([])  # Active WP longitude
        self.actwpalt  = np.array([])  # Active WP altitude to arrive at
        self.actwpspd  = np.array([])  # Active WP speed
        self.actwpturn = np.array([])  # Distance when to turn to next waypoint
        self.actwpflyby = np.array([])  # Distance when to turn to next waypoint

        # VNAV variablescruise level
        self.crzalt  = np.array([])    # Cruise altitude[m]
        self.dist2vs = np.array([])    # Distance to start V/S of VANAV
        self.actwpvs = np.array([])    # Actual V/S to use

        # Route info
        self.route = []

        self.desalt     = np.array([])  # desired altitude [m]
        self.deshdg     = np.array([])  # desired heading
        self.desvs      = np.array([])  # desired vertical speed [m/s]
        self.desspd     = np.array([])  # desired speed [m/s]

        # Display information on label
        self.label      = []  # Text and bitmap of traffic label
        self.trailcol   = []  # Trail color: default 'Blue'

        # Transmitted data to other aircraft due to truncated effect
        self.adsbtime   = np.array([])
        self.adsblat    = np.array([])
        self.adsblon    = np.array([])
        self.adsbalt    = np.array([])
        self.adsbtrk    = np.array([])
        self.adsbtas    = np.array([])
        self.adsbgs     = np.array([])
        self.adsbvs     = np.array([])

        #-----------------------------------------------------------------------------
        # Not per aircraft data

        # Scheduling of FMS and ASAS
        self.t0fms = -999.  # last time fms was called
        self.dtfms = 1.01  # interval for fms

        # Flight performance scheduling
        self.perfdt = 0.1           # [s] update interval of performance limits
        self.perft0 = -self.perfdt  # [s] last time checked (in terms of simt)
        self.warned2 = False        # Flag: Did we warn for default engine parameters yet?

        # ADS-B transmission-receiver model
        self.adsb = ADSBModel(self)

        # Import navigation data base
        self.navdb  = navdb

        # Traffic area: delete traffic when it leaves this area (so not when outside)
        self.swarea     = False
        self.arealat0   = 0.0  # [deg] lower latitude defining area
        self.arealat1   = 0.0  # [deg] upper latitude defining area
        self.arealon0   = 0.0  # [deg] lower longitude defining area
        self.arealon1   = 0.0  # [deg] upper longitude defining area
        self.areafloor  = -999999.0  # [m] Delete when descending through this h
        self.areadt     = 5.0  # [s] frequency of area check (simtime)
        self.areat0     = -100.  # last time checked
        self.arearadius = 100.0  # [NM] radius of experiment area if it is a circle

        self.inside = []
        self.fir_circle_point = (0.0, 0.0)
        self.fir_circle_radius = 1.0

        # Taxi switch
        self.swtaxi = False  # Default OFF: delete traffic below 1500 ft

        # Research Area ("Square" for Square, "Circle" for Circle area)
        self.area = ""

        # Bread crumbs for trails
        self.lastlat  = []
        self.lastlon  = []
        self.lasttim  = []
        self.trails   = Trails()
        self.swtrails = False  # Default switched off

        # Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)
        self.setNoise(False)

        self.eps = np.array([])

        self.asas.reset()

    def mcreate(self, count, actype=None, alt=None, spd=None, dest=None, area=None):
        """ Create multiple random aircraft in a specified area """
        idbase = chr(randint(65, 90)) + chr(randint(65, 90))
        if actype is None:
            actype = 'B744'

        for i in xrange(count):
            acid  = idbase + '%05d' % i
            aclat = random() * (area[1] - area[0]) + area[0]
            aclon = random() * (area[3] - area[2]) + area[2]
            achdg = float(randint(1, 360))
            acalt = (randint(2000, 39000) * ft) if alt is None else alt
            acspd = (randint(250, 450) * kts) if spd is None else spd

            self.create(acid, actype, aclat, aclon, achdg, acalt, acspd)

    def create(self, acid, actype, aclat, aclon, achdg, acalt, casmach):
        """Create an aircraft"""
        # Check if not already exist
        if self.id.count(acid.upper()) > 0:
            return False, acid + " already exists."  # already exists do nothing

        # Increase number of aircraft
        self.ntraf = self.ntraf + 1

        # Convert speed
        if 0.1 < casmach < 1.0 :
            acspd = mach2tas(casmach, acalt)
        else:
            acspd = cas2tas(casmach, acalt)

        # Process input
        self.id.append(acid.upper())
        self.type.append(actype)
        self.lat   = np.append(self.lat, aclat)
        self.lon   = np.append(self.lon, aclon)
        self.trk   = np.append(self.trk, achdg)  # TBD: add conversion hdg => trk
        self.alt   = np.append(self.alt, acalt)
        self.fll   = np.append(self.fll, (acalt) / (100 * ft))
        self.vs    = np.append(self.vs, 0.)
        c_temp, c_rho, c_p = vatmos(acalt)
        self.p     = np.append(self.p, c_p)
        self.rho   = np.append(self.rho, c_rho)
        self.Temp  = np.append(self.Temp, c_temp)
        self.dtemp = np.append(self.dtemp, 0)  # at the moment just ISA conditions
        self.tas   = np.append(self.tas, acspd)
        self.gs    = np.append(self.gs, acspd)
        self.cas   = np.append(self.cas, tas2cas(acspd, acalt))
        self.M     = np.append(self.M, tas2mach(acspd, acalt))

        # AC is initialized with neutral max bank angle
        self.bank = np.append(self.bank, radians(25.))
        if self.ntraf < 2:
            self.bphase = np.deg2rad(np.array([15, 35, 35, 35, 15, 45]))
        self.hdgsel = np.append(self.hdgsel, False)

        #------------------------------Performance data--------------------------------
        # Type specific data
        #(temporarily default values)
        self.avsdef = np.append(self.avsdef, 1500. * fpm)  # default vertical speed of autopilot
        self.aphi   = np.append(self.aphi, radians(25.))  # bank angle setting of autopilot
        self.ax     = np.append(self.ax, kts)  # absolute value of longitudinal accelleration

        # Crossover altitude
        self.abco   = np.append(self.abco, 0)
        self.belco  = np.append(self.belco, 1)

        # performance data
        self.perf.create(actype)

        # Traffic autopilot settings: hdg[deg], spd (CAS,m/s), alt[m], vspd[m/s]
        self.ahdg  = np.append(self.ahdg, achdg)  # selected heading [deg]
        self.aspd  = np.append(self.aspd, tas2cas(acspd, acalt))  # selected spd(cas) [m/s]
        self.aptas = np.append(self.aptas, acspd)  # [m/s]
        self.ama   = np.append(self.ama, 0.)  # selected spd above crossover (Mach) [-]
        self.apalt = np.append(self.apalt, acalt)  # selected alt[m]
        self.apfll = np.append(self.apfll, (acalt / 100))  # selected fl[ft/100]
        self.avs   = np.append(self.avs, 0.)  # selected vertical speed [m/s]

        # limit settings: initialize with 0
        self.limspd = np.append(self.limspd, 0.0)
        self.limalt = np.append(self.limalt, 0.0)
        self.limvs  = np.append(self.limvs, 0.0)

        # Help variables to save computation time
        self.coslat = np.append(self.coslat, cos(radians(aclat)))  # Cosine of latitude for flat-earth aproximations

        # Traffic navigation information
        self.dest.append("")
        self.orig.append("")

        # LNAV route navigation
        self.swlnav = np.append(self.swlnav, False)  # Lateral (HDG) based on nav
        self.swvnav = np.append(self.swvnav, False)  # Vertical/longitudinal (ALT+SPD) based on nav info

        self.actwplat   = np.append(self.actwplat, 89.99)  # Active WP latitude
        self.actwplon   = np.append(self.actwplon, 0.0)   # Active WP longitude
        self.actwpalt   = np.append(self.actwpalt, 0.0)   # Active WP altitude
        self.actwpspd   = np.append(self.actwpspd, -999.)   # Active WP speed
        self.actwpturn  = np.append(self.actwpturn, 1.0)   # Distance to active waypoint where to turn
        self.actwpflyby = np.append(self.actwpflyby, 1.0)   # Flyby/fly-over switch

        # VNAV cruise level
        self.crzalt = np.append(self.crzalt, -999.)    # Cruise altitude[m] <0=None
        self.dist2vs = np.append(self.dist2vs, -999.)  # Distance to start V/S of VANAV
        self.actwpvs = np.append(self.actwpvs, 0.0)    # Actual V/S to use then

        # Route info
        self.route.append(Route(self.navdb))  # create empty route connected with nav databse

        eas = tas2eas(acspd, acalt)

        self.desalt  = np.append(self.desalt, acalt)
        self.desvs   = np.append(self.desvs, 0.0)
        self.desspd  = np.append(self.desspd, eas)
        self.deshdg  = np.append(self.deshdg, achdg)

        # Area variable set to False to avoid deletion upon creation outside
        self.inside.append(False)

        # Display information on label
        self.label.append(['', '', '', 0])

        # Bread crumbs for trails
        self.trailcol.append(self.trails.defcolor)
        self.lastlat = np.append(self.lastlat, aclat)
        self.lastlon = np.append(self.lastlon, aclon)
        self.lasttim = np.append(self.lasttim, 0.0)

        # Transmitted data to other aircraft due to truncated effect
        self.adsbtime   = np.append(self.adsbtime, np.random.rand(self.trunctime))
        self.adsblat    = np.append(self.adsblat, aclat)
        self.adsblon    = np.append(self.adsblon, aclon)
        self.adsbalt    = np.append(self.adsbalt, acalt)
        self.adsbtrk    = np.append(self.adsbtrk, achdg)
        self.adsbtas    = np.append(self.adsbtas, acspd)
        self.adsbgs     = np.append(self.adsbgs, acspd)
        self.adsbvs     = np.append(self.adsbvs, 0.)

        self.eps        = np.append(self.eps, 0.01)

        self.asas.create(achdg, eas, acalt)

        return True

    def delete(self, acid):
        """Delete an aircraft"""

        # Look up index of aircraft
        idx = self.id2idx(acid)

        # Do nothing if not found
        if idx < 0:
            return False

        del self.id[idx]
        del self.type[idx]

        # Traffic basic data
        self.lat    = np.delete(self.lat, idx)
        self.lon    = np.delete(self.lon, idx)
        self.trk    = np.delete(self.trk, idx)
        self.alt    = np.delete(self.alt, idx)
        self.fll    = np.delete(self.fll, idx)
        self.vs     = np.delete(self.vs, idx)
        self.tas    = np.delete(self.tas, idx)
        self.gs     = np.delete(self.gs, idx)
        self.cas    = np.delete(self.cas, idx)
        self.M      = np.delete(self.M, idx)

        self.p      = np.delete(self.p, idx)
        self.rho    = np.delete(self.rho, idx)
        self.Temp   = np.delete(self.Temp, idx)
        self.dtemp  = np.delete(self.dtemp, idx)
        self.hdgsel = np.delete(self.hdgsel, idx)
        self.bank   = np.delete(self.bank, idx)

        # Crossover altitude
        self.abco   = np.delete(self.abco, idx)
        self.belco  = np.delete(self.belco, idx)

        # Type specific data (temporarily default values)
        self.avsdef = np.delete(self.avsdef, idx)
        self.aphi   = np.delete(self.aphi, idx)
        self.ax     = np.delete(self.ax, idx)

        # performance data
        self.perf.delete(idx)

        # Traffic autopilot settings: hdg[deg], spd (CAS,m/s), alt[m], vspd[m/s]
        self.ahdg   = np.delete(self.ahdg, idx)
        self.aspd   = np.delete(self.aspd, idx)
        self.ama    = np.delete(self.ama, idx)
        self.aptas  = np.delete(self.aptas, idx)
        self.apalt  = np.delete(self.apalt, idx)
        self.apfll  = np.delete(self.apfll, idx)
        self.avs    = np.delete(self.avs, idx)

        # limit settings
        self.limspd   = np.delete(self.limspd, idx)
        self.limalt   = np.delete(self.limalt, idx)
        self.limvs    = np.delete(self.limvs, idx)

        # Help variables to save computation time
        self.coslat = np.delete(self.coslat, idx)  # Cosine of latitude for flat-earth aproximations

        # Traffic navigation variables
        del self.dest[idx]
        del self.orig[idx]

        self.swlnav = np.delete(self.swlnav, idx)
        self.swvnav = np.delete(self.swvnav, idx)

        self.actwplat   = np.delete(self.actwplat, idx)
        self.actwplon   = np.delete(self.actwplon, idx)
        self.actwpalt   = np.delete(self.actwpalt, idx)
        self.actwpspd   = np.delete(self.actwpspd, idx)
        self.actwpturn  = np.delete(self.actwpturn, idx)
        self.actwpflyby = np.delete(self.actwpflyby, idx)

        # VNAV cruise level
        self.crzalt    = np.delete(self.crzalt, idx)
        self.dist2vs   = np.delete(self.dist2vs, idx)    # Distance to start V/S of VANAV
        self.actwpvs   = np.delete(self.actwpvs, idx)    # Actual V/S to use

        # Route info
        del self.route[idx]

        self.desalt     = np.delete(self.desalt, idx)
        self.desvs      = np.delete(self.desvs, idx)
        self.desspd     = np.delete(self.desspd, idx)
        self.deshdg     = np.delete(self.deshdg, idx)

        # Metrics, area
        del self.inside[idx]

        # Traffic display data: label
        del self.label[idx]

        # Delete bread crumb data
        self.lastlat = np.delete(self.lastlat, idx)
        self.lastlon = np.delete(self.lastlon, idx)
        self.lasttim = np.delete(self.lasttim, idx)
        del self.trailcol[idx]

        # Transmitted data to other aircraft due to truncated effect
        self.adsbtime = np.delete(self.adsbtime, idx)
        self.adsblat  = np.delete(self.adsblat, idx)
        self.adsblon  = np.delete(self.adsblon, idx)
        self.adsbalt  = np.delete(self.adsbalt, idx)
        self.adsbtrk  = np.delete(self.adsbtrk, idx)
        self.adsbtas  = np.delete(self.adsbtas, idx)
        self.adsbgs   = np.delete(self.adsbgs, idx)
        self.adsbvs   = np.delete(self.adsbvs, idx)

        # Decrease number fo aircraft
        self.ntraf = self.ntraf - 1

        self.eps = np.delete(self.eps, idx)

        self.asas.delete(idx)
        return True

    def update(self, simt, simdt):
        # Update only necessary if there is traffic
        if self.ntraf == 0:
            return

        #---------------- Atmosphere ----------------
        self.p, self.rho, self.Temp = vatmos(self.alt)

        ###############################################################################
        # Debugging: add 10000 random aircraft
        #            if simt>1.0 and self.ntraf<1000:
        #                for i in range(10000):
        #                   acid="KL"+str(i)
        #                   aclat = random.random()*180.-90.
        #                   aclon = random.random()*360.-180.
        #                   achdg = random.random()*360.
        #                   acalt = (random.random()*18000.+2000.)*0.3048
        #                   self.create(acid,'B747',aclat,aclon,achdg,acalt,350.)
        #
        #################################################################################

        #-------------------- ADSB update: --------------------

        self.adsbtime = self.adsbtime + simdt
        if self.ADSBtrunc:
            ADSB_update = np.where(self.adsbtime > self.trunctime)
        else:
            ADSB_update = range(self.ntraf)

        for i in ADSB_update:
            self.adsbtime[i] = self.adsbtime[i] - self.trunctime
            self.adsblat[i]  = self.lat[i]
            self.adsblon[i]  = self.lon[i]
            self.adsbalt[i]  = self.alt[i]
            self.adsbtrk[i]  = self.trk[i]
            self.adsbtas[i]  = self.tas[i]
            self.adsbgs[i]   = self.gs[i]
            self.adsbvs[i]   = self.vs[i]

        # New version ADSB Model
        self.adsb.update()

        #------------------- ASAS update: ---------------------
        # Reset label because of colour change
        # Save old result
        
        iconf0 = np.array(self.asas.iconf)

        self.asas.update(self, simt)

        # TODO: this doesn't work anymore when asas.iconf is a list of lists
        # chnged = np.where(iconf0 != np.array(self.asas.iconf))[0]
        if settings.gui=="pygame":        
            for i in range(self.ntraf):
                if np.any(iconf0[i] != self.asas.iconf[i]):
                    self.label[i] = [" ", " ", "", " "]
    
        #-----------------  FMS GUIDANCE & NAVIGATION  ------------------
        # Scheduling: when dt has passed or restart:
        if self.t0fms + self.dtfms < simt or simt < self.t0fms:
            self.t0fms = simt

            # FMS LNAV mode:
            qdr, dist = geo.qdrdist(self.lat, self.lon, self.actwplat, self.actwplon)  # [deg][nm])

            # Check whether shift based dist [nm] is required, set closer than WP turn distance
            iwpclose = np.where(self.swlnav * (dist < self.actwpturn))[0]

            # Shift waypoints for aircraft i where necessary
            for i in iwpclose:

                # Get next wp (lnavon = False if no more waypoints)
                lat, lon, alt, spd, xtoalt, toalt, lnavon, flyby =  \
                       self.route[i].getnextwp()  # note: xtoalt,toalt in [m]

                # End of route/no more waypoints: switch off LNAV
                if not lnavon:
                    self.swlnav[i] = False  # Drop LNAV at end of route

                # In case of no LNAV, do not allow VNAV mode on it sown
                if not self.swlnav[i]:
                    self.swvnav[i] = False

                self.actwplat[i]   = lat
                self.actwplon[i]   = lon
                self.actwpflyby[i] = int(flyby)  # 1.0 in case of fly by, els fly over

                # User has entered an altitude for this waypoint

                if alt >= 0.:
                    self.actwpalt[i] = alt

                # VNAV = FMS ALT/SPD mode
                # calculated altitude is available and active
                if toalt  >= 0. and self.swvnav[i]:  # somewhere there is an altitude constraint ahead

                    # Descent VNAV mode (T/D logic)
                    if self.alt[i] > toalt + 10. * ft:

                        #Steepness dh/dx in [m/m], for now 1:3 rule of thumb
                        steepness = 3000. * ft / (10. * nm)

                        #Calculate max allowed altitude at next wp (above toalt)
                        self.actwpalt[i] = toalt + xtoalt * steepness

                        # Dist to waypoint where descent should start
                        self.dist2vs[i] = (self.alt[i] - self.actwpalt[i]) / steepness

                        # Flat earth distance to next wp
                        dy = (lat - self.lat[i])
                        dx = (lon - self.lon[i]) * self.coslat[i]
                        legdist = 60. * nm * sqrt(dx * dx + dy * dy)

                        # If descent is urgent, descent with maximum steepness
                        if legdist < self.dist2vs[i]:
                            self.apalt[i] = self.actwpalt[i]  # dial in altitude of next waypoint as calculated

                            t2go         = max(0.1, legdist) / max(0.01, self.gs[i])
                            self.actwpvs[i]  = (self.actwpalt[i] - self.alt[i]) / t2go

                        else:
                            # normal case: still time till descent starts

                            # Calculate V/s using steepness,
                            # protect against zero/invalid ground speed value
                            self.actwpvs[i] = -steepness*(self.gs[i] +   \
                                            (self.gs[i]<0.2*self.tas[i])*self.tas[i])

                    # Climb VNAV mode: climb as soon as possible (T/C logic)
                    elif self.swvnav[i] and self.alt[i]<toalt-10.*ft:

                        self.actwpalt[i] = toalt
                        self.apalt[i]    = self.actwpalt[i]  # dial in altitude of next waypoint as calculated
                        self.dist2vs[i]  = 9999.

                    # Level leg: never start V/S
                    else:
                        self.dist2vs[i] = -999.

                #No altirude defined: never start V/S
                else:
                    self.dist2vs[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 self.swvnav[i] and self.actwpspd[i]>0.0: # check mode and value

                    # Select CAS or Mach command by checking value of actwpspd
                    if self.actwpspd[i]<2.0: # Mach command

                       self.aspd[i] = mach2cas(self.actwpspd[i],self.alt[i])
                       self.ama[i]  = self.actwpspd[i]

                    else:    # CAS command
                       self.aspd[i] = self.actwpspd[i]
                       self.ama[i]  = cas2tas(spd,self.alt[i])

                if spd>0. and self.swlnav[i] and self.swvnav[i]: # Valid speed and LNAV and VNAV ap modes are on
                   self.actwpspd[i] = spd
                else:
                   self.actwpspd[i] = -999.

                # Calculate distance before waypoint where to start the turn
                # Turn radius:      R = V2 tan phi / g
                # Distance to turn: wpturn = R * tan (1/2 delhdg) but max 4 times radius
                # using default bank angle per flight phase
                turnrad = self.tas[i]*self.tas[i]/tan(self.bank[i]) /g0 /nm # [nm] 

                dy = (self.actwplat[i]-self.lat[i])
                dx = (self.actwplon[i]-self.lon[i])*self.coslat[i]
                qdr[i] = degrees(atan2(dx,dy))

                self.actwpturn[i] = self.actwpflyby[i]*                     \
                     max(3.,abs(turnrad*tan(radians(0.5*degto180(qdr[i]-    \
                     self.route[i].wpdirfrom[self.route[i].iactwp])))))  # [nm]                

            #=============== End of Waypoint switching loop ===================
            
            # VNAV Guidance
            
            # Do VNAV start of descent check
            dy = (self.actwplat-self.lat)
            dx = (self.actwplon-self.lon)*self.coslat
            dist2wp = 60.*nm*np.sqrt(dx*dx+dy*dy)
            steepness = 3000.*ft/(10.*nm)

            # VNAV AP LOGIC: descend as late as possible, climb as soon as possible
            # First term: descend when distance to next wp is descent distance
            # Second term: climb when still below altitude of next waypoint
            self.swvnavvs = self.swlnav*self.swvnav*((dist2wp<self.dist2vs) + \
                                     (self.actwpalt>self.alt))            

            self.avs = (1-self.swvnavvs)*self.avs + self.swvnavvs*steepness*self.gs
            self.aalt = (1-self.swvnavvs)*self.apalt + self.swvnavvs*self.actwpalt

            # Set headings based on swlnav
            self.ahdg = np.where(self.swlnav, qdr, self.ahdg)

        #-------------END of FMS update -------------------
      
        # NOISE: Turbulence
        if self.turbulence:
            timescale=np.sqrt(simdt)
            trkrad=np.radians(self.trk)
            
            #write turbulences in array
            turb=np.array(self.standardturbulence)
            turb=np.where(turb>1e-6,turb,1e-6)
            
            #horizontal flight direction
            turbhf=np.random.normal(0,turb[0]*timescale,self.ntraf) #[m]
            
            #horizontal wing direction
            turbhw=np.random.normal(0,turb[1]*timescale,self.ntraf) #[m]
            
            #vertical direction
            turbalt=np.random.normal(0,turb[2]*timescale,self.ntraf) #[m]
            
            #lateral, longitudinal direction
            turblat=np.cos(trkrad)*turbhf-np.sin(trkrad)*turbhw #[m]
            turblon=np.sin(trkrad)*turbhf+np.cos(trkrad)*turbhw #[m]

        else:
            turbalt=np.zeros(self.ntraf) #[m]
            turblat=np.zeros(self.ntraf) #[m]
            turblon=np.zeros(self.ntraf) #[m]


        # ASAS AP switches

        #--------- Input to Autopilot settings to follow: destination or ASAS ----------

        # desired autopilot settings due to ASAS
        self.deshdg = self.asas.asasactive*self.asas.asashdg + (1-self.asas.asasactive)*self.ahdg
        self.desspd = self.asas.asasactive*self.asas.asasspd + (1-self.asas.asasactive)*self.aspd
        self.desalt = self.asas.asasactive*self.asas.asasalt + (1-self.asas.asasactive)*self.apalt
        self.desvs  = self.asas.asasactive*self.asas.asasvsp + (1-self.asas.asasactive)*self.avs

 #-------------- Performance limits autopilot settings --------------
        # Check difference with AP settings for trafperf and autopilot
        self.delalt = self.desalt - self.alt  # [m]

        # below crossover altitude: CAS=const, above crossover altitude: Mach = const
        # aptas has to be calculated before delspd
        self.aptas = vcas2tas(self.desspd, self.alt) * self.belco   +   \
                     vmach2tas(self.ama, self.alt) * self.abco
        self.delspd = self.desspd - self.tas


        # check for the flight envelope
        self.perf.limits()

        # Update autopilot settings with values within the flight envelope

        # Autopilot selected speed setting [m/s]
        # To do: add const Mach const CAS mode
        self.aspd = (self.limspd ==0)*self.desspd + (self.limspd!=0)*self.limspd

        # Autopilot selected altitude [m] limited when necessary
        self.aalt = (self.limalt ==0)*self.desalt + (self.limalt!=0)*self.limalt

        # Autopilot selected heading
        self.ahdg = self.deshdg

        # Autopilot selected vertical speed (V/S)
        self.avs = (self.limvs==0)*self.desvs + (self.limvs!=0)*self.limvs

        # To be discussed: Following change in VNAV mode only?
        # below crossover altitude: CAS=const, above crossover altitude: MA = const
        #climb/descend above crossover: Ma = const, else CAS = const  
        #ama is fixed when above crossover
        swma = np.where(self.abco*(self.ama == 0.)) # Above cross-over
        self.ama[swma] = vcas2mach(self.aspd[swma], self.alt[swma])

        # ama is deleted when below crossover
        swma2 = np.where(self.belco*(self.ama!=0.)) # below corss-over
        self.ama[swma2] = 0. 

        #---------- Basic Autopilot  modes ----------

        # SPD HOLD/SEL mode: aspd = autopilot selected speed (first only eas)
        # for information:    

# no more ?       self.aptas = (self.actwpspd > 0.01)*self.actwpspd*self.swvnav + \
#                            np.logical_or((self.actwpspd <= 0.01),np.logical_not (self.swvnav))*self.aptas

        self.delspd = self.aptas - self.tas 
        swspdsel = np.abs(self.delspd) > 0.4  # <1 kts = 0.514444 m/s
        ax = np.minimum(abs(self.delspd / max(1e-8,simdt)), self.ax)

        self.tas = swspdsel * (self.tas + ax * np.sign(self.delspd) *  \
                                          simdt) + (1. - swspdsel) * self.aptas

        # Speed conversions
        self.cas = vtas2cas(self.tas, self.alt)
        self.gs  = self.tas
        self.M   = vtas2mach(self.tas, self.alt)

        # Update performance every self.perfdt seconds
        if abs(simt - self.perft0) >= self.perfdt:
            self.perft0 = simt
            self.perf.perf()

        # Update aircraft altitude
        self.eps = np.array(self.ntraf * [0.01])  # almost zero for misc purposes
        swaltsel = np.abs(self.aalt-self.alt) >      \
                  np.maximum(3.,np.abs(2. * simdt * np.abs(self.vs))) # 3.[m] = 10 [ft] eps alt

        self.vs = swaltsel*np.sign(self.aalt-self.alt)*       \
                    ( (1-self.swvnav)*np.abs(1500./60.*ft) +    \
                      self.swvnav*np.abs(self.avs)         )

        self.alt = swaltsel * (self.alt + self.vs * simdt) +   \
                   (1. - swaltsel) * self.aalt + turbalt

        # HDG HOLD/SEL mode: ahdg = ap selected heading
        delhdg = (self.ahdg - self.trk + 180.) % 360 - 180.  # [deg]

        # omega = np.degrees(g0 * np.tan(self.aphi) / \
        # np.maximum(self.tas, self.eps))

        # nominal bank angles per phase from BADA 3.12
        omega = np.degrees(g0 * np.tan(self.bank) / \
                           np.maximum(self.tas, self.eps))

        self.hdgsel = np.abs(delhdg) > np.abs(2. * simdt * omega)

        self.trk = (self.trk + simdt * omega * self.hdgsel * np.sign(delhdg)) % 360.

        #--------- Kinematics: update lat,lon,alt ----------
        ds = simdt * self.gs

        self.lat = self.lat +        \
                   np.degrees((ds * np.cos(np.radians(self.trk)) + turblat) \
                                         / Rearth)

        self.coslat = np.cos(np.deg2rad(self.lat))

        self.lon = self.lon +        \
                   np.degrees((ds * np.sin(np.radians(self.trk)) + turblon) \
                                         / self.coslat / Rearth) 

        # Update trails when switched on
        if self.swtrails:
            self.trails.update(simt, self.lat, self.lon,
                               self.lastlat, self.lastlon,
                               self.lasttim, self.id, self.trailcol)
        else:
            self.lastlat = self.lat
            self.lastlon = self.lon
            self.lasttim[:] = simt

        # ----------------AREA check----------------
        # Update area once per areadt seconds:
        if self.swarea and abs(simt - self.areat0) > self.areadt:
            # Update loop timer
            self.areat0 = simt
            # Check all aircraft
            i = 0
            while (i < self.ntraf):
                # Current status
                if self.area == "Square":
                    inside = self.arealat0 <= self.lat[i] <= self.arealat1 and \
                             self.arealon0 <= self.lon[i] <= self.arealon1 and \
                             self.alt[i] >= self.areafloor and \
                             (self.alt[i] >= 1500 or self.swtaxi)

                elif self.area == "Circle":

                    # delete aircraft if it is too far from the center of the circular area, or if has decended below the minimum altitude
                    distance = geo.kwikdist(self.arealat0, self.arealon0, self.lat[i], self.lon[i])  # [NM]
                    inside = distance < self.arearadius and self.alt[i] >= self.areafloor

                # Compare with previous: when leaving area: delete command
                if self.inside[i] and not inside:
                    self.delete(self.id[i])

                else:
                    # Update area status
                    self.inside[i] = inside
                    i = i + 1

        return

    def id2idx(self, acid):
        """Find index of aircraft id"""
        try:
            return self.id.index(acid.upper())
        except:
            return -1

    def setTrails(self, *args):
        """ Set trails on/off, or change trail color of aircraft """
        if type(args[0]) == bool:
            # Set trails on/off
            self.swtrails = args[0]
            if len(args) > 1:
                self.trails.dt = args[1]
            if not self.swtrails:
                self.trails.clear()
        else:
            # Change trail color
            if len(args) < 2 or args[2] not in ["BLUE", "RED", "YELLOW"]:
                return False, "Set aircraft trail color with: TRAIL acid BLUE/RED/YELLOW"
            self.changeTrailColor(args[1], args[0])

    def changeTrailColor(self, color, idx):
        """Change color of aircraft trail"""
        self.trailcol[idx] = self.trails.colorList[color]
        return

    def setNoise(self, noiseflag=None):
        """Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)"""

        if noiseflag is None:
            return True, "Noise is currently " + ("on" if self.noise else "off")
        
        self.noise              = noiseflag           # Noise/turbulence switch
        self.trunctime          = 1                   # seconds
        self.transerror         = [1, 100, 100 * ft]  # [degree,m,m] standard bearing, distance, altitude error
        self.standardturbulence = [0, 0.1, 0.1]       # m/s standard turbulence  (nonnegative)
        # in (horizontal flight direction, horizontal wing direction, vertical)

        self.turbulence     = self.noise
        self.ADSBtransnoise = self.noise
        self.ADSBtrunc      = self.noise

        return True

    def engchange(self, acid, engid):
        """Change of engines"""
        self.perf.engchange(acid, engid)
        return

    def selhdg(self, idx, hdg):  # HDG command
        """ Select heading command: HDG acid, hdg """
        # Give autopilot commands
        self.ahdg[idx]   = float(hdg)
        self.swlnav[idx] = False
        # Everything went ok!
        return True

    def selspd(self, idx, casmach):  # SPD command
        """ Select speed command: SPD acid, casmach (= CASkts/Mach) """
        # When >=1.0 it is probably CASkts else it is Mach
        if 0.1 < casmach < 1.0:
            self.aspd[idx] = mach2cas(casmach, self.alt[idx])  # Convert Mach to CAS m/s
            self.ama[idx]  = casmach
        else:
            self.aspd[idx] = casmach  # CAS m/s
            self.ama[idx]  = cas2mach(casmach, self.alt[idx])
        # Switch off VNAV: SPD command overrides
        self.swvnav[idx]   = False

        return True

    def move(self, idx, lat, lon, alt=None, hdg=None, casmach=None, vspd=None):
        self.lat[idx]      = lat
        self.lon[idx]      = lon

        if alt:
            self.alt[idx]  = alt
            self.aalt[idx] = alt

        if hdg:
            self.trk[idx]  = hdg
            self.ahdg[idx] = hdg

        if casmach:
            # Convert speed
            if 0.1 < casmach < 1.0:
                self.tas[idx]  = mach2tas(casmach, alt)
                self.aspd[idx] = mach2cas(casmach, alt)
            else:
                self.tas[idx]  = cas2tas(casmach, alt)
                self.aspd[idx] = casmach

        if vspd:
            self.vs[idx]       = vspd
            self.swvnav[idx]   = False

    def selalt(self, idx, alt, vspd=None):
        """ Select altitude command: ALT acid, alt, [vspd] """
        self.apalt[idx]    = alt
        self.apfll[idx]    = alt / (100. * ft)
        self.swvnav[idx]   = False

        # Check for optional VS argument
        if vspd:
            self.avs[idx] = vspd
        else:
            delalt        = alt - self.alt[idx]
            # Check for VS with opposite sign => use default vs
            # by setting autopilot vs to zero
            if self.avs[idx] * delalt < 0. and abs(self.avs[idx]) > 0.01:
                self.avs[idx] = 0.

    def selvspd(self, idx, vspd):
        """ Vertical speed autopilot command: VS acid vspd """
        self.avs[idx] = vspd
        # self.vs[idx] = vspd
        self.swvnav[idx] = False

    def nom(self, idx):
        """ Reset acceleration back to nominal (1 kt/s^2): NOM acid """
        self.ax[idx] = kts

    def setTaxi(self, flag):
        """ Set taxi delete flag: OFF auto deletes traffic below 1500 ft """
        self.swtaxi = flag

    def setLNAV(self, idx, flag=None):
        """ Set LNAV on or off for a specific or for all aircraft """
        if idx is None:
            # All aircraft are targeted
            self.swlnav = np.array(self.ntraf*[flag])

        elif flag is None:
            return True, (self.id[idx] + ": LNAV is " + "ON" if self.swlnav[idx] else "OFF")

        elif flag:
            route = self.route[idx]
            if route.nwp > 0:
                self.swlnav[idx] = True
                route.direct(self, idx, route.wpname[route.findact(self, idx)])
            else:
                return False, ("LNAV " + self.id[idx] + ": no waypoints or destination specified")
        else:
            self.swlnav[idx] = False

    def setVNAV(self, idx, flag=None):
        """ Set VNAV on or off for a specific or for all aircraft """
        if idx is None:
            # All aircraft are targeted
            self.swvnav = np.array(self.ntraf*[flag])

        elif flag is None:
            return True, (self.id[idx] + ": VNAV is " + "ON" if self.swvnav[idx] else "OFF")

        elif flag:
            if not self.swlnav[idx]:
                return False, (self.id[idx] + ": VNAV ON requires LNAV to be ON")

            route = self.route[idx]
            if route.nwp > 0:
                self.swvnav[idx] = True
                route.direct(self, idx, route.wpname[route.findact(self, idx)])
            else:
                return False, ("VNAV " + self.id[idx] + ": no waypoints or destination specified")
        else:
            self.swvnav[idx] = False

    def setDestOrig(self, cmd, idx, *args):
        if len(args) == 0:
            if cmd == 'DEST':
                return True, ('DEST ' + self.id[idx] + ': ' + self.dest[idx])
            else:
                return True, ('ORIG ' + self.id[idx] + ': ' + self.orig[idx])

        route = self.route[idx]
        if len(args) == 1:
            name = args[0]
            apidx = self.navdb.getapidx(name)
            if apidx < 0:
                return False, (cmd + ": Airport " + name + " not found.")
            lat = self.navdb.aplat[apidx]
            lon = self.navdb.aplon[apidx]
        else:
            name = self.id[idx] + cmd
            lat, lon = args

        if cmd == "DEST":
            self.dest[idx] = name
            iwp = route.addwpt(self, idx, self.dest[idx], route.dest,
                               lat, lon, 0.0, self.cas[idx])
            # If only waypoint: activate
            if (iwp == 0) or (self.orig[idx] != "" and route.nwp == 2):
                self.actwplat[idx] = route.wplat[iwp]
                self.actwplon[idx] = route.wplon[iwp]
                self.actwpalt[idx] = route.wpalt[iwp]
                self.actwpspd[idx] = route.wpspd[iwp]

                self.swlnav[idx] = True
                route.iactwp = iwp

            # If not found, say so
            elif iwp < 0:
                return False, (self.dest[idx] + " not found.")

        # Origin: bookkeeping only for now
        else:
            self.orig[idx] = name
            iwp = route.addwpt(self, idx, self.orig[idx], route.orig,
                               self.lat[idx], self.lon[idx], 0.0, self.cas[idx])
            if iwp < 0:
                return False, (self.orig[idx] + " not found.")

    def acinfo(self, acid):
        idx      = self.id.index(acid)
        actype   = self.type[idx]
        lat, lon = self.lat[idx], self.lon[idx]
        alt, hdg = self.alt[idx] / ft, self.trk[idx]
        cas      = tas2cas(self.tas[idx], self.alt[idx]) / kts
        tas      = self.tas[idx] / kts
        route    = self.route[idx]
        line = "Info on %s %s index = %d\n" % (acid, actype, idx) \
             + "Pos = %.2f, %.2f. Spd: %d kts CAS, %d kts TAS\n" % (lat, lon, cas, tas) \
             + "Alt = %d ft, Hdg = %d\n" % (alt, hdg)
        if self.swlnav[idx] and route.nwp > 0 and route.iactwp >= 0:
            if self.swvnav[idx]:
                line += "VNAV, "
            line += "LNAV to " + route.wpname[route.iactwp] + "\n"
        if self.orig[idx] != "" or self.dest[idx] != "":
            line += "Flying"
            if self.orig[idx] != "":
                line += " from " + self.orig[idx]
            if self.dest[idx] != "":
                line += " to " + self.dest[idx]

        return line

    def setArea(self, scr, metric, *args):
        if args[0] == 'OFF':
            self.swarea = False
            self.area   = ""
            scr.objappend(2, "AREA", None)  # delete square areas
            scr.objappend(3, "AREA", None)  # delete circle areas
            return True

        if type(args[0]) == float and len(args) >= 4:
            # This is a square area
            self.arealat0 = min(args[0], args[2])
            self.arealat1 = max(args[0], args[2])
            self.arealon0 = min(args[1], args[3])
            self.arealon1 = max(args[1], args[3])

            if len(args) == 5:
                self.areafloor = args[4] * ft
            else:
                self.areafloor = -9999999.

            self.area = "Square"
            self.swarea = True
            scr.objappend(2, "AREA", [args[0], args[1], args[2], args[3]])

            # Avoid mass delete due to redefinition of area
            self.inside = self.ntraf * [False]
            return True
        elif args[0] == "FIR" and len(args) <= 3:
            for i in range(0, len(self.navdb.fir)):
                if args[1] == self.navdb.fir[i][0]:
                    break
            if args[1] != self.navdb.fir[i][0]:
                return False, "Unknown FIR, try again"

            metric.fir_number        = i
            metric.fir_circle_point  = metric.metric_Area.FIR_circle(self.navdb, metric.fir_number)
            metric.fir_circle_radius = float(args[1])

            if len(args) == 3:
                self.areafloor = args[2] * ft
            else:
                self.areafloor = -9999999.

            self.area   = "Circle"
            self.swarea = True
            self.inside = self.ntraf * [False]
            scr.objappend(3, "AREA", [metric.fir_circle_point[0] , metric.fir_circle_point[1], metric.fir_circle_radius])
            return True
        elif args[0] == "CIRCLE" and len(args) in [4, 5]:
            # draw circular experiment area
            self.arealat0 = args[1]    # Latitude of circle center [deg]
            self.arealon0 = args[2]    # Longitude of circle center [deg]
            self.arearadius = args[3]  # Radius of circle Center [NM]

            # Deleting traffic flying out of experiment area
            self.area = "Circle"
            self.swarea = True

            if len(args) == 5:
                self.areafloor = args[4] * ft  # [m]
            else:
                self.areafloor = -9999999.  # [m]

            # draw the circular experiment area on the radar gui
            scr.objappend(3, "AREA", [self.arealat0, self.arealon0, self.arearadius])

            # Avoid mass delete due to redefinition of area
            self.inside = self.ntraf * [False]

            return True

        return False
Exemplo n.º 2
0
class Traffic(DynamicArrays):
    """
    Traffic class definition    : Traffic data
    Methods:
        Traffic()            :  constructor
        reset()              :  Reset traffic database w.r.t a/c data
        create(acid,actype,aclat,aclon,achdg,acalt,acspd) : create aircraft
        delete(acid)         : delete an aircraft from traffic data
        deletall()           : delete all traffic
        update(sim)          : do a numerical integration step
        id2idx(name)         : return index in traffic database of given call sign
        engchange(i,engtype) : change engine type of an aircraft
        setNoise(A)          : Add turbulence
    Members: see create
    Created by  : Jacco M. Hoekstra
    """
    def __init__(self, navdb):
        self.wind = WindSim()

        # Define the periodic loggers
        # ToDo: explain what these line sdo in comments (type of logs?)
        datalog.definePeriodicLogger('SNAPLOG', 'SNAPLOG logfile.',
                                     settings.snapdt)
        datalog.definePeriodicLogger('INSTLOG', 'INSTLOG logfile.',
                                     settings.instdt)
        datalog.definePeriodicLogger('SKYLOG', 'SKYLOG logfile.',
                                     settings.skydt)

        with RegisterElementParameters(self):

            # Register the following parameters for logging
            with datalog.registerLogParameters('SNAPLOG', self):
                # Aircraft Info
                self.id = []  # identifier (string)
                self.type = []  # aircaft type (string)

                # Positions
                self.lat = np.array([])  # latitude [deg]
                self.lon = np.array([])  # longitude [deg]
                self.alt = np.array([])  # altitude [m]
                self.hdg = np.array([])  # traffic heading [deg]
                self.trk = np.array([])  # track angle [deg]

                # Velocities
                self.tas = np.array([])  # true airspeed [m/s]
                self.gs = np.array([])  # ground speed [m/s]
                self.gsnorth = np.array([])  # ground speed [m/s]
                self.gseast = np.array([])  # ground speed [m/s]
                self.cas = np.array([])  # calibrated airspeed [m/s]
                self.M = np.array([])  # mach number
                self.vs = np.array([])  # vertical speed [m/s]

                # Atmosphere
                self.p = np.array([])  # air pressure [N/m2]
                self.rho = np.array([])  # air density [kg/m3]
                self.Temp = np.array([])  # air temperature [K]
                self.dtemp = np.array([])  # delta t for non-ISA conditions

                # Traffic autopilot settings
                self.aspd = np.array([])  # selected spd(CAS) [m/s]
                self.aptas = np.array([])  # just for initializing
                self.ama = np.array(
                    [])  # selected spd above crossover altitude (Mach) [-]
                self.apalt = np.array([])  # selected alt[m]
                self.avs = np.array([])  # selected vertical speed [m/s]

            # Whether to perform LNAV and VNAV
            self.swlnav = np.array([], dtype=np.bool)
            self.swvnav = np.array([], dtype=np.bool)

            # Flight Models
            self.asas = ASAS(self)
            self.ap = Autopilot(self)
            self.pilot = Pilot(self)
            self.adsb = ADSB(self)
            self.trails = Trails(self)
            self.actwp = ActiveWaypoint(self)

            # Traffic performance data
            self.avsdef = np.array(
                [])  # [m/s]default vertical speed of autopilot
            self.aphi = np.array([])  # [rad] bank angle setting of autopilot
            self.ax = np.array(
                [])  # [m/s2] absolute value of longitudinal accelleration
            self.bank = np.array([])  # nominal bank angle, [radian]
            self.hdgsel = np.array(
                [], dtype=np.bool)  # determines whether aircraft is turning

            # Crossover altitude
            self.abco = np.array([])
            self.belco = np.array([])

            # limit settings
            self.limspd = np.array([])  # limit speed
            self.limspd_flag = np.array(
                [], dtype=np.bool
            )  # flag for limit spd - we have to test for max and min
            self.limalt = np.array([])  # limit altitude
            self.limvs = np.array(
                [])  # limit vertical speed due to thrust limitation
            self.limvs_flag = np.array([])

            # Display information on label
            self.label = []  # Text and bitmap of traffic label

            # Miscallaneous
            self.coslat = np.array([])  # Cosine of latitude for computations
            self.eps = np.array([])  # Small nonzero numbers

        # Default bank angles per flight phase
        self.bphase = np.deg2rad(np.array([15, 35, 35, 35, 15, 45]))

        self.reset(navdb)

    def reset(self, navdb):
        # This ensures that the traffic arrays (which size is dynamic)
        # are all reset as well, so all lat,lon,sdp etc but also objects adsb
        super(Traffic, self).reset()
        self.ntraf = 0

        # Reset models
        self.wind.clear()

        # Build new modules for area and turbulence
        self.area = Area(self)
        self.Turbulence = Turbulence(self)

        # Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)
        self.setNoise(False)

        # Import navigation data base
        self.navdb = navdb

        # Default: BlueSky internal performance model.
        # Insert your BADA files to the folder "BlueSky/data/coefficients/BADA"
        # for working with EUROCONTROL`s Base of Aircraft Data revision 3.12
        self.perf = Perf(self)

    def mcreate(self,
                count,
                actype=None,
                alt=None,
                spd=None,
                dest=None,
                area=None):
        """ Create multiple random aircraft in a specified area """
        idbase = chr(randint(65, 90)) + chr(randint(65, 90))
        if actype is None:
            actype = 'B744'

        for i in xrange(count):
            acid = idbase + '%05d' % i
            aclat = random() * (area[1] - area[0]) + area[0]
            aclon = random() * (area[3] - area[2]) + area[2]
            achdg = float(randint(1, 360))
            acalt = (randint(2000, 39000) * ft) if alt is None else alt
            acspd = (randint(250, 450) * kts) if spd is None else spd

            self.create(acid, actype, aclat, aclon, achdg, acalt, acspd)

    def create(self,
               acid=None,
               actype="B744",
               aclat=None,
               aclon=None,
               achdg=None,
               acalt=None,
               casmach=None):
        """Create an aircraft"""

        # Check if not already exist
        if self.id.count(acid.upper()) > 0:
            return False, acid + " already exists."  # already exists do nothing

        # Catch missing acid, replace by a default
        if acid == None or acid == "*":
            acid = "KL204"
            flno = 204
            while self.id.count(acid) > 0:
                flno = flno + 1
                acid = "KL" + str(flno)

        # Check for (other) missing arguments
        if actype == None or aclat == None or aclon == None or achdg == None \
            or acalt == None or casmach == None:

            return False,"CRE: Missing one or more arguments:"\
                         "acid,actype,aclat,aclon,achdg,acalt,acspd"

        super(Traffic, self).create()

        # Increase number of aircraft
        self.ntraf = self.ntraf + 1

        # Aircraft Info
        self.id[-1] = acid.upper()
        self.type[-1] = actype

        # Positions
        self.lat[-1] = aclat
        self.lon[-1] = aclon
        self.alt[-1] = acalt

        self.hdg[-1] = achdg
        self.trk[-1] = achdg

        # Velocities
        self.tas[-1], self.cas[-1], self.M[-1] = casormach(casmach, acalt)
        self.gs[-1] = self.tas[-1]
        self.gsnorth[-1] = self.tas[-1] * cos(radians(self.hdg[-1]))
        self.gseast[-1] = self.tas[-1] * sin(radians(self.hdg[-1]))

        # Atmosphere
        self.Temp[-1], self.rho[-1], self.p[-1] = vatmos(acalt)

        # Wind
        if self.wind.winddim > 0:
            vnwnd, vewnd = self.wind.getdata(self.lat[-1], self.lon[-1],
                                             self.alt[-1])
            self.gsnorth[-1] = self.gsnorth[-1] + vnwnd
            self.gseast[-1] = self.gseast[-1] + vewnd
            self.trk[-1] = np.degrees(
                np.arctan2(self.gseast[-1], self.gsnorth[-1]))
            self.gs[-1] = np.sqrt(self.gsnorth[-1]**2 + self.gseast[-1]**2)

        # Traffic performance data
        #(temporarily default values)
        self.avsdef[-1] = 1500. * fpm  # default vertical speed of autopilot
        self.aphi[-1] = radians(25.)  # bank angle setting of autopilot
        self.ax[-1] = kts  # absolute value of longitudinal accelleration
        self.bank[-1] = radians(25.)

        # Crossover altitude
        self.abco[
            -1] = 0  # not necessary to overwrite 0 to 0, but leave for clarity
        self.belco[-1] = 1

        # Traffic autopilot settings
        self.aspd[-1] = self.cas[-1]
        self.aptas[-1] = self.tas[-1]
        self.apalt[-1] = self.alt[-1]

        # Display information on label
        self.label[-1] = ['', '', '', 0]

        # Miscallaneous
        self.coslat[-1] = cos(
            radians(aclat))  # Cosine of latitude for flat-earth aproximations
        self.eps[-1] = 0.01

        # ----- Submodules of Traffic -----
        self.ap.create()
        self.actwp.create()
        self.pilot.create()
        self.adsb.create()
        self.area.create()
        self.asas.create()
        self.perf.create()
        self.trails.create()

        return True

    def delete(self, acid):
        """Delete an aircraft"""

        # Look up index of aircraft
        idx = self.id2idx(acid)
        # Do nothing if not found
        if idx < 0:
            return False
        # Decrease number of aircraft
        self.ntraf = self.ntraf - 1

        # Delete all aircraft parameters
        super(Traffic, self).delete(idx)

        # ----- Submodules of Traffic -----
        self.perf.delete(idx)
        self.area.delete(idx)
        return True

    def update(self, simt, simdt):
        # Update only if there is traffic ---------------------
        if self.ntraf == 0:
            return

        #---------- Atmosphere --------------------------------
        self.p, self.rho, self.Temp = vatmos(self.alt)

        #---------- ADSB Update -------------------------------
        self.adsb.update(simt)

        #---------- Fly the Aircraft --------------------------
        self.ap.update(simt)
        self.asas.update(simt)
        self.pilot.FMSOrAsas()

        #---------- Limit Speeds ------------------------------
        self.pilot.FlightEnvelope()

        #---------- Kinematics --------------------------------
        self.UpdateAirSpeed(simdt, simt)
        self.UpdateGroundSpeed(simdt)
        self.UpdatePosition(simdt)

        #---------- Performance Update ------------------------
        self.perf.perf(simt)

        #---------- Simulate Turbulence -----------------------
        self.Turbulence.Woosh(simdt)

        #---------- Aftermath ---------------------------------
        self.trails.update(simt)
        self.area.check(simt)
        return

    def UpdateAirSpeed(self, simdt, simt):
        # Acceleration
        self.delspd = self.pilot.spd - self.tas
        swspdsel = np.abs(self.delspd) > 0.4  # <1 kts = 0.514444 m/s
        ax = self.perf.acceleration(simdt)

        # Update velocities
        self.tas = self.tas + swspdsel * ax * np.sign(self.delspd) * simdt
        self.cas = vtas2cas(self.tas, self.alt)
        self.M = vtas2mach(self.tas, self.alt)

        # Turning
        turnrate = np.degrees(g0 * np.tan(self.bank) /
                              np.maximum(self.tas, self.eps))
        delhdg = (self.pilot.hdg - self.hdg + 180.) % 360 - 180.  # [deg]
        self.hdgsel = np.abs(delhdg) > np.abs(2. * simdt * turnrate)

        # Update heading
        self.hdg = (self.hdg +
                    simdt * turnrate * self.hdgsel * np.sign(delhdg)) % 360.

        # Update vertical speed
        delalt = self.pilot.alt - self.alt
        self.swaltsel = np.abs(delalt) > np.maximum(
            10 * ft, np.abs(2 * simdt * np.abs(self.vs)))
        self.vs = self.swaltsel * np.sign(delalt) * self.pilot.vs

    def UpdateGroundSpeed(self, simdt):
        # Compute ground speed and track from heading, airspeed and wind
        if self.wind.winddim == 0:  # no wind
            self.gsnorth = self.tas * np.cos(np.radians(self.hdg))
            self.gseast = self.tas * np.sin(np.radians(self.hdg))

            self.gs = self.tas
            self.trk = self.hdg

        else:
            windnorth, windeast = self.wind.getdata(self.lat, self.lon,
                                                    self.alt)
            self.gsnorth = self.tas * np.cos(np.radians(self.hdg)) + windnorth
            self.gseast = self.tas * np.sin(np.radians(self.hdg)) + windeast

            self.gs = np.sqrt(self.gsnorth**2 + self.gseast**2)
            self.trk = np.degrees(np.arctan2(self.gseast, self.gsnorth)) % 360.

    def UpdatePosition(self, simdt):
        # Update position
        self.alt = np.where(self.swaltsel, self.alt + self.vs * simdt,
                            self.pilot.alt)
        self.lat = self.lat + np.degrees(simdt * self.gsnorth / Rearth)
        self.coslat = np.cos(np.deg2rad(self.lat))
        self.lon = self.lon + np.degrees(
            simdt * self.gseast / self.coslat / Rearth)

    def id2idx(self, acid):
        """Find index of aircraft id"""
        try:
            return self.id.index(acid.upper())
        except:
            return -1

    def setNoise(self, noise=None):
        """Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)"""
        if noise is None:
            return True, "Noise is currently " + (
                "on" if self.Turbulence.active else "off")

        self.Turbulence.SetNoise(noise)
        self.adsb.SetNoise(noise)
        return True

    def engchange(self, acid, engid):
        """Change of engines"""
        self.perf.engchange(acid, engid)
        return

    def move(self, idx, lat, lon, alt=None, hdg=None, casmach=None, vspd=None):
        self.lat[idx] = lat
        self.lon[idx] = lon

        if alt:
            self.alt[idx] = alt
            self.apalt[idx] = alt

        if hdg:
            self.hdg[idx] = hdg
            self.ap.trk[idx] = hdg

        if casmach:
            self.tas[idx], self.aspd[-1], dummy = casormach(casmach, alt)

        if vspd:
            self.vs[idx] = vspd
            self.swvnav[idx] = False

    def nom(self, idx):
        """ Reset acceleration back to nominal (1 kt/s^2): NOM acid """
        self.ax[idx] = kts

    def acinfo(self, idx):
        acid = self.id[idx]
        actype = self.type[idx]
        lat, lon = self.lat[idx], self.lon[idx]
        alt, hdg, trk = self.alt[idx] / ft, self.hdg[idx], round(self.trk[idx])
        cas = self.cas[idx] / kts
        tas = self.tas[idx] / kts
        route = self.ap.route[idx]
        line = "Info on %s %s index = %d\n" % (acid, actype, idx) \
             + "Pos = %.2f, %.2f. Spd: %d kts CAS, %d kts TAS\n" % (lat, lon, cas, tas) \
             + "Alt = %d ft, Hdg = %d, Trk = %d\n" % (alt, hdg, trk)
        if self.swlnav[idx] and route.nwp > 0 and route.iactwp >= 0:
            if self.swvnav[idx]:
                line += "VNAV, "
            line += "LNAV to " + route.wpname[route.iactwp] + "\n"
        if self.ap.orig[idx] != "" or self.ap.dest[idx] != "":
            line += "Flying"
            if self.ap.orig[idx] != "":
                line += " from " + self.ap.orig[idx]
            if self.ap.dest[idx] != "":
                line += " to " + self.ap.dest[idx]

        return acid, line
Exemplo n.º 3
0
class Traffic(DynamicArrays):
    """
    Traffic class definition    : Traffic data
    Methods:
        Traffic()            :  constructor
        reset()              :  Reset traffic database w.r.t a/c data
        create(acid,actype,aclat,aclon,achdg,acalt,acspd) : create aircraft
        delete(acid)         : delete an aircraft from traffic data
        deletall()           : delete all traffic
        update(sim)          : do a numerical integration step
        id2idx(name)         : return index in traffic database of given call sign
        engchange(i,engtype) : change engine type of an aircraft
        setNoise(A)          : Add turbulence
    Members: see create
    Created by  : Jacco M. Hoekstra
    """

    def __init__(self, navdb):
        self.wind = WindSim()

        # Define the periodic loggers
        # ToDo: explain what these line sdo in comments (type of logs?)
        datalog.definePeriodicLogger('SNAPLOG', 'SNAPLOG logfile.', settings.snapdt)
        datalog.definePeriodicLogger('INSTLOG', 'INSTLOG logfile.', settings.instdt)
        datalog.definePeriodicLogger('SKYLOG', 'SKYLOG logfile.', settings.skydt)

        with RegisterElementParameters(self):

            # Register the following parameters for logging
            with datalog.registerLogParameters('SNAPLOG', self):
                # Aircraft Info
                self.id      = []  # identifier (string)
                self.type    = []  # aircaft type (string)

                # Positions
                self.lat     = np.array([])  # latitude [deg]
                self.lon     = np.array([])  # longitude [deg]
                self.alt     = np.array([])  # altitude [m]
                self.hdg     = np.array([])  # traffic heading [deg]
                self.trk     = np.array([])  # track angle [deg]

                # Velocities
                self.tas     = np.array([])  # true airspeed [m/s]
                self.gs      = np.array([])  # ground speed [m/s]
                self.gsnorth = np.array([])  # ground speed [m/s]
                self.gseast  = np.array([])  # ground speed [m/s]
                self.cas     = np.array([])  # calibrated airspeed [m/s]
                self.M       = np.array([])  # mach number
                self.vs      = np.array([])  # vertical speed [m/s]

                # Atmosphere
                self.p       = np.array([])  # air pressure [N/m2]
                self.rho     = np.array([])  # air density [kg/m3]
                self.Temp    = np.array([])  # air temperature [K]
                self.dtemp   = np.array([])  # delta t for non-ISA conditions

                # Traffic autopilot settings
                self.aspd   = np.array([])  # selected spd(CAS) [m/s]
                self.aptas  = np.array([])  # just for initializing
                self.ama    = np.array([])  # selected spd above crossover altitude (Mach) [-]
                self.apalt  = np.array([])  # selected alt[m]
                self.avs    = np.array([])  # selected vertical speed [m/s]
				
				# Speed offset
                self.spdoffset = np.array([]) # speed offset [kts]
                self.spdoffsetdev = np.array([]) # speed offset deviation [kts]
                self.spd_onoff = np.array([]) # speed offset on/off

            # Whether to perform LNAV and VNAV
            self.swlnav   = np.array([], dtype=np.bool)
            self.swvnav   = np.array([], dtype=np.bool)

            # Flight Models
            self.asas   = ASAS(self)
            self.ap     = Autopilot(self)
            self.pilot  = Pilot(self)
            self.adsb   = ADSB(self)
            self.trails = Trails(self)
            self.actwp  = ActiveWaypoint(self)

            # Traffic performance data
            self.avsdef = np.array([])  # [m/s]default vertical speed of autopilot
            self.aphi   = np.array([])  # [rad] bank angle setting of autopilot
            self.ax     = np.array([])  # [m/s2] absolute value of longitudinal accelleration
            self.bank   = np.array([])  # nominal bank angle, [radian]
            self.hdgsel = np.array([], dtype=np.bool)  # determines whether aircraft is turning

            # Crossover altitude
            self.abco   = np.array([])
            self.belco  = np.array([])

            # limit settings
            self.limspd      = np.array([])  # limit speed
            self.limspd_flag = np.array([], dtype=np.bool)  # flag for limit spd - we have to test for max and min
            self.limalt      = np.array([])  # limit altitude
            self.limvs       = np.array([])  # limit vertical speed due to thrust limitation
            self.limvs_flag  = np.array([])

            # Display information on label
            self.label       = []  # Text and bitmap of traffic label

            # Miscallaneous
            self.coslat = np.array([])  # Cosine of latitude for computations
            self.eps    = np.array([])  # Small nonzero numbers

        # Default bank angles per flight phase
        self.bphase = np.deg2rad(np.array([15, 35, 35, 35, 15, 45]))

        self.reset(navdb)

    def reset(self, navdb):
        # This ensures that the traffic arrays (which size is dynamic)
        # are all reset as well, so all lat,lon,sdp etc but also objects adsb
        super(Traffic, self).reset()
        self.ntraf = 0

        # Reset models
        self.wind.clear()

        # Build new modules for area and turbulence
        self.area       = Area(self)
        self.Turbulence = Turbulence(self)

        # Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)
        self.setNoise(False)

        # Import navigation data base
        self.navdb   = navdb

        # Default: BlueSky internal performance model.
        # Insert your BADA files to the folder "BlueSky/data/coefficients/BADA"
        # for working with EUROCONTROL`s Base of Aircraft Data revision 3.12
        self.perf    = Perf(self)
		
        self.AMAN=AMAN(AllFlights,unique_runways) 
        for rwy in unique_runways:
            self.AMAN.initial_schedule_popupflightsonly(intarrtime_AMAN_runway,rwy,simulation_start,unique_runways)

    def mcreate(self, count, actype=None, alt=None, spd=None, dest=None, area=None):
        """ Create multiple random aircraft in a specified area """
        idbase = chr(randint(65, 90)) + chr(randint(65, 90))
        if actype is None:
            actype = 'B744'

        for i in xrange(count):
            acid  = idbase + '%05d' % i
            aclat = random() * (area[1] - area[0]) + area[0]
            aclon = random() * (area[3] - area[2]) + area[2]
            achdg = float(randint(1, 360))
            acalt = (randint(2000, 39000) * ft) if alt is None else alt
            acspd = (randint(250, 450) * kts) if spd is None else spd

            self.create(acid, actype, aclat, aclon, achdg, acalt, acspd)

    def create(self, acid=None, actype="B744", aclat=None, aclon=None, achdg=None, acalt=None, casmach=None):
        """Create an aircraft"""

        # Check if not already exist
        if self.id.count(acid.upper()) > 0:
            return False, acid + " already exists."  # already exists do nothing

        # Catch missing acid, replace by a default
        if acid == None or acid =="*":
            acid = "KL204"
            flno = 204
            while self.id.count(acid)>0:
                flno = flno+1
                acid ="KL"+str(flno)
        
        # Check for (other) missing arguments
        if actype == None or aclat == None or aclon == None or achdg == None \
            or acalt == None or casmach == None:
            
            return False,"CRE: Missing one or more arguments:"\
                         "acid,actype,aclat,aclon,achdg,acalt,acspd"

        super(Traffic, self).create()

        # Increase number of aircraft
        self.ntraf = self.ntraf + 1

        # Aircraft Info
        self.id[-1]   = acid.upper()
        self.type[-1] = actype

        # Positions
        self.lat[-1]  = aclat
        self.lon[-1]  = aclon
        self.alt[-1]  = acalt

        self.hdg[-1]  = achdg
        self.trk[-1]  = achdg

        # Velocities
        self.tas[-1], self.cas[-1], self.M[-1] = casormach(casmach, acalt)
        self.gs[-1]      = self.tas[-1]
        self.gsnorth[-1] = self.tas[-1] * cos(radians(self.hdg[-1]))
        self.gseast[-1]  = self.tas[-1] * sin(radians(self.hdg[-1]))
		
		# Speed offset
        self.spd_onoff[-1] = 0. # Speed offset = 0 (off) on default

        # Atmosphere
        self.Temp[-1], self.rho[-1], self.p[-1] = vatmos(acalt)

        # Wind
        if self.wind.winddim > 0:
            vnwnd, vewnd     = self.wind.getdata(self.lat[-1], self.lon[-1], self.alt[-1])
            self.gsnorth[-1] = self.gsnorth[-1] + vnwnd
            self.gseast[-1]  = self.gseast[-1]  + vewnd
            self.trk[-1]     = np.degrees(np.arctan2(self.gseast[-1], self.gsnorth[-1]))
            self.gs[-1]      = np.sqrt(self.gsnorth[-1]**2 + self.gseast[-1]**2)

        # Traffic performance data
        #(temporarily default values)
        self.avsdef[-1] = 1500. * fpm   # default vertical speed of autopilot
        self.aphi[-1]   = radians(25.)  # bank angle setting of autopilot
        self.ax[-1]     = kts           # absolute value of longitudinal accelleration
        self.bank[-1]   = radians(25.)

        # Crossover altitude
        self.abco[-1]   = 0  # not necessary to overwrite 0 to 0, but leave for clarity
        self.belco[-1]  = 1

        # Traffic autopilot settings
        self.aspd[-1]  = self.cas[-1]
        self.aptas[-1] = self.tas[-1]
        self.apalt[-1] = self.alt[-1]

        # Display information on label
        self.label[-1] = ['', '', '', 0]

        # Miscallaneous
        self.coslat[-1] = cos(radians(aclat))  # Cosine of latitude for flat-earth aproximations
        self.eps[-1] = 0.01

        # ----- Submodules of Traffic -----
        self.ap.create()
        self.actwp.create()
        self.pilot.create()
        self.adsb.create()
        self.area.create()
        self.asas.create()
        self.perf.create()
        self.trails.create()

        return True

    def delete(self, acid):
        """Delete an aircraft"""

        # Look up index of aircraft
        idx = self.id2idx(acid)
        # Do nothing if not found
        if idx < 0:
            return False
        # Decrease number of aircraft
        self.ntraf = self.ntraf - 1

        # Delete all aircraft parameters
        super(Traffic, self).delete(idx)

        # ----- Submodules of Traffic -----
        self.perf.delete(idx)
        self.area.delete(idx)
        return True

    def update(self, simt, simdt):
        # Update only if there is traffic ---------------------
        if self.ntraf == 0:
            return

        #---------- Atmosphere --------------------------------
        self.p, self.rho, self.Temp = vatmos(self.alt)

        #---------- ADSB Update -------------------------------
        self.adsb.update(simt)

        #---------- Fly the Aircraft --------------------------
        self.ap.update(simt)
        self.asas.update(simt)
        self.pilot.FMSOrAsas()

        #---------- Limit Speeds ------------------------------
        self.pilot.FlightEnvelope()

        #---------- Kinematics --------------------------------
        self.UpdateAirSpeed(simdt, simt)
        self.UpdateGroundSpeed(simdt)
        self.UpdatePosition(simdt)

        #---------- Performance Update ------------------------
        self.perf.perf(simt)

        #---------- Simulate Turbulence -----------------------
        self.Turbulence.Woosh(simdt)

        #---------- Aftermath ---------------------------------
        self.trails.update(simt)
        self.area.check(simt)
		
		#---------- AMAN --------------------------------------
        i=0 
        while(i<self.ntraf): 
            temp1, temp2 = qdrdist(self.lat[i], self.lon[i], 52.309, 4.764) # Check distance towards EHAM
            if temp2<2. and self.alt[i]<1.: # If aircraft within 2 nm from airport and below 1 meter, delete it            
                self.delete(self.id[i]) 
            i=i+1
		
		# Calculate energy cost per flight
        self.AMAN.calculate_energy_cost(self.id,self.vs,self.tas,CD_0,CD_2,self.rho,WingSurface,self.hdg,self.trk,mass_nominal,simdt)
        
		# Increase iteration counter
        self.AMAN.IterationCounter=self.AMAN.IterationCounter+1
         
		# Update trajectory predictor, scheduler and SARA every five seconds
        if self.AMAN.IterationCounter%2==0:
            # Check for speed offset
            self.AMAN.speed_offset_switch(self.id,AMAN_horizon,self.spd_onoff)
			
			# Update Trajectory Predictions
            self.AMAN.update_TP(self.id,self.lat,self.lon,self.tas/kts,self.actwp.lat,self.actwp.lon,simt)
            
			# Update schedule per runway
            for rwy in unique_runways:     
                if '--node' in sys.argv:
                    if var_TP==str('ASAPBASIC'):
                        self.AMAN.scheduler_ASAP_basic(self.id,Take_into_account_schedule_horizon,rwy,intarrtime_AMAN_runway,Freeze_horizon,unique_runways)
                    elif var_TP==str('DYNAMIC'):
                        self.AMAN.scheduler_dynamic(self.id,Take_into_account_schedule_horizon,rwy,intarrtime_AMAN_runway)
                    elif var_TP==str('ASAPUPGRADE'):
                        self.AMAN.scheduler_ASAP_upgrade(self.id,Take_into_account_schedule_horizon,rwy,intarrtime_AMAN_runway,Freeze_horizon,unique_runways)
                else:                       
					self.AMAN.scheduler_ASAP_basic(self.id,Take_into_account_schedule_horizon,rwy,intarrtime_AMAN_runway,Freeze_horizon,unique_runways)                    
                    # #self.AMAN.scheduler_dynamic(self.id,Take_into_account_schedule_horizon,rwy,intarrtime_AMAN_runway)                    
                    # #self.AMAN.scheduler_ASAP_upgrade(self.id,Take_into_account_schedule_horizon,rwy,intarrtime_AMAN_runway,Freeze_horizon,unique_runways)
            
			# Update SARA advisories
            self.AMAN.update_SARA(self.id,self.alt,self.ap.route,SARA_horizon,simt,approach_margin)
            
			# Save variables in logger
            self.AMAN.AMAN_LOG_arrtimes_and_energycost(self.id,simulation_start)
            self.AMAN.AMAN_LOG_STAhistory(self.id,simt)
            self.AMAN.AMAN_LOG_lowleveldelay(self.id,simt)
            self.AMAN.AMAN_LOG_seqhistory(self.id)
            self.AMAN.AMAN_LOG_CBAShistory(self.id,simulation_start)
            #self.AMAN.AMAN_LOG_ETA_CBAShistory(self.id,simt)
            self.AMAN.AMAN_LOG_traffic_bunches(self.id,approach_margin,simt)
        return

    def UpdateAirSpeed(self, simdt, simt):
        # Acceleration
        self.delspd = self.pilot.spd - self.tas
        swspdsel = np.abs(self.delspd) > 0.4  # <1 kts = 0.514444 m/s
        ax = self.perf.acceleration(simdt)

        # Update velocities
        self.tas = self.tas + swspdsel * ax * np.sign(self.delspd) * simdt
        self.cas = vtas2cas(self.tas, self.alt)
        self.M   = vtas2mach(self.tas, self.alt)

        # Turning
        turnrate = np.degrees(g0 * np.tan(self.bank) / np.maximum(self.tas, self.eps))
        delhdg   = (self.pilot.hdg - self.hdg + 180.) % 360 - 180.  # [deg]
        self.hdgsel = np.abs(delhdg) > np.abs(2. * simdt * turnrate)

        # Update heading
        self.hdg = (self.hdg + simdt * turnrate * self.hdgsel * np.sign(delhdg)) % 360.

        # Update vertical speed
        delalt   = self.pilot.alt - self.alt
        self.swaltsel = np.abs(delalt) > np.maximum(10 * ft, np.abs(2 * simdt * np.abs(self.vs)))
        self.vs  = self.swaltsel * np.sign(delalt) * self.pilot.vs

    def UpdateGroundSpeed(self, simdt):
        # Compute ground speed and track from heading, airspeed and wind
        spd_random = self.spdoffset * kts + np.random.randn(self.ntraf) * self.spdoffsetdev * kts
		#self.spd_onoff = array([0 0 0 0 0 0 1 0 0 0 etc.]) # Array whether speed offset + randomness is on/off per aircraft
        tasx = self.tas + spd_random * self.spd_onoff
        #print(tasx)
        #tasx = self.tas + self.spdoffset * kts # np.random.randn(self.ntraf) * self.spdoffsetdev * kts
        if self.wind.winddim == 0:  # no wind
            self.gsnorth  = tasx * np.cos(np.radians(self.hdg))
            self.gseast   = tasx * np.sin(np.radians(self.hdg))

            self.gs  = self.tas
            self.trk = self.hdg

        else:
            windnorth, windeast = self.wind.getdata(self.lat, self.lon, self.alt)
            self.gsnorth  = tasx * np.cos(np.radians(self.hdg)) + windnorth
            self.gseast   = tasx * np.sin(np.radians(self.hdg)) + windeast

            self.gs  = np.sqrt(self.gsnorth**2 + self.gseast**2)
            self.trk = np.degrees(np.arctan2(self.gseast, self.gsnorth)) % 360.

    def UpdatePosition(self, simdt):
        # Update position
        self.alt = np.where(self.swaltsel, self.alt + self.vs * simdt, self.pilot.alt)
        self.lat = self.lat + np.degrees(simdt * self.gsnorth / Rearth)
        self.coslat = np.cos(np.deg2rad(self.lat))
        self.lon = self.lon + np.degrees(simdt * self.gseast / self.coslat / Rearth)

    def id2idx(self, acid):
        """Find index of aircraft id"""
        try:
            return self.id.index(acid.upper())
        except:
            return -1

    def setNoise(self, noise=None):
        """Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)"""
        if noise is None:
            return True, "Noise is currently " + ("on" if self.Turbulence.active else "off")

        self.Turbulence.SetNoise(noise)
        self.adsb.SetNoise(noise)
        return True

    def engchange(self, acid, engid):
        """Change of engines"""
        self.perf.engchange(acid, engid)
        return

    def move(self, idx, lat, lon, alt=None, hdg=None, casmach=None, vspd=None):
        self.lat[idx]      = lat
        self.lon[idx]      = lon

        if alt:
            self.alt[idx]   = alt
            self.apalt[idx] = alt

        if hdg:
            self.hdg[idx]  = hdg
            self.ap.trk[idx] = hdg

        if casmach:
            self.tas[idx], self.aspd[-1], dummy = casormach(casmach, alt)

        if vspd:
            self.vs[idx]       = vspd
            self.swvnav[idx] = False

    def nom(self, idx):
        """ Reset acceleration back to nominal (1 kt/s^2): NOM acid """
        self.ax[idx] = kts
		
    def setdeltaspeed(self, idx, offset=0.):
		self.spdoffset[idx] = offset
	
    def setdeltaspeeddev(self, idx, offset=0.):
		self.spdoffsetdev[idx] = offset

    def poscommand(self, scr, idxorwp):# Show info on aircraft(int) or waypoint or airport (str)
        """POS command: Show info or an aircraft, airport, waypoint or navaid"""
        # Aircraft index
        if type(idxorwp)==int and idxorwp >= 0:

            idx           = idxorwp
            acid          = self.id[idx]
            actype        = self.type[idx]
            lat, lon      = self.lat[idx], self.lon[idx]
            alt, hdg, trk = self.alt[idx] / ft, self.hdg[idx], round(self.trk[idx])
            cas           = self.cas[idx] / kts
            tas           = self.tas[idx] / kts
            route         = self.ap.route[idx]
            
            # Position report
            lines = "Info on %s %s index = %d\n" % (acid, actype, idx)  \
                 + "Pos = %.2f, %.2f. Spd: %d kts CAS, %d kts TAS\n" % (lat, lon, cas, tas) \
                 + "Alt = %d ft, Hdg = %d, Trk = %d\n" % (alt, hdg, trk)

            # FMS AP modes
            if self.swlnav[idx] and route.nwp > 0 and route.iactwp >= 0:

                if self.swvnav[idx]:
                    lines = lines + "VNAV, "

                lines += "LNAV to " + route.wpname[route.iactwp] + "\n"

            # Flight info: Destination and origin
            if self.ap.orig[idx] != "" or self.ap.dest[idx] != "":
                lines = lines +  "Flying"

                if self.ap.orig[idx] != "":
                    lines = lines +  " from " + self.ap.orig[idx]

                if self.ap.dest[idx] != "":
                    lines = lines +  " to " + self.ap.dest[idx]

            # Show a/c info and highlight route of aircraft in radar window
            # and pan to a/c (to show route)
            return scr.showacinfo(acid,lines)        

        # Waypoint: airport, navaid or fix
        else:
            wp = idxorwp.upper()

            # Reference position for finding nearest            
            reflat = scr.ctrlat
            reflon = scr.ctrlon            
            
            lines = "Info on "+wp+":\n"
                       
            # First try airports (most used and shorter, hence faster list)
            iap = self.navdb.getaptidx(wp)
            if iap>=0:                
                aptypes = ["large","medium","small"]
                lines = lines + self.navdb.aptname[iap]+"\n"                 \
                        + "is a "+ aptypes[max(-1,self.navdb.aptype[iap]-1)] \
                        +" airport at:\n"                                    \
                        + latlon2txt(self.navdb.aptlat[iap],                 \
                                     self.navdb.aptlon[iap]) + "\n"          \
                        + "Elevation: "                                      \
                        + str(int(round(self.navdb.aptelev[iap]/ft)))        \
                        + " ft \n"

               # Show country name
                try:
                     ico = self.navdb.cocode2.index(self.navdb.aptco[iap].upper())
                     lines = lines + "in "+self.navdb.coname[ico]+" ("+      \
                             self.navdb.aptco[iap]+")"
                except:
                     ico = -1
                     lines = lines + "Country code: "+self.navdb.aptco[iap]
                try:
                    rwytxt = str(self.navdb.rwythresholds[self.navdb.aptid[iap]].keys())
                    lines = lines + "\nRunways: " +rwytxt.strip("[]").replace("'","")
                except:
                    pass

            # Not found as airport, try waypoints & navaids
            else:
                iwps = self.navdb.getwpindices(wp,reflat,reflon)
                if iwps[0]>=0:
                    typetxt = ""
                    desctxt = ""
                    lastdesc = "XXXXXXXX"
                    for i in iwps:
                        
                        # One line type text                        
                        if typetxt == "":
                            typetxt = typetxt+self.navdb.wptype[i]
                        else:
                            typetxt = typetxt+" and "+self.navdb.wptype[i]

                        # Description: multi-line
                        samedesc = self.navdb.wpdesc[i]==lastdesc
                        if desctxt == "":
                            desctxt = desctxt +self.navdb.wpdesc[i]
                            lastdesc = self.navdb.wpdesc[i]
                        elif not samedesc:
                            desctxt = desctxt +"\n"+self.navdb.wpdesc[i]
                            lastdesc = self.navdb.wpdesc[i]
                            
                        # Navaid: frequency
                        if self.navdb.wptype[i] in ["VOR","DME","TACAN"] and not samedesc:
                            desctxt = desctxt + " "+ str(self.navdb.wpfreq[i])+" MHz"
                        elif self.navdb.wptype[i]=="NDB" and not samedesc:
                            desctxt = desctxt+ " " + str(self.navdb.wpfreq[i])+" kHz"  

                    iwp = iwps[0]

                    # Basic info
                    lines = lines + wp +" is a "+ typetxt       \
                           + " at\n"\
                           + latlon2txt(self.navdb.wplat[iwp],                \
                                        self.navdb.wplon[iwp])
                    # Navaids have description                    
                    if len(desctxt)>0:
                        lines = lines+ "\n" + desctxt           

                    # VOR give variation
                    if self.navdb.wptype[iwp]=="VOR":
                        lines = lines + "\nVariation: "+ \
                                     str(self.navdb.wpvar[iwp])+" deg"

  
                    # How many others?
                    nother = self.navdb.wpid.count(wp)-len(iwps)
                    if nother>0:
                        verb = ["is ","are "][min(1,max(0,nother-1))]
                        lines = lines +"\nThere "+verb + str(nother) +\
                                   " other waypoint(s) also named " + wp
                else:
                    return False,idxorwp+" not found as a/c, airport, navaid or waypoint"

            # Show what we found on airport and navaid/waypoint
            scr.echo(lines)
            
        return True
Exemplo n.º 4
0
class Traffic(DynamicArrays):
    """
    Traffic class definition    : Traffic data
    Methods:
        Traffic()            :  constructor
        reset()              :  Reset traffic database w.r.t a/c data
        create(acid,actype,aclat,aclon,achdg,acalt,acspd) : create aircraft
        delete(acid)         : delete an aircraft from traffic data
        deletall()           : delete all traffic
        update(sim)          : do a numerical integration step
        id2idx(name)         : return index in traffic database of given call sign
        engchange(i,engtype) : change engine type of an aircraft
        setNoise(A)          : Add turbulence
    Members: see create
    Created by  : Jacco M. Hoekstra
    """

    def __init__(self):
        self.wind = WindSim()

        # Define the periodic loggers
        # ToDo: explain what these line sdo in comments (type of logs?)
        datalog.definePeriodicLogger('SNAPLOG', 'SNAPLOG logfile.', settings.snapdt)
        datalog.definePeriodicLogger('INSTLOG', 'INSTLOG logfile.', settings.instdt)
        datalog.definePeriodicLogger('SKYLOG', 'SKYLOG logfile.', settings.skydt)

        with RegisterElementParameters(self):

            # Register the following parameters for logging
            with datalog.registerLogParameters('SNAPLOG', self):
                # Aircraft Info
                self.id      = []  # identifier (string)
                self.type    = []  # aircaft type (string)

                # Positions
                self.lat     = np.array([])  # latitude [deg]
                self.lon     = np.array([])  # longitude [deg]
                self.alt     = np.array([])  # altitude [m]
                self.hdg     = np.array([])  # traffic heading [deg]
                self.trk     = np.array([])  # track angle [deg]

                # Velocities
                self.tas     = np.array([])  # true airspeed [m/s]
                self.gs      = np.array([])  # ground speed [m/s]
                self.gsnorth = np.array([])  # ground speed [m/s]
                self.gseast  = np.array([])  # ground speed [m/s]
                self.cas     = np.array([])  # calibrated airspeed [m/s]
                self.M       = np.array([])  # mach number
                self.vs      = np.array([])  # vertical speed [m/s]

                # Atmosphere
                self.p       = np.array([])  # air pressure [N/m2]
                self.rho     = np.array([])  # air density [kg/m3]
                self.Temp    = np.array([])  # air temperature [K]
                self.dtemp   = np.array([])  # delta t for non-ISA conditions

                # Traffic autopilot settings
                self.aspd   = np.array([])  # selected spd(CAS) [m/s]
                self.aptas  = np.array([])  # just for initializing
                self.ama    = np.array([])  # selected spd above crossover altitude (Mach) [-]
                self.apalt  = np.array([])  # selected alt[m]
                self.avs    = np.array([])  # selected vertical speed [m/s]

            # Whether to perform LNAV and VNAV
            self.swlnav   = np.array([], dtype=np.bool)
            self.swvnav   = np.array([], dtype=np.bool)

            # Flight Models
            self.asas   = ASAS()
            self.ap     = Autopilot()
            self.pilot  = Pilot()
            self.adsb   = ADSB()
            self.trails = Trails()
            self.actwp  = ActiveWaypoint()

            # Traffic performance data
            self.avsdef = np.array([])  # [m/s]default vertical speed of autopilot
            self.aphi   = np.array([])  # [rad] bank angle setting of autopilot
            self.ax     = np.array([])  # [m/s2] absolute value of longitudinal accelleration
            self.bank   = np.array([])  # nominal bank angle, [radian]
            self.hdgsel = np.array([], dtype=np.bool)  # determines whether aircraft is turning

            # Crossover altitude
            self.abco   = np.array([])
            self.belco  = np.array([])

            # limit settings
            self.limspd      = np.array([])  # limit speed
            self.limspd_flag = np.array([], dtype=np.bool)  # flag for limit spd - we have to test for max and min
            self.limalt      = np.array([])  # limit altitude
            self.limvs       = np.array([])  # limit vertical speed due to thrust limitation
            self.limvs_flag  = np.array([])

            # Display information on label
            self.label       = []  # Text and bitmap of traffic label

            # Miscallaneous
            self.coslat = np.array([])  # Cosine of latitude for computations
            self.eps    = np.array([])  # Small nonzero numbers

        # Default bank angles per flight phase
        self.bphase = np.deg2rad(np.array([15, 35, 35, 35, 15, 45]))

        self.reset()

    def reset(self):
        # This ensures that the traffic arrays (which size is dynamic)
        # are all reset as well, so all lat,lon,sdp etc but also objects adsb
        super(Traffic, self).reset()
        self.ntraf = 0

        # Reset models
        self.wind.clear()

        # Build new modules for area and turbulence
        self.area       = Area()
        self.Turbulence = Turbulence()

        # Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)
        self.setNoise(False)

        # Default: BlueSky internal performance model.
        # Insert your BADA files to the folder "BlueSky/data/coefficients/BADA"
        # for working with EUROCONTROL`s Base of Aircraft Data revision 3.12
        self.perf    = Perf()
        self.trails.reset()

    def mcreate(self, count, actype=None, alt=None, spd=None, dest=None):
        """ Create multiple random aircraft in a specified area """
        area = bs.scr.getviewlatlon()
        idbase = chr(randint(65, 90)) + chr(randint(65, 90))
        if actype is None:
            actype = 'B744'

        n = count
        super(Traffic, self).create(n)

        # Increase number of aircraft
        self.ntraf = self.ntraf + count

        acids = []
        aclats = []
        aclons = []
        achdgs = []
        acalts = []
        acspds = []

        for i in xrange(count):
            acids.append((idbase + '%05d' % i).upper())
            aclats.append(random() * (area[1] - area[0]) + area[0])
            aclons.append(random() * (area[3] - area[2]) + area[2])
            achdgs.append(float(randint(1, 360)))
            acalts.append((randint(2000, 39000) * ft) if alt is None else alt)
            acspds.append((randint(250, 450) * kts) if spd is None else spd)

        # Aircraft Info
        self.id[-n:]   = acids
        self.type[-n:] = [actype] * n

        # Positions
        self.lat[-n:]  = aclats
        self.lon[-n:]  = aclons
        self.alt[-n:]  = acalts

        self.hdg[-n:]  = achdgs
        self.trk[-n:]  = achdgs

        # Velocities
        self.tas[-n:], self.cas[-n:], self.M[-n:] = vcasormach(acspds, acalts)
        self.gs[-n:]      = self.tas[-n:]
        self.gsnorth[-n:] = self.tas[-n:] * np.cos(np.radians(self.hdg[-n:]))
        self.gseast[-n:]  = self.tas[-n:] * np.sin(np.radians(self.hdg[-n:]))

        # Atmosphere
        self.p[-n:], self.rho[-n:], self.Temp[-n:] = vatmos(acalts)

        # Wind
        if self.wind.winddim > 0:
            vnwnd, vewnd     = self.wind.getdata(self.lat[-n:], self.lon[-n:], self.alt[-n:])
            self.gsnorth[-n:] = self.gsnorth[-n:] + vnwnd
            self.gseast[-n:]  = self.gseast[-n:]  + vewnd
            self.trk[-n:]     = np.degrees(np.arctan2(self.gseast[-n:], self.gsnorth[-n:]))
            self.gs[-n:]      = np.sqrt(self.gsnorth[-n:]**2 + self.gseast[-n:]**2)

        # Traffic performance data
        #(temporarily default values)
        self.avsdef[-n:] = 1500. * fpm   # default vertical speed of autopilot
        self.aphi[-n:]   = np.radians(25.)  # bank angle setting of autopilot
        self.ax[-n:]     = kts           # absolute value of longitudinal accelleration
        self.bank[-n:]   = np.radians(25.)

        # Crossover altitude
        self.abco[-n:]   = 0  # not necessary to overwrite 0 to 0, but leave for clarity
        self.belco[-n:]  = 1

        # Traffic autopilot settings
        self.aspd[-n:]  = self.cas[-n:]
        self.aptas[-n:] = self.tas[-n:]
        self.apalt[-n:] = self.alt[-n:]

        # Display information on label
        self.label[-n:] = ['', '', '', 0]

        # Miscallaneous
        self.coslat[-n:] = np.cos(np.radians(aclats))  # Cosine of latitude for flat-earth aproximations
        self.eps[-n:] = 0.01

        # ----- Submodules of Traffic -----
        self.ap.create(n)
        self.actwp.create(n)
        self.pilot.create(n)
        self.adsb.create(n)
        self.area.create(n)
        self.asas.create(n)
        self.perf.create(n)
        self.trails.create(n)

    def create(self, acid=None, actype="B744", aclat=None, aclon=None, achdg=None, acalt=None, casmach=None):
        """Create an aircraft"""

        # Check if not already exist
        if self.id.count(acid.upper()) > 0:
            return False, acid + " already exists."  # already exists do nothing

        # Catch missing acid, replace by a default
        if acid is None or acid == "*":
            acid = "KL204"
            flno = 204
            while self.id.count(acid) > 0:
                flno = flno + 1
                acid = "KL" + str(flno)

        # Check for (other) missing arguments
        if actype is None or aclat is None or aclon is None or achdg is None \
                or acalt is None or casmach is None:

            return False, "CRE: Missing one or more arguments:"\
                          "acid,actype,aclat,aclon,achdg,acalt,acspd"

        super(Traffic, self).create()

        # Increase number of aircraft
        self.ntraf = self.ntraf + 1

        # Aircraft Info
        self.id[-1]   = acid.upper()
        self.type[-1] = actype

        # Positions
        self.lat[-1]  = aclat
        self.lon[-1]  = aclon
        self.alt[-1]  = acalt

        self.hdg[-1]  = achdg
        self.trk[-1]  = achdg

        # Velocities
        self.tas[-1], self.cas[-1], self.M[-1] = casormach(casmach, acalt)
        self.gs[-1]      = self.tas[-1]
        self.gsnorth[-1] = self.tas[-1] * cos(radians(self.hdg[-1]))
        self.gseast[-1]  = self.tas[-1] * sin(radians(self.hdg[-1]))

        # Atmosphere
        self.p[-1], self.rho[-1], self.Temp[-1] = vatmos(acalt)

        # Wind
        if self.wind.winddim > 0:
            vnwnd, vewnd     = self.wind.getdata(self.lat[-1], self.lon[-1], self.alt[-1])
            self.gsnorth[-1] = self.gsnorth[-1] + vnwnd
            self.gseast[-1]  = self.gseast[-1]  + vewnd
            self.trk[-1]     = np.degrees(np.arctan2(self.gseast[-1], self.gsnorth[-1]))
            self.gs[-1]      = np.sqrt(self.gsnorth[-1]**2 + self.gseast[-1]**2)

        # Traffic performance data
        #(temporarily default values)
        self.avsdef[-1] = 1500. * fpm   # default vertical speed of autopilot
        self.aphi[-1]   = radians(25.)  # bank angle setting of autopilot
        self.ax[-1]     = kts           # absolute value of longitudinal accelleration
        self.bank[-1]   = radians(25.)

        # Crossover altitude
        self.abco[-1]   = 0  # not necessary to overwrite 0 to 0, but leave for clarity
        self.belco[-1]  = 1

        # Traffic autopilot settings
        self.aspd[-1]  = self.cas[-1]
        self.aptas[-1] = self.tas[-1]
        self.apalt[-1] = self.alt[-1]

        # Display information on label
        self.label[-1] = ['', '', '', 0]

        # Miscallaneous
        self.coslat[-1] = cos(radians(aclat))  # Cosine of latitude for flat-earth aproximations
        self.eps[-1] = 0.01

        # ----- Submodules of Traffic -----
        self.ap.create()
        self.actwp.create()
        self.pilot.create()
        self.adsb.create()
        self.area.create()
        self.asas.create()
        self.perf.create()
        self.trails.create()

        return True

    def creconfs(self, acid, actype, targetidx, dpsi, cpa, tlosh, dH=None, tlosv=None, spd=None):
        latref  = self.lat[targetidx]  # deg
        lonref  = self.lon[targetidx]  # deg
        altref  = self.alt[targetidx]  # m
        trkref  = radians(self.trk[targetidx])
        gsref   = self.gs[targetidx]   # m/s
        vsref   = self.vs[targetidx]   # m/s
        cpa     = cpa * nm
        pzr     = settings.asas_pzr * nm
        pzh     = settings.asas_pzh * ft

        trk     = trkref + radians(dpsi)
        gs      = gsref if spd is None else spd
        if dH is None:
            acalt = altref
            acvs  = 0.0
        else:
            acalt = altref + dH
            tlosv = tlosh if tlosv is None else tlosv
            acvs  = vsref - np.sign(dH) * (abs(dH) - pzh) / tlosv

        # Horizontal relative velocity vector
        gsn, gse     = gs    * cos(trk),          gs    * sin(trk)
        vreln, vrele = gsref * cos(trkref) - gsn, gsref * sin(trkref) - gse
        # Relative velocity magnitude
        vrel    = sqrt(vreln * vreln + vrele * vrele)
        # Relative travel distance to closest point of approach
        drelcpa = tlosh * vrel + (0 if cpa > pzr else sqrt(pzr * pzr - cpa * cpa))
        # Initial intruder distance
        dist    = sqrt(drelcpa * drelcpa + cpa * cpa)
        # Rotation matrix diagonal and cross elements for distance vector
        rd      = drelcpa / dist
        rx      = cpa / dist
        # Rotate relative velocity vector to obtain intruder bearing
        brn     = degrees(atan2(-rx * vreln + rd * vrele,
                                 rd * vreln + rx * vrele))

        # Calculate intruder lat/lon
        aclat, aclon = geo.qdrpos(latref, lonref, brn, dist / nm)

        # convert groundspeed to CAS, and track to heading
        wn, we     = self.wind.getdata(aclat, aclon, acalt)
        tasn, tase = gsn - wn, gse - we
        acspd      = vtas2cas(sqrt(tasn * tasn + tase * tase), acalt)
        achdg      = degrees(atan2(tase, tasn))

        # Create and, when necessary, set vertical speed
        self.create(acid, actype, aclat, aclon, achdg, acalt, acspd)
        self.ap.selalt(len(self.lat) - 1, altref, acvs)
        self.vs[-1] = acvs

    def delete(self, acid):
        """Delete an aircraft"""

        # Look up index of aircraft
        idx = self.id2idx(acid)
        # Do nothing if not found
        if idx < 0:
            return False
        # Decrease number of aircraft
        self.ntraf = self.ntraf - 1

        # Delete all aircraft parameters
        super(Traffic, self).delete(idx)

        # ----- Submodules of Traffic -----
        self.perf.delete(idx)
        self.area.delete(idx)
        return True

    def update(self, simt, simdt):
        # Update only if there is traffic ---------------------
        if self.ntraf == 0:
            return

        #---------- Atmosphere --------------------------------
        self.p, self.rho, self.Temp = vatmos(self.alt)

        #---------- ADSB Update -------------------------------
        self.adsb.update(simt)

        #---------- Fly the Aircraft --------------------------
        self.ap.update(simt)
        self.asas.update(simt)
        self.pilot.FMSOrAsas()

        #---------- Limit Speeds ------------------------------
        self.pilot.FlightEnvelope()

        #---------- Kinematics --------------------------------
        self.UpdateAirSpeed(simdt, simt)
        self.UpdateGroundSpeed(simdt)
        self.UpdatePosition(simdt)

        #---------- Performance Update ------------------------
        self.perf.perf(simt)

        #---------- Simulate Turbulence -----------------------
        self.Turbulence.Woosh(simdt)

        #---------- Aftermath ---------------------------------
        self.trails.update(simt)
        self.area.check(simt)
        return

    def UpdateAirSpeed(self, simdt, simt):
        # Acceleration
        self.delspd = self.pilot.spd - self.tas

        swspdsel = np.abs(self.delspd) > 0.4  # <1 kts = 0.514444 m/s
        ax = self.perf.acceleration(simdt)

        # Update velocities
        self.tas = self.tas + swspdsel * ax * np.sign(self.delspd) * simdt

        self.cas = vtas2cas(self.tas, self.alt)
        self.M   = vtas2mach(self.tas, self.alt)

        # Turning
        turnrate = np.degrees(g0 * np.tan(self.bank) / np.maximum(self.tas, self.eps))
        delhdg   = (self.pilot.hdg - self.hdg + 180.) % 360 - 180.  # [deg]
        self.hdgsel = np.abs(delhdg) > np.abs(2. * simdt * turnrate)

        # Update heading
        self.hdg = (self.hdg + simdt * turnrate * self.hdgsel * np.sign(delhdg)) % 360.

        # Update vertical speed
        delalt   = self.pilot.alt - self.alt
        self.swaltsel = np.abs(delalt) > np.maximum(10 * ft, np.abs(2. * simdt * np.abs(self.vs)))
        self.vs  = self.swaltsel * np.sign(delalt) * np.abs(self.pilot.vs)

    def UpdateGroundSpeed(self, simdt):
        # Compute ground speed and track from heading, airspeed and wind
        if self.wind.winddim == 0:  # no wind
            self.gsnorth  = self.tas * np.cos(np.radians(self.hdg))
            self.gseast   = self.tas * np.sin(np.radians(self.hdg))

            self.gs  = self.tas
            self.trk = self.hdg

        else:
            windnorth, windeast = self.wind.getdata(self.lat, self.lon, self.alt)
            self.gsnorth  = self.tas * np.cos(np.radians(self.hdg)) + windnorth
            self.gseast   = self.tas * np.sin(np.radians(self.hdg)) + windeast

            self.gs  = np.sqrt(self.gsnorth**2 + self.gseast**2)
            self.trk = np.degrees(np.arctan2(self.gseast, self.gsnorth)) % 360.

    def UpdatePosition(self, simdt):
        # Update position
        self.alt = np.where(self.swaltsel, self.alt + self.vs * simdt, self.pilot.alt)
        self.lat = self.lat + np.degrees(simdt * self.gsnorth / Rearth)
        self.coslat = np.cos(np.deg2rad(self.lat))
        self.lon = self.lon + np.degrees(simdt * self.gseast / self.coslat / Rearth)

    def id2idx(self, acid):
        """Find index of aircraft id"""
        try:
            return self.id.index(acid.upper())
        except:
            return -1

    def setNoise(self, noise=None):
        """Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)"""
        if noise is None:
            return True, "Noise is currently " + ("on" if self.Turbulence.active else "off")

        self.Turbulence.SetNoise(noise)
        self.adsb.SetNoise(noise)
        return True

    def engchange(self, acid, engid):
        """Change of engines"""
        self.perf.engchange(acid, engid)
        return

    def move(self, idx, lat, lon, alt=None, hdg=None, casmach=None, vspd=None):
        self.lat[idx]      = lat
        self.lon[idx]      = lon

        if alt:
            self.alt[idx]   = alt
            self.apalt[idx] = alt

        if hdg:
            self.hdg[idx]  = hdg
            self.ap.trk[idx] = hdg

        if casmach:
            self.tas[idx], self.aspd[-1], dummy = casormach(casmach, alt)

        if vspd:
            self.vs[idx]       = vspd
            self.swvnav[idx] = False

    def nom(self, idx):
        """ Reset acceleration back to nominal (1 kt/s^2): NOM acid """
        self.ax[idx] = kts

    def poscommand(self, idxorwp):# Show info on aircraft(int) or waypoint or airport (str)
        """POS command: Show info or an aircraft, airport, waypoint or navaid"""
        # Aircraft index
        if type(idxorwp)==int and idxorwp >= 0:

            idx           = idxorwp
            acid          = self.id[idx]
            actype        = self.type[idx]
            latlon        = latlon2txt(self.lat[idx], self.lon[idx])
            alt           = round(self.alt[idx] / ft)
            hdg           = round(self.hdg[idx])
            trk           = round(self.trk[idx])
            cas           = round(self.cas[idx] / kts)
            tas           = round(self.tas[idx] / kts)
            gs            = round(self.gs[idx]/kts)
            M             = self.M[idx]
            VS            = round(self.vs[idx]/ft*60.)
            route         = self.ap.route[idx]

            # Position report

            lines = "Info on %s %s index = %d\n" %(acid, actype, idx)     \
                  + "Pos: "+latlon+ "\n"                                  \
                  + "Hdg: %03d   Trk: %03d\n"        %(hdg, trk)              \
                  + "Alt: %d ft  V/S: %d fpm\n"  %(alt,VS)                \
                  + "CAS/TAS/GS: %d/%d/%d kts   M: %.3f\n"%(cas,tas,gs,M)

            # FMS AP modes
            if self.swlnav[idx] and route.nwp > 0 and route.iactwp >= 0:

                if self.swvnav[idx]:
                    lines = lines + "VNAV, "

                lines += "LNAV to " + route.wpname[route.iactwp] + "\n"

            # Flight info: Destination and origin
            if self.ap.orig[idx] != "" or self.ap.dest[idx] != "":
                lines = lines +  "Flying"

                if self.ap.orig[idx] != "":
                    lines = lines +  " from " + self.ap.orig[idx]

                if self.ap.dest[idx] != "":
                    lines = lines +  " to " + self.ap.dest[idx]

            # Show a/c info and highlight route of aircraft in radar window
            # and pan to a/c (to show route)
            return bs.scr.showacinfo(acid,lines)

        # Waypoint: airport, navaid or fix
        else:
            wp = idxorwp.upper()

            # Reference position for finding nearest
            reflat = bs.scr.ctrlat
            reflon = bs.scr.ctrlon

            lines = "Info on "+wp+":\n"

            # First try airports (most used and shorter, hence faster list)
            iap = bs.navdb.getaptidx(wp)
            if iap>=0:
                aptypes = ["large","medium","small"]
                lines = lines + bs.navdb.aptname[iap]+"\n"                 \
                        + "is a "+ aptypes[max(-1,bs.navdb.aptype[iap]-1)] \
                        +" airport at:\n"                                    \
                        + latlon2txt(bs.navdb.aptlat[iap],                 \
                                     bs.navdb.aptlon[iap]) + "\n"          \
                        + "Elevation: "                                      \
                        + str(int(round(bs.navdb.aptelev[iap]/ft)))        \
                        + " ft \n"

               # Show country name
                try:
                    ico = bs.navdb.cocode2.index(bs.navdb.aptco[iap].upper())
                    lines = lines + "in "+bs.navdb.coname[ico]+" ("+      \
                             bs.navdb.aptco[iap]+")"
                except:
                    ico = -1
                    lines = lines + "Country code: "+bs.navdb.aptco[iap]
                try:
                    rwytxt = str(bs.navdb.rwythresholds[bs.navdb.aptid[iap]].keys())
                    lines = lines + "\nRunways: " +rwytxt.strip("[]").replace("'","")
                except:
                    pass

            # Not found as airport, try waypoints & navaids
            else:
                iwps = bs.navdb.getwpindices(wp,reflat,reflon)
                if iwps[0]>=0:
                    typetxt = ""
                    desctxt = ""
                    lastdesc = "XXXXXXXX"
                    for i in iwps:

                        # One line type text
                        if typetxt == "":
                            typetxt = typetxt+bs.navdb.wptype[i]
                        else:
                            typetxt = typetxt+" and "+bs.navdb.wptype[i]

                        # Description: multi-line
                        samedesc = bs.navdb.wpdesc[i]==lastdesc
                        if desctxt == "":
                            desctxt = desctxt +bs.navdb.wpdesc[i]
                            lastdesc = bs.navdb.wpdesc[i]
                        elif not samedesc:
                            desctxt = desctxt +"\n"+bs.navdb.wpdesc[i]
                            lastdesc = bs.navdb.wpdesc[i]

                        # Navaid: frequency
                        if bs.navdb.wptype[i] in ["VOR","DME","TACAN"] and not samedesc:
                            desctxt = desctxt + " "+ str(bs.navdb.wpfreq[i])+" MHz"
                        elif bs.navdb.wptype[i]=="NDB" and not samedesc:
                            desctxt = desctxt+ " " + str(bs.navdb.wpfreq[i])+" kHz"

                    iwp = iwps[0]

                    # Basic info
                    lines = lines + wp +" is a "+ typetxt       \
                           + " at\n"\
                           + latlon2txt(bs.navdb.wplat[iwp],  \
                                        bs.navdb.wplon[iwp])
                    # Navaids have description
                    if len(desctxt)>0:
                        lines = lines+ "\n" + desctxt

                    # VOR give variation
                    if bs.navdb.wptype[iwp]=="VOR":
                        lines = lines + "\nVariation: "+ \
                                     str(bs.navdb.wpvar[iwp])+" deg"


                    # How many others?
                    nother = bs.navdb.wpid.count(wp)-len(iwps)
                    if nother>0:
                        verb = ["is ","are "][min(1,max(0,nother-1))]
                        lines = lines +"\nThere "+verb + str(nother) +\
                                   " other waypoint(s) also named " + wp

                    # In which airways?
                    connect = bs.navdb.listconnections(wp, \
                                                bs.navdb.wplat[iwp],
                                                bs.navdb.wplon[iwp])
                    if len(connect)>0:
                        awset = set([])
                        for c in connect:
                            awset.add(c[0])

                        lines = lines+"\nAirways: "+"-".join(awset)


               # Try airway id
                else:  # airway
                    awid = wp
                    airway = bs.navdb.listairway(awid)
                    if len(airway)>0:
                        lines = ""
                        for segment in airway:
                            lines = lines+"Airway "+ awid + ": " + \
                                    " - ".join(segment)+"\n"
                        lines = lines[:-1] # cut off final newline
                    else:
                        return False,idxorwp+" not found as a/c, airport, navaid or waypoint"

            # Show what we found on airport and navaid/waypoint
            bs.scr.echo(lines)

        return True

    def airwaycmd(self,key=""):
        # Show conections of a waypoint

        reflat = bs.scr.ctrlat
        reflon = bs.scr.ctrlon

        if key=="":
            return False,'AIRWAY needs waypoint or airway'

        if bs.navdb.awid.count(key)>0:
            return self.poscommand(key.upper())
        else:
            # Find connecting airway legs
            wpid = key.upper()
            iwp = bs.navdb.getwpidx(wpid,reflat,reflon)
            if iwp<0:
                return False,key + " not found."

            wplat = bs.navdb.wplat[iwp]
            wplon = bs.navdb.wplon[iwp]
            connect = bs.navdb.listconnections(key.upper(),wplat,wplon)
            if len(connect)>0:
                lines = ""
                for c in connect:
                    if len(c)>=2:
                        # Add airway, direction, waypoint
                        lines = lines+ c[0]+": to "+c[1]+"\n"
                return True, lines[:-1]  # exclude final newline
            else:
                return False,"No airway legs found for ",key