def create(self, n=1): super(Pilot, self).create(n) self.alt[-n:] = bs.traf.alt[-n:] self.spd[-n:] = vtas2eas(bs.traf.tas[-n:], bs.traf.alt[-n:]) self.hdg[-n:] = bs.traf.hdg[-n:] self.trk[-n:] = bs.traf.trk[-n:]
def resolve(self, conf, ownship, intruder): ''' Resolve all current conflicts ''' # required change in velocity dv = np.zeros((ownship.ntraf, 3)) for ((ac1, ac2), qdr, dist, tcpa, tLOS) in zip(conf.confpairs, conf.qdr, conf.dist, conf.tcpa, conf.tLOS): idx1 = ownship.id.index(ac1) idx2 = intruder.id.index(ac2) if idx1 > -1 and idx2 > -1: dv_eby = self.Eby_straight( ownship, intruder, conf, qdr, dist, tcpa, tLOS, idx1, idx2) dv[idx1] -= dv_eby dv[idx2] += dv_eby # now we have the change in speed vector for each aircraft. dv=np.transpose(dv) # the old speed vector, cartesian coordinates trkrad = np.radians(ownship.trk) v = np.array([np.sin(trkrad) * ownship.tas,\ np.cos(trkrad)*ownship.tas,\ ownship.vs]) # the new speed vector newv = dv + v # the new speed vector in polar coordinates newtrack = (np.arctan2(newv[0,:], newv[1,:]) * 180 / np.pi) % 360 newgs = np.sqrt(newv[0,:] ** 2 + newv[1,:] ** 2) neweas = vtas2eas(newgs, ownship.alt) # Cap the velocity neweascapped = np.maximum(ownship.perf.vmin, np.minimum(ownship.perf.vmax, neweas)) return newtrack, neweascapped, newv[2, :], np.sign(newv[2, :]) * 1e5
def resolve(asas, traf): if not asas.swasas: return # required change in velocity dv = np.zeros((traf.ntraf, 3)) #if possible, solve conflicts once and copy results for symmetrical conflicts, #if that is not possible, solve each conflict twice, once for each A/C if not traf.ADSBtrunc and not traf.ADSBtransnoise: for conflict in asas.conflist_now: id1, id2 = asas.ConflictToIndices(conflict) if id1 != "Fail" and id2 != "Fail": dv_eby = Eby_straight(traf, asas, id1, id2) dv[id1] -= dv_eby dv[id2] += dv_eby else: for i in range(asas.nconf): confpair = asas.confpairs[i] ac1 = confpair[0] ac2 = confpair[1] id1 = traf.id.index(ac1) id2 = traf.id.index(ac2) dv_eby = Eby_straight(asas, id1, id2) dv[id1] -= dv_eby # now we have the change in speed vector for each aircraft. dv=np.transpose(dv) # the old speed vector, cartesian coordinates trkrad=np.radians(traf.trk) v=np.array([np.sin(trkrad)*traf.tas,\ np.cos(trkrad)*traf.tas,\ traf.vs]) # the new speed vector newv=dv+v # the new speed vector in polar coordinates newtrack=(np.arctan2(newv[0,:],newv[1,:])*180/np.pi) %360 newgs=np.sqrt(newv[0,:]**2+newv[1,:]**2) neweas=vtas2eas(newgs,traf.alt) # Cap the velocity neweascapped=np.maximum(asas.vmin,np.minimum(asas.vmax,neweas)) # now assign in the traf class asas.hdg = newtrack asas.tas = neweascapped asas.vs = newv[2,:] asas.alt = np.sign(asas.vs)*1e5
def resolve(asas, traf): if not asas.swasas: return # required change in velocity dv = np.zeros((traf.ntraf, 3)) #if possible, solve conflicts once and copy results for symmetrical conflicts, #if that is not possible, solve each conflict twice, once for each A/C if not traf.ADSBtrunc and not traf.ADSBtransnoise: for conflict in asas.conflist_now: id1, id2 = asas.ConflictToIndices(conflict) if id1 != "Fail" and id2 != "Fail": dv_eby = Eby_straight(traf, asas, id1, id2) dv[id1] -= dv_eby dv[id2] += dv_eby else: for i in range(asas.nconf): confpair = asas.confpairs[i] ac1 = confpair[0] ac2 = confpair[1] id1 = traf.id.index(ac1) id2 = traf.id.index(ac2) dv_eby = Eby_straight(asas, id1, id2) dv[id1] -= dv_eby # now we have the change in speed vector for each aircraft. dv = np.transpose(dv) # the old speed vector, cartesian coordinates trkrad = np.radians(traf.trk) v=np.array([np.sin(trkrad)*traf.tas,\ np.cos(trkrad)*traf.tas,\ traf.vs]) # the new speed vector newv = dv + v # the new speed vector in polar coordinates newtrack = (np.arctan2(newv[0, :], newv[1, :]) * 180 / np.pi) % 360 newgs = np.sqrt(newv[0, :]**2 + newv[1, :]**2) neweas = vtas2eas(newgs, traf.alt) # Cap the velocity neweascapped = np.maximum(asas.vmin, np.minimum(asas.vmax, neweas)) # now assign in the traf class asas.hdg = newtrack asas.tas = neweascapped asas.vs = newv[2, :] asas.alt = np.sign(asas.vs) * 1e5
def resolve(dbconf, traf): # ------------------------------------------------------------------------- # First, perform special Conflict Detection to find which conflicts to resolve # Relative horizontal positions: p[a,b] is p from a to b qdrrel = np.radians(dbconf.qdr-traf.trk.reshape((len(traf.trk),1))) xrel= np.sin(qdrrel)*dbconf.dist yrel= np.cos(qdrrel)*dbconf.dist # Define the two borders: b1=np.where( yrel -np.abs(xrel) > -dbconf.dist_a ,True,False) b2=np.where( xrel**2 + (yrel-dbconf.dist_b)**2 < (dbconf.dist_a+dbconf.dist_b)**2 ,True,False) # Create conflict matrix dbconf.swconfl_dg = b1*b2*(1.-np.eye(traf.ntraf)) # ------------------------------------------------------------------------- # Second, create a list of conflicts and aircraft indices to iterate over dgconfidxs = np.where(dbconf.swconfl_dg) dgnconf = len(dgconfidxs[0]) dgiown = dgconfidxs[0] dgioth = dgconfidxs[1] # ------------------------------------------------------------------------- # Third, create control matrix and find controls # Also, aircraft in DGconflict should follow ASAS dbconf.asasactive.fill(False) controls=np.zeros((traf.ntraf,3),dtype=np.int) for i in range(dgnconf): id1=dgiown[i] id2=dgioth[i] dbconf.asasactive[id1]=True dbconf.asasactive[id2]=True ctrl=Difgamehor(traf, dbconf,id1,id2) controls[id1]+=ctrl-np.array([1,1,1]) # Now, write controls as limits in traf class # First: limit superpositioned controls to maximum values controls+=np.array([1,1,1]) maxcontrols=np.array([2,2,2]) controls=np.where(controls<0,0,controls) controls=np.where(controls>maxcontrols,maxcontrols,controls) # From indices to values acccontrol=dbconf.a_o[controls[:,0]] bankcontrol=dbconf.b_o[controls[:,1]] climbcontrol=traf.avsdef*np.sign(controls[:,2]) # Now assign in the traf class -------------------------------------------- # Change autopilot desired speed dbconf.asasspd=vtas2eas(np.where(acccontrol==0,traf.gs,\ np.where(acccontrol>0,dbconf.vmax,dbconf.vmin)),traf.alt) # Set acceleration for aircraft in conflict traf.ax=np.where(dbconf.asasactive,abs(acccontrol),kts) # Change autopilot desired heading dbconf.asashdg=np.where(bankcontrol==0,traf.trk,\ np.where(bankcontrol>0,traf.trk+90,traf.trk-90))%360 # Set bank angle for aircraft in conflict traf.aphi=np.radians(np.where(dbconf.asasactive,np.abs(bankcontrol),25.)) # Change autopilot desired altitude traf.aalt=np.where(climbcontrol==0,traf.alt,\ np.where(climbcontrol>0,1e9,-1e9)) # Set climb rate for aircraft in conflict dbconf.asasvsp=np.abs(climbcontrol)