def calcfp( self ): # Current Flight Plan calculations, which actualize based on flight condition """Do flight plan calculations""" # self.delwpt("T/D") # self.delwpt("T/C") # Direction to waypoint self.nwp = len(self.wpname) # Create flight plan calculation table self.wpdirfrom = self.nwp * [0.] self.wpdistto = self.nwp * [0.] self.wpialt = self.nwp * [-1] self.wptoalt = self.nwp * [-999.] self.wpxtoalt = self.nwp * [1.] # Avoid division by zero self.wpirta = self.nwp * [-1] self.wptorta = self.nwp * [-999.] self.wpxtorta = self.nwp * [1.] #[m] Avoid division by zero # No waypoints: make empty variables to be safe and return: nothing to do if self.nwp == 0: return # Calculate lateral leg data # LNAV: Calculate leg distances and directions for i in range(0, self.nwp - 1): qdr, dist = geo.qdrdist(self.wplat[i], self.wplon[i], self.wplat[i + 1], self.wplon[i + 1]) self.wpdirfrom[i] = qdr self.wpdistto[i + 1] = dist #[nm] distto is in nautical miles if self.nwp > 1: self.wpdirfrom[-1] = self.wpdirfrom[-2] # Calculate longitudinal leg data # VNAV: calc next altitude constraint: index, altitude and distance to it ialt = -1 # index to waypoint with next altitude constraint toalt = -999. # value of next altitude constraint xtoalt = 0. # distance to next altitude constraint from this wp for i in range(self.nwp - 1, -1, -1): # waypoint with altitude constraint (dest of al specified) if self.wptype[i] == Route.dest: ialt = i toalt = 0. xtoalt = 0. # [m] elif self.wpalt[i] >= 0: ialt = i toalt = self.wpalt[i] xtoalt = 0. # [m] # waypoint with no altitude constraint:keep counting else: if i != self.nwp - 1: xtoalt = xtoalt + self.wpdistto[ i + 1] * nm # [m] xtoalt is in meters! else: xtoalt = 0.0 self.wpialt[i] = ialt self.wptoalt[i] = toalt #[m] self.wpxtoalt[i] = xtoalt #[m] # RTA: calc next rta constraint: index, altitude and distance to it # If any RTA. if any(array(self.wprta) >= 0.0): #print("Yes, I found RTAs") irta = -1 # index of wp torta = -999. # next rta value xtorta = 0. # distance to next rta for i in range(self.nwp - 1, -1, -1): # waypoint with rta: reset counter, update rts if self.wprta[i] >= 0: irta = i torta = self.wprta[i] xtorta = 0. # [m] # waypoint with no altitude constraint:keep counting else: if i != self.nwp - 1: # No speed or rta constraint: add to xtorta if self.wpspd[i] <= 0.0: xtorta = xtorta + self.wpdistto[ i + 1] * nm # [m] xtoalt is in meters! else: # speed constraint on this leg: shift torta to account for this # altitude unknown if self.wptoalt[i] > 0.: alt = toalt else: # TODO: current a/c altitude would be better guess, but not accessible here # as we do not know aircraft index for this route alt = 10000. * ft # default to minimize errors, when no alt constraints are present legtas = casormach2tas(self.wpspd[i], alt) #TODO: account for wind at this position vy adding wind vectors to waypoints? # xtorta stays the same! This leg will not be available for RTA scheduling, so distance # is not in xtorta. Therefore we need to subtract legtime to ignore this leg for the RTA # scheduling legtime = self.wpdistto[i + 1] / legtas torta = torta - legtime else: xtorta = 0.0 torta = -999.0 self.wpirta[i] = irta self.wptorta[i] = torta # [s] self.wpxtorta[i] = xtorta # [m]
def creconfs(self, acid, actype, targetidx, dpsi, dcpa, tlosh, dH=None, tlosv=None, spd=None): ''' Create an aircraft in conflict with target aircraft. Arguments: - acid: callsign of new aircraft - actype: aircraft type of new aircraft - targetidx: id (callsign) of target aircraft - dpsi: Conflict angle (angle between tracks of ownship and intruder) (deg) - cpa: Predicted distance at closest point of approach (NM) - tlosh: Horizontal time to loss of separation ((hh:mm:)sec) - dH: Vertical distance (ft) - tlosv: Vertical time to loss of separation - spd: Speed of new aircraft (CAS/Mach, kts/-) ''' 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 tasref = self.tas[targetidx] # m/s vsref = self.vs[targetidx] # m/s cpa = dcpa * nm pzr = bs.settings.asas_pzr * nm pzh = bs.settings.asas_pzh * ft trk = trkref + radians(dpsi) 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 if spd: # CAS or Mach provided: convert to groundspeed, assuming that # wind at intruder position is similar to wind at ownship position tas = tasref if spd is None else casormach2tas(spd, acalt) tasn, tase = tas * cos(trk), tas * sin(trk) wn, we = self.wind.getdata(latref, lonref, acalt) gsn, gse = tasn + wn, tase + we else: # Groundspeed is the same as ownship gsn, gse = gsref * cos(trk), gsref * sin(trk) # Horizontal relative velocity vector 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.kwikpos(latref, lonref, brn, dist / nm) # convert groundspeed to CAS, and track to heading using actual # intruder position wn, we = self.wind.getdata(aclat, aclon, acalt) tasn, tase = gsn - wn, gse - we acspd = tas2cas(sqrt(tasn * tasn + tase * tase), acalt) achdg = degrees(atan2(tase, tasn)) # Create and, when necessary, set vertical speed self.cre(acid, actype, aclat, aclon, achdg, acalt, acspd) self.ap.selaltcmd(len(self.lat) - 1, altref, acvs) self.vs[-1] = acvs