def Reached(self, qdr, dist, flyby): # 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 = bs.traf.tas * bs.traf.tas / \ np.maximum(bs.traf.eps, np.tan(bs.traf.bank) * g0 * nm) # [nm] next_qdr = np.where(self.next_qdr < -900., qdr, self.next_qdr) # Avoid circling # away = np.abs(degto180(bs.traf.trk - next_qdr)+180.)>90. # away = np.abs(degto180(bs.traf.trk - next_qdr))>90. away = np.abs(degto180(bs.traf.trk%360. - qdr%360.))>90. incircle = dist<turnrad*1.01 circling = away*incircle # distance to turn initialisation point [nm] self.turndist = flyby*np.minimum(100., np.abs(turnrad * np.tan(np.radians(0.5 * np.abs(degto180(qdr%360. - next_qdr%360.)))))) # Check whether shift based dist [nm] is required, set closer than WP turn distanc return np.where(bs.traf.swlnav * ((dist < self.turndist)+circling))[0]
def Reached(self, qdr, dist, flyby): # 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 = bs.traf.tas * bs.traf.tas / \ (np.maximum(bs.traf.eps, np.tan(bs.traf.bank)) * g0) \ # Turn radius in meters! next_qdr = np.where(self.next_qdr < -900., qdr, self.next_qdr) # Avoid circling by checking for flying away away = np.abs(degto180(bs.traf.trk % 360. - qdr % 360.)) > 90. # difference large than 90 # Ratio between distance close enough to switch to next wp when flying away # When within pro1 nm and flying away: switch also proxfact = 1.01 # Turnradius scales this contant , factor => [turnrad] incircle = dist < turnrad * proxfact circling = away * incircle # [True/False] passed wp,used for flyover as well # distance to turn initialisation point [m] self.turndist = flyby * np.abs(turnrad * np.tan( np.radians(0.5 * np.abs(degto180(qdr % 360. - next_qdr % 360.))))) # Check whether shift based dist is required, set closer than WP turn distance swreached = np.where(bs.traf.swlnav * ((dist < self.turndist) + circling))[0] # Return True/1.0 for a/c where we have reached waypoint return swreached
def update_geovector(self): ''' Update traffic according to geovector rules ''' # Check if any geovectors are defined in the first place, return if not if not bool(geovector.geovecs): return # Retrieve vectorized limits for each aircraft in the current timestep gsmin, gsmax, trkmin, trkmax, vsmin, vsmax = get_limits() # Convert ground speed to calibrated airspeed casmin = vtas2cas(np.ones(traf.ntraf) * gsmin, traf.alt) casmax = vtas2cas(np.ones(traf.ntraf) * gsmax, traf.alt) # Limit selected airspeed where setting exceeds limit self.spd = np.fmax(self.spd, casmin) self.spd = np.fmin(self.spd, casmax) # Limit selected track where setting exceeds limit self.trk = np.where(degto180(self.trk - trkmin) < 0, trkmin, self.trk) # left of minimum self.trk = np.where(degto180(self.trk - trkmax) > 0, trkmax, self.trk) # right of maximum # Check which aircraft breach the vertical speed limits vsbreach = np.logical_or(self.vs < vsmin, self.vs > vsmax) # Limit selected vertical speed where setting exceeds limit self.vs = np.fmax(self.vs, vsmin) self.vs = np.fmin(self.vs, vsmax) # If vs is not zero but aircraft already at altitude, altitude needs to be changed changealt = np.logical_and(np.abs(self.vs) > 0., (self.alt - traf.alt) < 1e-6) # Only change altitude when vs limit will be breached self.alt = np.where(np.logical_and(vsbreach, changealt), traf.alt + 100*self.vs, self.alt)
def Reached(self, qdr, dist, flyby): # 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 = bs.traf.tas * bs.traf.tas / \ (np.maximum(bs.traf.eps, np.tan(bs.traf.bank)) * g0) \ # Turn radius in meters! next_qdr = np.where(self.next_qdr < -900., qdr, self.next_qdr) # Avoid circling by checking for flying away away = np.abs(degto180(bs.traf.trk%360. - qdr%360.)) > 90. # difference large than 90 # Ratio between distance close enough to switch to next wp when flying away # When within pro1 nm and flying away: switch also proxfact = 1.01 # Turnradius scales this contant , factor => [turnrad] incircle = dist<turnrad*proxfact circling = away*incircle # [True/False] passed wp,used for flyover as well # distance to turn initialisation point [m] self.turndist = flyby*np.abs(turnrad * np.tan(np.radians(0.5 * np.abs(degto180(qdr%360. - next_qdr%360.))))) # Check whether shift based dist is required, set closer than WP turn distance swreached = np.where(bs.traf.swlnav * ((dist < self.turndist)+circling))[0] # Return True/1.0 for a/c where we have reached waypoint return swreached
def Reached(self, qdr, dist, flyby): # 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 = bs.traf.tas * bs.traf.tas / \ np.maximum(bs.traf.eps, np.tan(bs.traf.bank) * g0 * nm) # [nm] next_qdr = np.where(self.next_qdr < -900., qdr, self.next_qdr) # Avoid circling # away = np.abs(degto180(bs.traf.trk - next_qdr)+180.)>90. # away = np.abs(degto180(bs.traf.trk - next_qdr))>90. away = np.abs(degto180(bs.traf.trk % 360. - qdr % 360.)) > 90. incircle = dist < turnrad * 1.01 circling = away * incircle # distance to turn initialisation point [nm] self.turndist = flyby * np.minimum( 100., np.abs(turnrad * np.tan( np.radians( 0.5 * np.abs(degto180(qdr % 360. - next_qdr % 360.)))))) # Check whether shift based dist [nm] is required, set closer than WP turn distanc return np.where(bs.traf.swlnav * ((dist < self.turndist) + circling))[0]
def applygeovec(): # Apply each geovector for vec in geovecs: areaname = vec[0] if areafilter.hasArea(areaname): swinside = areafilter.checkInside(areaname, traf.lat, traf.lon, traf.alt) gsmin, gsmax, trkmin, trkmax, vsmin, vsmax = vec[1:] # -----Ground speed limiting # For now assume no wind: so use tas as gs if gsmin: casmin = vtas2cas(np.ones(traf.ntraf) * gsmin, traf.alt) usemin = traf.selspd < casmin traf.selspd[swinside & usemin] = casmin[swinside & usemin] if gsmax: casmax = vtas2cas(np.ones(traf.ntraf) * gsmax, traf.alt) usemax = traf.selspd > casmax traf.selspd[swinside & usemax] = casmax[swinside & usemax] #------ Limit Track(so hdg) # Max track interval is 180 degrees to avoid ambiguity of what is inside the interval if trkmin and trkmax: # Use degto180 to avodi problems for e.g interval [350,30] usemin = swinside & (degto180(traf.trk - trkmin) < 0 ) # Left of minimum usemax = swinside & (degto180(traf.trk - trkmax) > 0 ) # Right of maximum #print(usemin,usemax) traf.ap.trk[swinside & usemin] = trkmin traf.ap.trk[swinside & usemax] = trkmax # -----Ground speed limiting # For now assume no wind: so use tas as gs if vsmin: traf.selvs[swinside & (traf.vs < vsmin)] = vsmin # Activate V/S mode by using a slightly higher altitude than current values traf.selalt[swinside & (traf.vs < vsmin)] = traf.alt[swinside & (traf.vs < vsmin)] + \ np.sign(vsmin)*200.*ft if vsmax: traf.selvs[swinside & (traf.vs > vsmax)] = vsmax # Activate V/S mode by using a slightly higher altitude than current values traf.selalt[swinside & (traf.vs > vsmax)] = traf.alt[swinside & (traf.vs > vsmax)] + \ np.sign(vsmax)*200.*ft return
def applygeovec(): # Apply each geovector for vec in geovecs: areaname = vec[0] if areafilter.hasArea(areaname): swinside = areafilter.checkInside(areaname, traf.lat, traf.lon, traf.alt) gsmin,gsmax,trkmin,trkmax,vsmin,vsmax = vec[1:] # -----Ground speed limiting # For now assume no wind: so use tas as gs if gsmin: casmin = vtas2cas(np.ones(traf.ntraf)*gsmin,traf.alt) usemin = traf.selspd<casmin traf.selspd[swinside & usemin] = casmin[swinside & usemin] if gsmax: casmax = vtas2cas(np.ones(traf.ntraf)*gsmax,traf.alt) usemax = traf.selspd > casmax traf.selspd[swinside & usemax] = casmax[swinside & usemax] #------ Limit Track(so hdg) # Max track interval is 180 degrees to avoid ambiguity of what is inside the interval if trkmin and trkmax: # Use degto180 to avodi problems for e.g interval [350,30] usemin = swinside & (degto180(traf.trk - trkmin)<0) # Left of minimum usemax = swinside & (degto180(traf.trk - trkmax)>0) # Right of maximum #print(usemin,usemax) traf.ap.trk[swinside & usemin] = trkmin traf.ap.trk[swinside & usemax] = trkmax # -----Ground speed limiting # For now assume no wind: so use tas as gs if vsmin: traf.selvs[swinside & (traf.vs<vsmin)] = vsmin # Activate V/S mode by using a slightly higher altitude than current values traf.selalt[swinside & (traf.vs < vsmin)] = traf.alt[swinside & (traf.vs < vsmin)] + \ np.sign(vsmin)*200.*ft if vsmax: traf.selvs[swinside & (traf.vs > vsmax)] = vsmax # Activate V/S mode by using a slightly higher altitude than current values traf.selalt[swinside & (traf.vs > vsmax)] = traf.alt[swinside & (traf.vs > vsmax)] + \ np.sign(vsmax)*200.*ft return
def get_limits(): ''' Get active geovector limits per aircraft (vectorized) ''' # Initialize arrays for active limit values per aircraft traf_gsmin = np.full(traf.ntraf, np.nan) traf_gsmax = np.full(traf.ntraf, np.nan) traf_trkmin = np.full(traf.ntraf, np.nan) traf_trkmax = np.full(traf.ntraf, np.nan) traf_vsmin = np.full(traf.ntraf, np.nan) traf_vsmax = np.full(traf.ntraf, np.nan) # Determine active limits for each aircraft (if any) for geoname, geovec in geovector.geovecs.items(): # Create arrays with limit values gsmin = np.full(traf.ntraf, geovec.gsmin if geovec.gsmin is not None else np.nan) gsmax = np.full(traf.ntraf, geovec.gsmax if geovec.gsmax is not None else np.nan) trkmin = np.full(traf.ntraf, geovec.trkmin if geovec.trkmin is not None else np.nan) trkmax = np.full(traf.ntraf, geovec.trkmax if geovec.trkmax is not None else np.nan) vsmin = np.full(traf.ntraf, geovec.vsmin if geovec.vsmin is not None else np.nan) vsmax = np.full(traf.ntraf, geovec.vsmax if geovec.vsmax is not None else np.nan) # Determine which aircraft are inside the current geovector swinside = checkInside(geoname, traf.lat, traf.lon, traf.alt) # Set limits for each aircraft # The limits depend on the location of each aircraft # If an aircraft is subject to multiple geovectors: the intersection remains traf_gsmin[swinside] = np.fmax(traf_gsmin[swinside], gsmin[swinside]) traf_gsmax[swinside] = np.fmin(traf_gsmax[swinside], gsmax[swinside]) traf_trkmin[swinside] = np.where(np.invert(degto180(trkmin[swinside] - traf_trkmin[swinside]) < 0),\ trkmin[swinside], traf_trkmin[swinside]) traf_trkmax[swinside] = np.where(np.invert(degto180(trkmax[swinside] - traf_trkmax[swinside]) > 0),\ trkmax[swinside], traf_trkmax[swinside]) traf_vsmin[swinside] = np.fmax(traf_vsmin[swinside], vsmin[swinside]) traf_vsmax[swinside] = np.fmin(traf_vsmax[swinside], vsmax[swinside]) # Check if the intersection of geovectors is empty (set of allowed velocity vectors is empty) empt = np.logical_or(traf_gsmin > traf_gsmax, traf_vsmin > traf_vsmax) # check if min spd limit > max spd limit empt = np.logical_or(empt, degto180(traf_trkmin - traf_trkmax) > 0) # check if min trk limit > max trk limit # For aircraft with empty intersection of geovectors, set limits to nan (hence ignore all limits) # In this case it is not clear which limits to adhere to traf_gsmin[empt] = np.nan traf_gsmax[empt] = np.nan traf_trkmin[empt] = np.nan traf_trkmax[empt] = np.nan traf_vsmin[empt] = np.nan traf_vsmax[empt] = np.nan return traf_gsmin, traf_gsmax, traf_trkmin, traf_trkmax, traf_vsmin, traf_vsmax
def findact(self, i): """ Find best default active waypoint. This function is called during route creation""" # print "findact is called.!" # Check for easy answers first if self.nwp <= 0: return -1 elif self.nwp == 1: return 0 # Find closest wplat = array(self.wplat) wplon = array(self.wplon) dy = wplat - bs.traf.lat[i] dx = (wplon - bs.traf.lon[i]) * bs.traf.coslat[i] dist2 = dx * dx + dy * dy iwpnear = argmin(dist2) #Unless behind us, next waypoint? if iwpnear + 1 < self.nwp: qdr = degrees(arctan2(dx[iwpnear], dy[iwpnear])) delhdg = abs(degto180(bs.traf.trk[i] - qdr)) # we only turn to the first waypoint if we can reach the required # heading before reaching the waypoint time_turn = max(0.01, bs.traf.tas[i]) * radians(delhdg) / ( g0 * tan(bs.traf.bank[i])) time_straight = dist2[iwpnear] * nm / max(0.01, bs.traf.tas[i]) if time_turn > time_straight: iwpnear = iwpnear + 1 return iwpnear
def step(self): self.actnum += 1 prev_state = self.state # Update state qdr, dist = qdrdist(traf.lat[self.acidx], traf.lon[self.acidx], traf.ap.route[self.acidx].wplat[-2], traf.ap.route[self.acidx].wplon[-2]) t = agent.sta - ETA(agent.acidx) print('STA {}, ETA {}, t {}'.format(agent.sta, ETA(agent.acidx), t)) hdg_rel = degto180(qdr - traf.hdg[agent.acidx]) # self.state = np.array([dist, t, hdg_ref, # traf.tas[agent.acidx]]) self.state = np.array([dist, t, hdg_rel / 180.]) # Check episode termination if dist < 1 or agent.sta - sim.simt < -60: if agent.epsilon > agent.epsilon_min: agent.epsilon -= 0.9 / 1000. self.done = True env.reset() reward = self.gen_reward() print('State {}'.format(self.state)) print('Reward {}, epsilon {}'.format(reward, agent.epsilon)) if train_phase: self.log() return self.state, reward, self.done, prev_state
def Reached(self, qdr, dist, flyby, flyturn, turnradnm): # 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 # First calculate turn distance next_qdr = np.where(self.next_qdr < -900., qdr, self.next_qdr) flybyturndist, turnrad = self.calcturn(bs.traf.tas, bs.traf.bank, qdr, next_qdr, turnradnm) self.turndist = np.logical_or(flyby, flyturn) * flybyturndist # Avoid circling by checking for flying away away = np.abs(degto180(bs.traf.trk % 360. - qdr % 360.)) > 90. # difference large than 90 # Ratio between distance close enough to switch to next wp when flying away # When within pro1 nm and flying away: switch also proxfact = 1.02 # Turnradius scales this contant , factor => [turnrad] incircle = dist < turnrad * proxfact circling = away * incircle # [True/False] passed wp,used for flyover as well # Check whether shift based dist is required, set closer than WP turn distance swreached = np.where(bs.traf.swlnav * ((dist < self.turndist) + circling))[0] # Return True/1.0 for a/c where we have reached waypoint return swreached
def findact(self,i): """ Find best default active waypoint. This function is called during route creation""" # print "findact is called.!" # Check for easy answers first if self.nwp<=0: return -1 elif self.nwp == 1: return 0 # Find closest wplat = array(self.wplat) wplon = array(self.wplon) dy = (wplat - bs.traf.lat[i]) dx = (wplon - bs.traf.lon[i]) * bs.traf.coslat[i] dist2 = dx*dx + dy*dy iwpnear = argmin(dist2) #Unless behind us, next waypoint? if iwpnear+1<self.nwp: qdr = degrees(arctan2(dx[iwpnear],dy[iwpnear])) delhdg = abs(degto180(bs.traf.trk[i]-qdr)) # we only turn to the first waypoint if we can reach the required # heading before reaching the waypoint time_turn = max(0.01,bs.traf.tas[i])*radians(delhdg)/(g0*tan(bs.traf.bank[i])) time_straight= sqrt(dist2[iwpnear])*60.*nm/max(0.01,bs.traf.tas[i]) if time_turn > time_straight: iwpnear += 1 return iwpnear
def calcturn(self,tas,bank,wpqdr,next_wpqdr): # Turn radius in meters! turnrad = tas * tas / (np.maximum(0.01, np.tan(bank)) * g0) # turndist in meters turndist = np.abs(turnrad * np.tan(np.radians(0.5 * np.abs(degto180(wpqdr%360. - next_wpqdr%360.))))) return turndist,turnrad
def log(self): dist = self.state[0] t = self.state[1] hdg = self.state[2] hdg_ref = 60. hdg_diff = (degto180(hdg_ref - hdg)) f = open(self.fname, 'a') f.write("{};{};{};{};{};{};{};{};{};{};{}\n".format(self.ep, self.actnum, self.reward, dist, t, hdg_diff, agent.sta, sim.simt, traf.lat[self.acidx], traf.lon[self.acidx], agent.epsilon)) f.close()
def update_data(self): if self.limtype == 'gsmin': value = self.limval - traf.gs[traf.id2idx(self.acid)] elif self.limtype == 'gsmax': value = traf.gs[traf.id2idx(self.acid)] - self.limval elif self.limtype == 'trkmin': value = degto180(self.limval - traf.trk[traf.id2idx(self.acid)]) elif self.limtype == 'trkmax': value = degto180(traf.trk[traf.id2idx(self.acid)] - self.limval) elif self.limtype == 'vsmin': value = self.limval - traf.vs[traf.id2idx(self.acid)] elif self.limtype == 'vsmax': value = traf.vs[traf.id2idx(self.acid)] - self.limval # Append new value to breachvals self.breachvals = np.append(self.breachvals, value)
def direct(self, idx, wpnam): """Set active point to a waypoint by name""" name = wpnam.upper().strip() if name != "" and self.wpname.count(name) > 0: wpidx = self.wpname.index(name) self.iactwp = wpidx bs.traf.actwp.lat[idx] = self.wplat[wpidx] bs.traf.actwp.lon[idx] = self.wplon[wpidx] bs.traf.actwp.flyby[idx] = self.wpflyby[wpidx] self.calcfp() bs.traf.ap.ComputeVNAV(idx, self.wptoalt[wpidx], self.wpxtoalt[wpidx]) # If there is a speed specified, process it if self.wpspd[wpidx]>0.: # Set target speed for autopilot if self.wpalt[wpidx] < 0.0: alt = bs.traf.alt[idx] else: alt = self.wpalt[wpidx] # Check for valid Mach or CAS if self.wpspd[wpidx] <2.0: cas = mach2cas(self.wpspd[wpidx], alt) else: cas = self.wpspd[wpidx] # Save it for next leg bs.traf.actwp.spd[idx] = cas # When already in VNAV: fly it if bs.traf.swvnav[idx]: bs.traf.selspd[idx]=cas # No speed specified for next leg else: bs.traf.actwp.spd[idx] = -999. qdr, dist = geo.qdrdist(bs.traf.lat[idx], bs.traf.lon[idx], bs.traf.actwp.lat[idx], bs.traf.actwp.lon[idx]) turnrad = bs.traf.tas[idx]*bs.traf.tas[idx]/tan(radians(25.)) / g0 / nm # [nm]default bank angle 25 deg bs.traf.actwp.turndist[idx] = (bs.traf.actwp.flyby[idx] > 0.5) * \ turnrad*abs(tan(0.5*radians(max(5., abs(degto180(qdr - self.wpdirfrom[self.iactwp])))))) # [nm] bs.traf.swlnav[idx] = True return True else: return False, "Waypoint " + wpnam + " not found"
def direct(self, idx, wpnam): """Set active point to a waypoint by name""" name = wpnam.upper().strip() if name != "" and self.wpname.count(name) > 0: wpidx = self.wpname.index(name) self.iactwp = wpidx bs.traf.actwp.lat[idx] = self.wplat[wpidx] bs.traf.actwp.lon[idx] = self.wplon[wpidx] bs.traf.actwp.flyby[idx] = self.wpflyby[wpidx] self.calcfp() bs.traf.ap.ComputeVNAV(idx, self.wptoalt[wpidx], self.wpxtoalt[wpidx]) # If there is a speed specified, process it if self.wpspd[wpidx] > 0.: # Set target speed for autopilot if self.wpalt[wpidx] < 0.0: alt = bs.traf.alt[idx] else: alt = self.wpalt[wpidx] # Check for valid Mach or CAS if self.wpspd[wpidx] < 2.0: cas = mach2cas(self.wpspd[wpidx], alt) else: cas = self.wpspd[wpidx] # Save it for next leg bs.traf.actwp.spd[idx] = cas # When already in VNAV: fly it if bs.traf.swvnav[idx]: bs.traf.selspd[idx] = cas # No speed specified for next leg else: bs.traf.actwp.spd[idx] = -999. qdr, dist = geo.qdrdist(bs.traf.lat[idx], bs.traf.lon[idx], bs.traf.actwp.lat[idx], bs.traf.actwp.lon[idx]) turnrad = bs.traf.tas[idx] * bs.traf.tas[idx] / tan( radians(25.)) / g0 / nm # [nm]default bank angle 25 deg bs.traf.actwp.turndist[idx] = (bs.traf.actwp.flyby[idx] > 0.5) * \ turnrad*abs(tan(0.5*radians(max(5., abs(degto180(qdr - self.wpdirfrom[self.iactwp])))))) # [nm] bs.traf.swlnav[idx] = True return True else: return False, "Waypoint " + wpnam + " not found"
def update_geofence(self): ''' Update traffic according to geofence avoidance functionality ''' # Get geofence hits self.conlst = geofence.Geofence.detect_all(traf, self.dtlook) self.geoconf = np.array([any(con) for con in self.conlst]) # If any geofence conflicts are found, change heading accordingly if np.any(self.geoconf): # Get idx of all aircraft currently in conflict with a geofence and retrieve list of areas idxs = np.where(self.geoconf)[0] areaconfs = np.array(self.conlst)[self.geoconf] # Iterate over each aircraft in conflict with at least one geofence for idx, areaconf in zip(idxs, areaconfs): # First check if the aircraft is inside a geofence inside = np.array([area.checkInside(traf.lat[idx], traf.lon[idx], traf.alt[idx]) for area in areaconf]) # If already inside a geofence, use previously determined track to exit geofence if np.any(inside): self.trk[idx] = self.esc_trk[idx] else: # Make list of vertices for this specific aircraft vertices = [] # list to store all vertices [vertices.extend([coord for coord in area.coordinates]) for area in areaconf] # Compute bearings from aircraft to each of the vertices bearings = np.array([qdrdist(traf.lat[idx], traf.lon[idx], vertices[i], \ vertices[i+1])[0] for i in range(0, len(vertices), 2)]) # Find outer bearings and select closest to current FMS track diffs = degto180(traf.trk[idx] - bearings) outer = np.logical_or(diffs == np.min(diffs), diffs == np.max(diffs)) bearings = bearings[outer] diffs = np.absolute(degto180(self.trk[idx] - bearings)) # self.trk = FMS trk target = bearings[diffs == np.min(diffs)][0] # Override target trk for autopilot self.trk[idx] = target # Compute escape trk to be used in case the aircraft enters the geofence # Escape trk is equal to last target with an offset of 45 degrees towards the geofence edge self.esc_trk[idx] = target + np.sign(degto180(target - traf.trk[idx]))*45
def log(self): dist = self.state[0] t = self.state[1] hdg = self.state[2] hdg_ref = 60. hdg_diff = (degto180(hdg_ref - hdg)) f = open(self.fname, 'a') f.write("{};{};{};{};{};{};{}\n".format(self.ep, self.reward, dist, hdg_diff, t, agent.epsilon, agent.sta)) f.close()
def calcturn(self,tas,bank,wpqdr,next_wpqdr,turnradnm=-999.): # Calculate turn radius using current speed or use specified turnradius turnrad = np.where(turnradnm+0.*tas<0., tas * tas / (np.maximum(0.01, np.tan(bank)) * g0), turnradnm*nm) # turndist is in meters turndist = np.abs(turnrad * np.tan(np.radians(0.5 * np.abs(degto180(wpqdr%360. - next_wpqdr%360.))))) return turndist,turnrad
def Reached(self, qdr, dist, flyby, flyturn, turnradnm, swlastwp): # Calculate distance before waypoint where to start the turn # Note: this is a vectorized function, called with numpy traffic arrays # It returns the indices where the Reached criterion is True # # 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 # First calculate turn distance next_qdr = np.where(self.next_qdr < -900., qdr, self.next_qdr) turntas = np.where(bs.traf.actwp.turnspd < 0.0, bs.traf.tas, bs.traf.actwp.turnspd) flybyturndist, turnrad = self.calcturn(turntas, bs.traf.ap.bankdef, qdr, next_qdr, turnradnm) # Turb dist iz ero for flyover, calculated distance for others self.turndist = np.logical_or(flyby, flyturn) * flybyturndist # Avoid circling by checking for flying away on almost straight legs with small turndist # difference between direction to and track larger than 90 # and close to waypoint based on ground speed, assumption using vicinity criterion: # flying away and within 4 sec distance based on ground speed (4 sec = sensitivity tuning parameter) close2wp = dist / (np.maximum(0.0001, np.abs( bs.traf.gs))) < 4.0 # Waypoint is within 4 seconds flight time # When close to waypoint or passing the last waypoint, switch when flying away from active waypoint away = np.logical_or(close2wp, swlastwp) * ( np.abs(degto180(bs.traf.trk % 360. - qdr % 360.)) > 90. ) # difference large than 90 # Ratio between distance close enough to switch to next wp when flying away # When within pro1 nm and flying away: switch also proxfact = 1.02 # Turnradius scales this contant , factor => [turnrad] incircle = dist < turnrad * proxfact circling = away * incircle # [True/False] passed wp,used for flyover as well # Check whether shift based dist is required, set closer than WP turn distance # Detect indices swreached = np.where(bs.traf.swlnav * np.logical_or( away, np.logical_or(dist < self.turndist, circling)))[0] # Return indices for which condition is True/1.0 for a/c where we have reached waypoint return swreached
def check_reached(self): qdr, dist = qdrdist(traf.lat, traf.lon, traf.actwp.lat, traf.actwp.lon) # [deg][nm]) # check which aircraft have reached their destination by checkwaypoint type = 3 (destination) and the relative # heading to this waypoint is exceeding 150 dest = np.asarray([traf.ap.route[i].wptype[traf.ap.route[i].iactwp] for i in range(traf.ntraf)]) == 3 away = np.abs(degto180(traf.trk % 360. - qdr % 360.)) > 100. d = dist<2 reached = np.where(away * dest * d)[0] # print('track', traf.trk, qdr, dist) # What is the desired behaviour. When an aircraft has reached, it will be flagged as reached. # Therefore the aircraft should get a corresponding reward as well for training purposes in the replay memory # Next the corresponding aircraft should be deleted. # If no more aircraft are present, next episode should start. # print('reached', reached) done = np.zeros((traf.ntraf), dtype=bool) done[reached] = True self.done = done
def gen_reward(self): dist = self.state[0] t = self.state[1] hdg = self.state[2] hdg_ref = 60. a_dist = -0.2 a_t = -0.1 a_hdg = -0.07 dist_rew = 2 + a_dist * dist t_rew = 6 + a_t * abs(t) if self.done: hdg_rew = a_hdg * abs(degto180(hdg_ref - hdg)) else: hdg_rew = 0 self.reward = dist_rew + t_rew + hdg_rew return self.reward
def row_priority(self, ownship, intruder, idx1, idx2): ''' Check if ownship has priority according right-of-way rules - Ownship with intruder on the right has priority - In case of parallel tracks: aircraft with higher GS has priority - Parallel tracks have a difference of less than 1 degree (also head-on) - In case of same GS, no aircraft has priority ''' # Determine heading difference between the aircraft dhdg = degto180(ownship.trk[idx1] - intruder.trk[idx2]) # Parallel tracks (with accuracy of 1 degree) if abs(dhdg) < 1. or abs(dhdg) > 179.: # Aircraft with higher speed has prio prio = True if ownship.gs[idx1] > intruder.gs[idx2] else False # Left-converging elif dhdg > 0: prio = True # Right-converging else: prio = False return prio
def findact(self, i): """ Find best default active waypoint. This function is called during route creation""" #print "findact is called.!" # Check for easy answers first if self.nwp <= 0: return -1 elif self.nwp == 1: return 0 # Find closest wplat = array(self.wplat) wplon = array(self.wplon) dy = (wplat - bs.traf.lat[i]) dx = (wplon - bs.traf.lon[i]) * bs.traf.coslat[i] dist2 = dx * dx + dy * dy # Note: the max() prevents walking back, even in cases when this might be apropriate, # such as when previous waypoints have been deleted iwpnear = max(self.iactwp, argmin(dist2)) #Unless behind us, next waypoint? if iwpnear + 1 < self.nwp: qdr = degrees(arctan2(dx[iwpnear], dy[iwpnear])) delhdg = abs(degto180(bs.traf.trk[i] - qdr)) # we only turn to the first waypoint if we can reach the required # heading before reaching the waypoint time_turn = max(0.01, bs.traf.tas[i]) * radians(delhdg) / ( g0 * tan(bs.traf.bank[i])) time_straight = sqrt(dist2[iwpnear]) * 60. * nm / max( 0.01, bs.traf.tas[i]) if time_turn > time_straight: iwpnear += 1 return iwpnear
def Reached(self, qdr, dist, flyby, flyturn, turnradnm): # Calculate distance before waypoint where to start the turn # Note: this is a vectorized function, called with numpy arrays # It returns the indices where the Reached criterion is True # # 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 # First calculate turn distance next_qdr = np.where(self.next_qdr < -900., qdr, self.next_qdr) flybyturndist, turnrad = self.calcturn(bs.traf.tas, bs.traf.bank, qdr, next_qdr, turnradnm) self.turndist = np.logical_or(flyby, flyturn) * flybyturndist # Avoid circling by checking for flying away on almost straight legs with small turndist # difference between direction to and track larger than 90 # but avoid switching wayspoint when trk undefined due to standing still (groundspeed (<1 m/s) away = (np.abs(bs.traf.gs) > 1) * ( np.abs(degto180(bs.traf.trk % 360. - qdr % 360.)) > 90. ) # difference large than 90 # Ratio between distance close enough to switch to next wp when flying away # When within pro1 nm and flying away: switch also proxfact = 1.02 # Turnradius scales this contant , factor => [turnrad] incircle = dist < turnrad * proxfact circling = away * incircle # [True/False] passed wp,used for flyover as well # Check whether shift based dist is required, set closer than WP turn distance # Detect indices swreached = np.where(bs.traf.swlnav * np.logical_or( away, np.logical_or(dist < self.turndist, circling)))[0] # Return indices for which condition is True/1.0 for a/c where we have reached waypoint return swreached
def direct(self, idx, wpnam): #print("Hello from direct") """Set active point to a waypoint by name""" name = wpnam.upper().strip() if name != "" and self.wpname.count(name) > 0: wpidx = self.wpname.index(name) self.iactwp = wpidx bs.traf.actwp.lat[idx] = self.wplat[wpidx] bs.traf.actwp.lon[idx] = self.wplon[wpidx] bs.traf.actwp.flyby[idx] = self.wpflyby[wpidx] bs.traf.actwp.flyturn[idx] = self.wpflyturn[wpidx] bs.traf.actwp.turnrad[idx] = self.wpturnrad[wpidx] bs.traf.actwp.turnspd[idx] = self.wpturnspd[wpidx] # Do calculation for VNAV self.calcfp() bs.traf.actwp.xtoalt[idx] = self.wpxtoalt[wpidx] bs.traf.actwp.nextaltco[idx] = self.wptoalt[wpidx] bs.traf.actwp.torta[idx] = self.wptorta[ wpidx] # available for active RTA-guidance bs.traf.actwp.xtorta[idx] = self.wpxtorta[ wpidx] # available for active RTA-guidance #VNAV calculations like V/S and speed for RTA bs.traf.ap.ComputeVNAV(idx, self.wptoalt[wpidx], self.wpxtoalt[wpidx],\ self.wptorta[wpidx],self.wpxtorta[wpidx]) # If there is a speed specified, process it if self.wpspd[wpidx] > 0.: # Set target speed for autopilot if self.wpalt[wpidx] < 0.0: alt = bs.traf.alt[idx] else: alt = self.wpalt[wpidx] # Check for valid Mach or CAS if self.wpspd[wpidx] < 2.0: cas = mach2cas(self.wpspd[wpidx], alt) else: cas = self.wpspd[wpidx] # Save it for next leg bs.traf.actwp.nextspd[idx] = cas # No speed specified for next leg else: bs.traf.actwp.nextspd[idx] = -999. qdr, dist = geo.qdrdist(bs.traf.lat[idx], bs.traf.lon[idx], bs.traf.actwp.lat[idx], bs.traf.actwp.lon[idx]) if self.wpflyturn[wpidx] or self.wpturnrad[wpidx] < 0.: turnrad = self.wpturnrad[wpidx] else: turnrad = bs.traf.tas[idx] * bs.traf.tas[idx] / tan( radians(25.)) / g0 / nm # [nm]default bank angle 25 deg bs.traf.actwp.turndist[idx] = (bs.traf.actwp.flyby[idx] > 0.5) * \ turnrad*abs(tan(0.5*radians(max(5., abs(degto180(qdr - self.wpdirfrom[self.iactwp])))))) # [nm] bs.traf.swlnav[idx] = True return True else: return False, "Waypoint " + wpnam + " not found"
def resolve(self, conf, ownship, intruder): ''' Main resolution module ''' # --> Resolution setting options (through METHOD stack command): # 1 - MVP, GEO, vertical = METHOD BOTH 3D # 2 - MVP, GEO, no vertical = METHOD BOTH 2D # 3 - MVP, no GEO, vertical = METHOD MVP 3D # 4 - MVP, no GEO, no vertical = METHOD MVP 2D # 5 - no MVP, GEO, vertical = METHOD GEO 3D # 6 - no MVP, GEO, no vertical = METHOD GEO 2D # ------------------------------------------------------------------ # resolve() process: # 1 -> if mvp is active, compute pairwise summed horizontal MVP solution # (this will give newtrk, newgs, newvs, newalt for all aircraft) # 2 -> check which aircraft will breach given the solution (from 1) # (if mvp is inactive, return True for all aircraft) # 3 -> reset newtrk, newgs, newvs, newalt to current aircraft state for aircraft with breach = True # 4 -> loop through all pairwise conflicts where ownship breach == True (from 2) # 4.a -> if mvp is active, compute dv_mvp for pairwise conflict # - if mvp does not breach, proceed directly to (4.d) # 4.b -> if geo is active, compute dv_geo for pairwise conflict # 4.c -> using a decision criterion, choose mvp or geo solution # (non-existing maneuvers will be automatically dismissed) # 4.d -> if vertical is active, compute dv_ver # - if vertical is not active, proceed directly to (4.f) # 4.e -> using a decision criterion, choose horizontal (from 6) or vertical (from 7) # (not computed maneuvers will be automatically dismissed) # 4.f -> add chosen dv to already existing dv for ownship (pairwise summed) # 5 -> compute average of horizontal maneuver: pairwise-summed average # 6 -> add dv of all aircraft to newtrk, newgs, newvs, newalt from (1) # (dv for breaching aircraft is zero by default) # 7 -> cap velocities and return newtrk, newgs, newvs, newalt # Initialize resomethod log for all conflicts in the current timestep self.resomethod = np.full(len(conf.confpairs), 'MVP') # Reset number of conflicts traf_nconf = np.zeros(traf.ntraf, dtype=int) # Create newvs, newalt newvs = np.zeros(ownship.ntraf) if np.any(self.layers): newalt = np.array(self.layers[self.layidx]) else: newalt = np.array(ownship.alt) # Initialize switch to keep track of vertical maneuvers traf_swalt = np.zeros(ownship.ntraf, dtype=np.bool) # Get array containing active limits for each aircraft gsmin, gsmax, trkmin, trkmax, vsmin, vsmax = ap_geo.get_limits() # Create empty dv array for all aircraft dv_traf = np.zeros((ownship.ntraf, 3)) # STEP 1: If MVP is active, determine pairwise summed horizontal solution and check for breaches ===== if self.mvp_active: newtrk, newgs, _, _ = super().resolve(conf, ownship, intruder) # STEP 2: Check for breaches ========================================================================= eps = 1e-10 # small increment for limit to prevent floating point errors leading to wrong result breachgs = np.logical_or(newgs < (gsmin - eps), newgs > (gsmax + eps)) breachtrk = np.logical_or( degto180(newtrk - trkmin) < 0, degto180(newtrk - trkmax) > 0) # Bool array indicating which aircraft will breach a geovector limit through a CR maneuver swbreach = (breachgs | breachtrk) & conf.inconf # STEP 3: Reset newtrk, newgs, newvs, newalt for the breaching aircraft ============================== newtrk[swbreach] = np.array(ownship.trk[swbreach]) newgs[swbreach] = np.array(ownship.gs[swbreach]) # If MVP is not active, set each aircraft to be breaching # This ensures that all conflicts get a solution from the GEO module # Also create newtrk, newgs else: newtrk = np.array(ownship.trk) newgs = np.array(ownship.gs) swbreach = np.ones(ownship.ntraf, dtype=np.bool) # STEP 4: Loop through all conflicts in the current timestep ========================================= nconf = 0 # keeping track of conflict number to store resolution method for ((ac1, ac2), qdr, dist, tcpa, tLOS) in zip(conf.confpairs, conf.qdr, \ conf.dist, conf.tcpa, conf.tLOS): # Retrieve ownship and intruder index idx1 = ownship.id.index(ac1) idx2 = intruder.id.index(ac2) # CR is switched off for aircraft in a geofence conflict: do not resolve try: if ap_geo.APgeo().geoconf[idx1]: self.resomethod[nconf] = 'FCE' nconf += 1 continue except AttributeError: # if geofence autopilot is not implemented pass # Check if horizontal pairwise summed MVP solution will breach at all if not swbreach[idx1]: nconf += 1 # update conflict counter continue # go to next iteration in for loop # Check if the aircraft is already performing a vertical maneuver if traf_swalt[idx1]: self.resomethod[nconf] = 'IND' nconf += 1 continue # Retrieve horizontal geovector limits for the ownship gsl = gsmin[idx1] gsu = gsmax[idx1] trkl = trkmin[idx1] trku = trkmax[idx1] # STEP 4a: MVP ====================================================================================== if self.mvp_active: dv_mvp, _ = super().MVP(ownship, intruder, conf, qdr, dist, tcpa, tLOS, idx1, idx2) # Set mvp vs to zero dv_mvp[2] = 0. #Check if MVP solution leads to a geovector breach gs_mvp = ownship.gs[idx1] + np.sqrt(dv_mvp[0]**2 + dv_mvp[1]**2) trk_mvp = ownship.trk[idx1] + ( np.rad2deg(np.arctan2(dv_mvp[0], dv_mvp[1])) % 360) mvp_breach = True if gs_mvp < gsl or gs_mvp > gsu or \ degto180(trk_mvp - trkl) < 0 or degto180(trk_mvp - trku) > 0 else False else: #if MVP not active dv_mvp, mvp_breach = np.full(3, np.nan), True # STEP 4b: GEO ====================================================================================== # Only compute the alternative maneuver if the MVP solution will breach if mvp_breach: if self.geo_active: # Retrieve intruder rpz and horizontal ownship geovector limits rpz = conf.rpz[idx2] * self.resofach # GEO maneuver only exists if the pair is not in LoS if dist > rpz: dv_geo, type_geo = self.GEO(ownship, intruder, gsl, gsu, trkl, trku, qdr, \ dist, rpz, idx1, idx2) # Check if GEO solution leads to a geovector breach geo_breach = True if type_geo == 'INS' else False elif not self.mvp_active: # if already in LoS and MVP not active, do nothing dv_geo, geo_breach, type_geo = np.zeros( 3), False, 'LOS' else: # if already in LoS but MVP maneuver can be used to escape dv_geo, geo_breach, type_geo = np.full( 3, np.nan), False, 'LOS' else: #if GEO not active dv_geo, geo_breach, type_geo = np.full( 3, np.nan), True, 'undefined' # STEP 4c: Choose horizontal maneuver ================================================================ p_mvp = p_horz if mvp_breach else 1 #penalty factor for mvp maneuver p_geo = p_horz if geo_breach else 1 #penalty factor for geo maneuver criterion_mvp = p_mvp * np.sqrt(dv_mvp[0]**2 + dv_mvp[1]**2) criterion_geo = p_geo * np.sqrt(dv_geo[0]**2 + dv_geo[1]**2) criteria = np.array([criterion_mvp, criterion_geo]) # The maneuver with the smallest existing horizontal state change magnitude will be chosen do_geo = True if np.nanargmin(criteria) == 1 else False if do_geo: dv, self.resomethod[nconf] = dv_geo, type_geo p_hor = p_vert if geo_breach else 1 else: dv, self.resomethod[nconf], p_hor = dv_mvp, 'MVP', p_vert # If the MVP solution for this pairwise conflict will not breach, choose MVP else: dv, self.resomethod[nconf], p_hor = dv_mvp, 'MVP', 1 # STEP 4d: vertical ================================================================================= # Compute vertical maneuver if active and choose between horizontal and vertical alt_cur = newalt[idx1] if self.ver_active: # Vertical maneuver is only allowed if the aircraft has priority if self.row_priority(ownship, intruder, idx1, idx2): dv_ver, alt_res, layidx = self.vertical( ownship, intruder, idx1, idx2, conf.hpz[idx2], tLOS) # Retrieve vertical geovector limits for the ownship vsl, vsu = vsmin[idx1], vsmax[idx1] # Check if vertical solution leads to a geovector breach and set penalty ver_breach = True if dv_ver[2] < vsl or dv_ver[ 2] > vsu else False p_ver = p_vert if ver_breach else 1 # Determine if traffic is in lookahead disc on current (_cur) and resolution (_res) alt # Lookahead disc equals ownship ground speed * lookahead time with height = hpz # Furthermore, number of instant LoS will be determined for both altitudes traf_qdr, traf_dist = qdrdist(ownship.lat[idx1], ownship.lon[idx1], intruder.lat, intruder.lon) traf_qdr, traf_dist = np.radians(traf_qdr), traf_dist * nm traf_dh_cur = alt_cur - intruder.alt traf_dh_res = alt_res - intruder.alt # inside horizontally for both the lookahead disc and the PZ ins_hor = traf_dist < (ownship.gs[idx1] * conf.dtlookahead[idx1]) los_hor = traf_dist < conf.rpz[ idx1] # inside ownship PZ = LoS los_hor[ idx1] = False # for LoS count -> exclude the ownship # inside vertically on current and resolution altitude ins_ver_cur = np.abs(traf_dh_cur) < conf.hpz[idx1] ins_ver_res = np.abs(traf_dh_res) < conf.hpz[idx1] # if both are True: traffic is inside ins_cur = np.logical_and(ins_hor, ins_ver_cur) ins_res = np.logical_and(ins_hor, ins_ver_res) los_cur = np.logical_and(los_hor, ins_ver_cur) los_res = np.logical_and(los_hor, ins_ver_res) # number of instant losses of separation nlos_cur = los_cur.sum() nlos_res = los_res.sum() # Determine if traffic is converging with ownship # Aircraft are converging if distance between them will get smaller in the future drel = np.array([ np.sin(traf_qdr) * traf_dist, np.cos(traf_qdr) * traf_dist, traf_dh_cur ]).T vall = np.array( [intruder.gseast, intruder.gsnorth, intruder.vs]).T vown = np.array([ ownship.gseast[idx1], ownship.gsnorth[idx1], ownship.vs[idx1] ]).T vrel = vall - vown dnew = drel + vrel # look one second ahead traf_newdist = np.sqrt(dnew[:, 0]**2 + dnew[:, 1]**2 + dnew[:, 2]**2) conv_cur = traf_newdist[ins_cur] < traf_dist[ins_cur] conv_res = traf_newdist[ins_res] < traf_dist[ins_res] # Count number of converging traffic # (works since ownship is not converging with itself) nconv_cur = conv_cur.sum() nconv_res = conv_res.sum() # Compute decision criterion for both current and resolution altitude criterion_cur = p_hor * (nconv_cur + 100 * nlos_cur ) # x100 penalty for instant LoS criterion_res = p_ver * (nconv_res + 100 * nlos_res ) # " " # STEP 4e: Choose between horizontal and vertical maneuver and set dv accordingly ==================== do_ver = True if criterion_res < criterion_cur else False if do_ver: dv, self.resomethod[nconf] = dv_ver, 'VER' newalt[idx1] = alt_res self.layidx[idx1] = layidx traf_swalt[idx1] = True # Reset previously computed horizontal maneuvers # These are not necessary any more dv_traf[idx1, 0:2] = 0. for n in range( nconf ): # loop through previously solved pairwise conflicts if ownship.id.index( conf.confpairs[n] [0]) == idx1: # check if ownship = idx1 self.resomethod[n] = 'IND' # STEP 4f: Finally append chosen dv to complete dv array ============================================= dv_traf[idx1] += dv traf_nconf[idx1] += 1 # Update conflict counter nconf += 1 # STEP 5: Compute average of horizontal pairwise-summed state changes ================================= dv_traf[:, 0] = np.where(dv_traf[:, 0] == 0., 0., dv_traf[:, 0] / traf_nconf) # average eartherly gs state-change dv_traf[:, 1] = np.where(dv_traf[:, 1] == 0., 0., dv_traf[:, 1] / traf_nconf) # average northerly gs state-change # STEP 6: Update velocity vector of all aircraft ===================================================== newtrk = np.radians(newtrk) newgseast = newgs * np.sin(newtrk) newgsnorth = newgs * np.cos(newtrk) v_traf = np.array([newgseast, newgsnorth, newvs]) v_traf += dv_traf.T # Convert velocity vector to cyclindrical coordinate system newtrk = (np.arctan2(v_traf[0, :], v_traf[1, :]) * 180 / np.pi) % 360 newgs = (np.sqrt(v_traf[0, :]**2 + v_traf[1, :]**2)) newvs = v_traf[2, :] # STEP 7: Cap the velocity and vertical speed and return ============================================= newgscapped = np.maximum(ownship.perf.vmin, np.minimum(ownship.perf.vmax, newgs)) newvscapped = np.maximum(ownship.perf.vsmin, np.minimum(ownship.perf.vsmax, newvs)) return newtrk, newgscapped, newvscapped, newalt
def ETA(acidx): # Assuming no wind. # Assume complete route is available including orig --> destination # print(np.where(traf.ap.route[acidx].wptype)) # Acceleration value depends on flight phase and aircraft type in BADA # The longitudinal acceleration is constant for all airborne flight # phases in the bada model. ax = traf.perf.acceleration() iactwp = traf.ap.route[acidx].iactwp nwp = len(traf.ap.route[acidx].wpname) # Construct a tables with relevant route parameters before the 4D prediction dist2vs = [0] * nwp Vtas = [0] * nwp Vcas = traf.ap.route[acidx].wpspd.copy() dsturn = [0] * nwp qdr = [0] * nwp s = [0] * nwp turndist = [0] * nwp lat = traf.ap.route[acidx].wplat.copy() lon = traf.ap.route[acidx].wplon.copy() altatwp = traf.ap.route[acidx].wpalt.copy() wpialt = traf.ap.route[acidx].wpialt.copy() for wpidx in range(iactwp - 1, nwp): # If the next waypoint is the active waypoint, use aircraft data # Else use previous waypoint data if wpidx == iactwp - 1: Vtas[wpidx] = traf.gs[acidx] # [m/s] Vcas[wpidx] = traf.cas[acidx] lat[wpidx] = traf.lat[acidx] # [deg] lon[wpidx] = traf.lon[acidx] # [deg] altatwp[wpidx] = traf.alt[acidx] # [m] else: lat[wpidx] = traf.ap.route[acidx].wplat[wpidx] # [deg] lon[wpidx] = traf.ap.route[acidx].wplon[wpidx] #[deg] qdr, s[wpidx] = qdrdist(lat[wpidx - 1], lon[wpidx - 1], lat[wpidx], lon[wpidx]) # [nm] s[wpidx] *= nm # [m] # check for valid speed, if no speed is given take the speed # already in memory from previous waypoint. if traf.ap.route[acidx].wpspd[wpidx] < 0: Vtas[wpidx] = Vtas[wpidx - 1] Vcas[wpidx] = Vcas[wpidx - 1] else: Vtas[wpidx] = vcas2tas( traf.ap.route[acidx].wpspd[wpidx], traf.ap.route[acidx].wpalt[wpidx]).item() # [m/s] # Compute distance correction for flyby turns. if wpidx < nwp - 2: #bearing of leg --> Second leg. nextqdr, nexts = qdrdist(lat[wpidx], lon[wpidx], lat[wpidx + 1], lon[wpidx + 1]) #[deg, nm] nexts *= nm # [m] dist2turn, turnrad = traf.actwp.calcturn( Vtas[wpidx], traf.bank[acidx], qdr, nextqdr) # [m], [m] delhdg = np.abs( degto180(qdr % 360 - nextqdr % 360)) # [deg] Heading change distofturn = np.pi * turnrad * delhdg / 360 # [m] half of distance on turn radius dsturn[wpidx] = dist2turn - distofturn turndist[wpidx] = dist2turn # Now loop to fill the VNAV constraints. A second loop is necessary # Because the complete speed profile and turn profile is required. curalt = traf.alt[acidx] for wpidx in range(iactwp, nwp): # If Next altitude is not the altitude constraint, check if the # aircraft should already decent. Otherwise aircraft stays at # current altitude. Otherwise expected altitude is filled in. if wpidx < wpialt[wpidx]: # Compute whether decent should start or speed is still same dist2vs[wpidx] = turndist[wpidx] + \ np.abs(curalt - traf.ap.route[acidx].wptoalt[wpidx]) / traf.ap.steepness # If the dist2vs is smaller everything should be fine if traf.ap.route[acidx].wpxtoalt[wpidx] > dist2vs[wpidx]: dist2vs[wpidx] = 0 altatwp[wpidx] = curalt else: dist2vs[wpidx] = dist2vs[wpidx] - traf.ap.route[ acidx].wpxtoalt[wpidx] if curalt > traf.ap.route[acidx].wptoalt[wpidx]: sign = -1 else: sign = 1 curalt = curalt + dist2vs[wpidx] * traf.ap.steepness * sign altatwp[wpidx] = curalt # If there is an altitude constraint on the current waypoint compute # dist2vs for that waypoint. else: dist2vs[wpidx] = np.abs( curalt - traf.ap.route[acidx].wptoalt[wpidx]) / traf.ap.steepness if dist2vs[wpidx] > traf.ap.route[acidx].wpxtoalt[wpidx]: dist2vs[wpidx] = traf.ap.route[acidx].wpxtoalt[wpidx] # Start computing the actual 4D trajectory t_total = 0 for wpidx in range(iactwp, nwp): # If V1 != V2 the aircraft will first immediately accelerate # according to the performance model. To compute the time for the # leg, constant acceleration is assumed Vcas1 = Vcas[wpidx - 1] Vtas1 = Vtas[wpidx - 1] Vcas2 = Vcas[wpidx] Vtas2 = Vtas[wpidx] s_leg = s[wpidx] - dsturn[wpidx - 1] - dsturn[wpidx] if Vcas1 != Vcas2: axsign = 1 + -2 * (Vcas1 > Vcas2) # [-] t_acc = (Vcas2 - Vcas1) / ax * axsign # [s] s_ax = t_acc * (Vtas2 + Vtas1) / 2 # [m] Constant acceleration else: s_ax = 0 # [m] t_acc = 0 s_nom = s_leg - s_ax - dist2vs[wpidx] # [m] Distance of normal flight # Check if acceleration distance and start of descent overlap if s_nom < 0: t_vs = 0 t_nom = 0 if dist2vs[wpidx] <= s_leg: s1 = 0 s2 = s_ax s3 = s_leg - s_ax else: s1 = s_leg - dist2vs[wpidx] s2 = dist2vs[wpidx] + s_ax - s_leg s3 = s_leg - s_ax t1 = abc(0.5 * vcas2tas(ax, altatwp[wpidx - 1]), Vtas1, s1) Vcas_vs = Vcas1 - t1 * ax t2 = (Vcas2 - Vcas_vs) / ax alt2 = altatwp[wpidx - 1] - s2 * traf.ap.steepness Vtas_vs = vcas2tas(Vcas_vs, alt2) t3 = s3 / ((Vtas_vs + Vtas2) / 2) t_leg = t1 + t2 + t3 else: Vtas_nom = vcas2tas(Vcas2, altatwp[wpidx - 1]) t_nom = s_nom / Vtas_nom #Assume same speed for now. t_vs = dist2vs[wpidx] / (Vtas_nom + Vtas2) / 2 if dist2vs[wpidx] < 1: t_vs = 0 t_leg = t_nom + t_acc + t_vs print(t_leg) t_total += t_leg # Debug print statements # print('wptype ', traf.ap.route[acidx].wptype[traf.ap.route[acidx].iactwp]) # print('DEBUG') # print('Vtas ', Vtas) # print('Vcas ', Vcas) # print('dsturn ', dsturn) # print('s ', s) # print('turndist ', turndist) # print('lat ', lat) # print('lon ', lon) # print('altatwp ', altatwp) # print('wpialt ', traf.ap.route[acidx].wpialt) # print('wptoalt ', traf.ap.route[acidx].wptoalt) # print('wpxtoalt ', traf.ap.route[acidx].wpxtoalt) # print('altatwp ', altatwp) # print('dist2vs ', dist2vs) # print('eta', sim.simt + t_total) # print(' ') return t_total
def applygeovec(): # Apply each geovector for areaname, vec in geovecs.items(): if areafilter.hasArea(areaname): swinside = areafilter.checkInside(areaname, traf.lat, traf.lon, traf.alt) insids = set(np.array(traf.id)[swinside]) newids = insids - vec.previnside delids = vec.previnside - insids # Store LNAV/VNAV status of new aircraft for acid in newids: idx = traf.id2idx(acid) vec.prevstatus[acid] = [traf.swlnav[idx], traf.swvnav[idx]] # Revert aircraft who have exited the geovectored area to their original status for acid in delids: idx = traf.id2idx(acid) if idx >= 0: traf.swlnav[idx], traf.swvnav[idx] = vec.prevstatus.pop( acid) vec.previnside = insids # -----Ground speed limiting # For now assume no wind: so use tas as gs if vec.gsmin is not None: casmin = vtas2cas(np.ones(traf.ntraf) * vec.gsmin, traf.alt) usemin = traf.selspd < casmin traf.selspd[swinside & usemin] = casmin[swinside & usemin] traf.swvnav[swinside & usemin] = False if vec.gsmax is not None: casmax = vtas2cas(np.ones(traf.ntraf) * vec.gsmax, traf.alt) usemax = traf.selspd > casmax traf.selspd[swinside & usemax] = casmax[swinside & usemax] traf.swvnav[swinside & usemax] = False #------ Limit Track(so hdg) # Max track interval is 180 degrees to avoid ambiguity of what is inside the interval if None not in [vec.trkmin, vec.trkmax]: # Use degto180 to avodi problems for e.g interval [350,30] usemin = swinside & (degto180(traf.trk - vec.trkmin) < 0 ) # Left of minimum usemax = swinside & (degto180(traf.trk - vec.trkmax) > 0 ) # Right of maximum traf.swlnav[swinside & (usemin | usemax)] = False traf.ap.trk[swinside & usemin] = vec.trkmin traf.ap.trk[swinside & usemax] = vec.trkmax # -----Ground speed limiting # For now assume no wind: so use tas as gs if vec.vsmin is not None: traf.selvs[swinside & (traf.vs < vec.vsmin)] = vec.vsmin traf.swvnav[swinside & (traf.vs < vec.vsmin)] = False # Activate V/S mode by using a slightly higher altitude than current values traf.selalt[swinside & (traf.vs < vec.vsmin)] = traf.alt[swinside & (traf.vs < vec.vsmin)] + \ np.sign(vec.vsmin)*200.*ft if vec.vsmax is not None: traf.selvs[swinside & (traf.vs > vec.vsmax)] = vec.vsmax traf.swvnav[swinside & (traf.vs < vec.vsmax)] = False # Activate V/S mode by using a slightly higher altitude than current values traf.selalt[swinside & (traf.vs > vec.vsmax)] = traf.alt[swinside & (traf.vs > vec.vsmax)] + \ np.sign(vec.vsmax)*200.*ft return
def update_geolog(self): ''' Write to geovector log ''' # Loop over all geovectors for geoname, geovec in geovector.geovecs.items(): try: self.previds[geoname + lims[0]] # If the geovector is newly created: except KeyError: # Create new previous ids item in dict for limname in lims: key = geoname + limname self.previds[key] = set() # Create new geovector item in dict self.geovecs[geoname] = np.array([geovec.gsmin, \ geovec.gsmax, geovec.trkmin, geovec.trkmax, geovec.vsmin, geovec.vsmax]) # Check aircraft inside geovector area geoinside = areafilter.checkInside(geoname, traf.lat, traf.lon, traf.alt) # Get gs breaches if geovec.gsmin is not None: gsl = geovec.gsmin - 1e-10 #prevent floating point errors swgsmin = traf.gs < gsl else: swgsmin = np.zeros(traf.ntraf, dtype=bool) if geovec.gsmax is not None: gsr = geovec.gsmax + 1e-10 #prevent floating point errors swgsmax = traf.gs > gsr else: swgsmax = np.zeros(traf.ntraf, dtype=bool) #Compute course breach values if None not in (geovec.trkmin, geovec.trkmax): swtrkmin = degto180(traf.trk - geovec.trkmin) < 0. swtrkmax = degto180(traf.trk - geovec.trkmax) > 0. else: swtrkmin = np.zeros(traf.ntraf, dtype=bool) swtrkmax = np.zeros(traf.ntraf, dtype=bool) # Get vs breaches if geovec.vsmin is not None: vsl = geovec.vsmin - 1e-10 #prevent floating point errors swvsmin = traf.vs < vsl else: swvsmin = np.zeros(traf.ntraf, dtype=bool) if geovec.vsmax is not None: vsr = geovec.vsmax + 1e-10 #prevent floating point errors swvsmax = traf.vs > vsr else: swvsmax = np.zeros(traf.ntraf, dtype=bool) # Update self.tcrb here to avoid having to check geovector breaches twice swbreaches = (swgsmin | swgsmax | swtrkmin | swtrkmax | swvsmin | swvsmax) & geoinside self.tcrb[self.currinside & traf.cr.active & swbreaches] += settings.simdt # Get id of all aircraft currently breaching the geovector inside the log area for lim in lims: if lim == "gsmin": curids = set( np.array(traf.id)[swgsmin & geoinside & self.currinside]) elif lim == "gsmax": curids = set( np.array(traf.id)[swgsmax & geoinside & self.currinside]) elif lim == "trkmin": curids = set( np.array(traf.id)[swtrkmin & geoinside & self.currinside]) elif lim == "trkmax": curids = set( np.array(traf.id)[swtrkmax & geoinside & self.currinside]) elif lim == "vsmin": curids = set( np.array(traf.id)[swvsmin & geoinside & self.currinside]) elif lim == "vsmax": curids = set( np.array(traf.id)[swvsmax & geoinside & self.currinside]) # Determine new and del ids newids = curids - self.previds[geoname + lim] delids = self.previds[geoname + lim] - curids # For new ids: create new breach object for id in newids: name = geoname + lim + id self.breaches[name] = self.Breach(id, geoname, lim, \ self.geovecs[geoname][lims.index(lim)]) # For del ids: send data and pop item for id in delids: name = geoname + lim + id val, avg, dur = self.breaches[name].send_data() self.geolog.log(id, geoname, lim, val, avg, dur) self.breaches.pop(name) # For curids: update value for id in curids: name = geoname + lim + id self.breaches[name].update_data() # Update previds self.previds[geoname + lim] = curids
def GEO(self, ownship, intruder, gsmin, gsmax, trkmin, trkmax, qdr, dist, s_h, idx1, idx2): ''' Compute alternative horizontal maneuver ('GEO') for pairwise conflict in geovector area ''' # String indicating the maneuver that is performed manv = 'OPT' #Create velocity vectors and compute relative velocity vown = np.array([ownship.gseast[idx1], ownship.gsnorth[idx1]]) vint = np.array([intruder.gseast[idx2], intruder.gsnorth[idx2]]) vrel = vown - vint # Compute implicitly coordinated velocity obstacle unit vector nd = unitvector(qdr) #unit vector pointing to intruder r1 = s_h / dist r2 = np.sqrt(1 - r1**2) r = np.array([[r2, r1], [-r1, r2]]) #rotation matrix to get VO legs nt1 = np.dot(r, nd) #unit vector describing VO leg 1 nt2 = np.dot(r.T, nd) #unit vector describing VO leg 2 dots = np.array([np.dot(vrel, nt1), np.dot(vrel, nt2)]) nt = np.array([nt1, nt2])[np.argmax(dots)] #ensure implicit coordination # Compute optimal implicitly coordinated solution in Cartesian coordinates vsol = vint + (max(dots)) * nt # Convert optimal solution to polar coordinates solgs = np.sqrt(vsol[0]**2 + vsol[1]**2) soltrk = np.rad2deg(np.arctan2(vsol[0], vsol[1])) % 360 # Check if the optimal solution will breach the geovector limit(s) if gsmin is not None and solgs < gsmin: breach = True elif gsmax is not None and solgs > gsmax: breach = True elif None not in [trkmin, trkmax]: breach = (degto180(soltrk - trkmin) < 0) | (degto180(soltrk - trkmax) > 0) else: breach = False # If the optimal solution is not feasible, compute intersections with geovector if breach: # Insufficient OPT if GEO is not feasible but OPT leads to breach: manv = 'INS' # V_intersection = V_intruder + c * n_t # Array to store all scaling values 'c' corresponding to intersections cvals = np.array([]) # Compute c values for intersections with gs and trk limits for gslim in (gsmin, gsmax): if gslim is not None: disc = np.dot(np.dot(vint, nt), np.dot(vint, nt)) \ - (np.dot(vint, vint) - (gslim)**2) if disc > 0: #disc = discriminant of quadratic function c1 = -(np.dot(vint, nt) + np.sqrt(disc)) c2 = -(np.dot(vint, nt) - np.sqrt(disc)) cvals = np.append(cvals, (c1, c2)) elif disc == 0: c1 = -np.dot(vint, nt) cvals = np.append(cvals, c1) if None not in (trkmin, trkmax): for trklim in (trkmin, trkmax): ng = unitvector(trklim) c1 = (np.cross(-vint, ng)) / (np.cross(nt, ng)) cvals = np.append(cvals, c1) # Remove trivial solutions from cvals (negative values) cvals = cvals[cvals >= 0.] # Compare c values to the one for optimal resolution (copt) # The closest value to copt represents the smallest required change in velocity if not cvals.size == 0: #check if 'GEO' maneuver exists copt = np.dot(vrel, nt) c = cvals[np.argmin(np.abs(cvals - copt))] vsol = vint + c * nt # If GEO maneuver exists, this maneuver type is returned manv = 'GEO' # Convert final solution to velocity change dv = np.append(vsol - vown, 0.) #append to account for vs component return dv, manv
def update(self): # FMS LNAV mode: # qdr[deg],distinnm[nm] qdr, distinnm = geo.qdrdist(bs.traf.lat, bs.traf.lon, bs.traf.actwp.lat, bs.traf.actwp.lon) # [deg][nm]) self.qdr2wp = qdr dist2wp = distinnm*nm # Conversion to meters # FMS route update and possibly waypoint shift. Note: qdr, dist2wp will be updated accordingly in case of wp switch self.update_fms(qdr, dist2wp) # Updates self.qdr2wp when necessary #================= Continuous FMS guidance ======================== # Waypoint switching in the loop above was scalar (per a/c with index i) # Code below is vectorized, with arrays for all aircraft # Do VNAV start of descent check #dy = (bs.traf.actwp.lat - bs.traf.lat) #[deg lat = 60 nm] #dx = (bs.traf.actwp.lon - bs.traf.lon) * bs.traf.coslat #[corrected deg lon = 60 nm] #dist2wp = 60. * nm * np.sqrt(dx * dx + dy * dy) # [m] # VNAV logic: descend as late as possible, climb as soon as possible startdescent = (dist2wp < self.dist2vs) + (bs.traf.actwp.nextaltco > bs.traf.alt) # If not lnav:Climb/descend if doing so before lnav/vnav was switched off # (because there are no more waypoints). This is needed # to continue descending when you get into a conflict # while descending to the destination (the last waypoint) # Use 0.1 nm (185.2 m) circle in case turndist might be zero self.swvnavvs = bs.traf.swvnav * np.where(bs.traf.swlnav, startdescent, dist2wp <= np.maximum(185.2,bs.traf.actwp.turndist)) #Recalculate V/S based on current altitude and distance to next alt constraint # How much time do we have before we need to descend? t2go2alt = np.maximum(0.,(dist2wp + bs.traf.actwp.xtoalt - bs.traf.actwp.turndist)) \ / np.maximum(0.5,bs.traf.gs) # use steepness to calculate V/S unless we need to descend faster bs.traf.actwp.vs = np.maximum(self.steepness*bs.traf.gs, \ np.abs((bs.traf.actwp.nextaltco-bs.traf.alt)) \ /np.maximum(1.0,t2go2alt)) self.vnavvs = np.where(self.swvnavvs, bs.traf.actwp.vs, self.vnavvs) #was: self.vnavvs = np.where(self.swvnavvs, self.steepness * bs.traf.gs, self.vnavvs) # self.vs = np.where(self.swvnavvs, self.vnavvs, bs.traf.apvsdef * bs.traf.limvs_flag) selvs = np.where(abs(bs.traf.selvs) > 0.1, bs.traf.selvs, bs.traf.apvsdef) # m/s self.vs = np.where(self.swvnavvs, self.vnavvs, selvs) self.alt = np.where(self.swvnavvs, bs.traf.actwp.nextaltco, bs.traf.selalt) # When descending or climbing in VNAV also update altitude command of select/hold mode bs.traf.selalt = np.where(self.swvnavvs,bs.traf.actwp.nextaltco,bs.traf.selalt) # LNAV commanded track angle self.trk = np.where(bs.traf.swlnav, self.qdr2wp, self.trk) # FMS speed guidance: anticipate accel/decel distance for next leg or turn # Calculate actual distance it takes to decelerate/accelerate based on two cases: turning speed (decel) # Normally next leg speed (actwp.spd) but in case we fly turns with a specified turn speed # use the turn speed # Is turn speed specified and are we not already slow enough? We only decelerate for turns, not accel. turntas = np.where(bs.traf.actwp.turnspd>0.0, vcas2tas(bs.traf.actwp.turnspd, bs.traf.alt), -1.0+0.*bs.traf.tas) swturnspd = bs.traf.actwp.flyturn*(turntas>0.0)*(bs.traf.actwp.turnspd>0.0) turntasdiff = np.maximum(0.,(bs.traf.tas - turntas)*(turntas>0.0)) # dt = dv/a ; dx = v*dt + 0.5*a*dt2 => dx = dv/a * (v + 0.5*dv) ax = bs.traf.perf.acceleration() dxturnspdchg = np.where(swturnspd, np.abs(turntasdiff)/np.maximum(0.01,ax)*(bs.traf.tas+0.5*np.abs(turntasdiff)), 0.0*bs.traf.tas) # Decelerate or accelerate for next required speed because of speed constraint or RTA speed nexttas = vcasormach2tas(bs.traf.actwp.nextspd,bs.traf.alt) tasdiff = (nexttas - bs.traf.tas)*(bs.traf.actwp.spd>=0.) # [m/s] # dt = dv/a dx = v*dt + 0.5*a*dt2 => dx = dv/a * (v + 0.5*dv) dxspdconchg = np.abs(tasdiff)/np.maximum(0.01, np.abs(ax)) * (bs.traf.tas+0.5*np.abs(tasdiff)) # Check also whether VNAVSPD is on, if not, SPD SEL has override for next leg # and same for turn logic usenextspdcon = (dist2wp < dxspdconchg)*(bs.traf.actwp.nextspd> -990.) * \ bs.traf.swvnavspd*bs.traf.swvnav*bs.traf.swlnav useturnspd = np.logical_or(bs.traf.actwp.turntonextwp,(dist2wp < dxturnspdchg+bs.traf.actwp.turndist) *\ swturnspd*bs.traf.swvnavspd*bs.traf.swvnav*bs.traf.swlnav) # Hold turn mode can only be switched on here, cannot be switched off here (happeps upon passing wp) bs.traf.actwp.turntonextwp = np.logical_or(bs.traf.actwp.turntonextwp,useturnspd) # Which CAS/Mach do we have to keep? VNAV, last turn or next turn? oncurrentleg = (abs(degto180(bs.traf.trk - qdr)) < 2.0) # [deg] inoldturn = (bs.traf.actwp.oldturnspd > 0.) * np.logical_not(oncurrentleg) # Avoid using old turning speeds when turning of this leg to the next leg # by disabling (old) turningspd when on leg bs.traf.actwp.oldturnspd = np.where(oncurrentleg*(bs.traf.actwp.oldturnspd>0.), -999., bs.traf.actwp.oldturnspd) # turnfromlastwp can only be switched off here, not on (latter happens upon passing wp) bs.traf.actwp.turnfromlastwp = np.logical_and(bs.traf.actwp.turnfromlastwp,inoldturn) # Select speed: turn sped, next speed constraint, or current speed constraint bs.traf.selspd = np.where(useturnspd,bs.traf.actwp.turnspd, np.where(usenextspdcon, bs.traf.actwp.nextspd, np.where((bs.traf.actwp.spdcon>0)*bs.traf.swvnavspd,bs.traf.actwp.spd, bs.traf.selspd))) # Temporary override when still in old turn bs.traf.selspd = np.where(inoldturn*(bs.traf.actwp.oldturnspd>0.)*bs.traf.swvnavspd*bs.traf.swvnav*bs.traf.swlnav, bs.traf.actwp.oldturnspd,bs.traf.selspd) #debug if inoldturn[0]: #debug print("inoldturn bs.traf.trk =",bs.traf.trk[0],"qdr =",qdr) #debug elif usenextspdcon[0]: #debug print("usenextspdcon") #debug elif useturnspd[0]: #debug print("useturnspd") #debug elif bs.traf.actwp.spdcon>0: #debug print("using current speed constraint") #debug else: #debug print("no speed given") # Below crossover altitude: CAS=const, above crossover altitude: Mach = const self.tas = vcasormach2tas(bs.traf.selspd, bs.traf.alt) return
def act_continuous(self, state): state_copy = state.copy() # No action should be taken if there are no aircraft, otherwise the program crashes. # n_aircraft = int(np.prod(obs.shape) / self.state_size) if type(state)==list: n_aircraft = int(np.prod(state[0].shape) / self.state_size) if len(state_copy[0].shape)==2: state_copy[0] = np.expand_dims(state_copy[0], axis=0) if len(state_copy[1].shape)==2: state_copy[1] = np.expand_dims(state_copy[1], axis=0) obs = state_copy[0] state_copy[1] = state[1].reshape(1, n_aircraft * n_aircraft, state[1].shape[-1]) else: n_aircraft = int(np.prod(state.shape)/self.state_size) if len(state_copy.shape)==2: state_copy = np.expand_dims(state_copy, axis=0) obs = state_copy if n_aircraft==0: return # state[0] = state[0].reshape(1, state[0].shape[0], state[0].shape[1]) # state[0] = self.pad_zeros(state[0], self.max_agents).reshape((1, self.max_agents, self.state_size)) # state[1] = self.pad_nines(state[1], self.max_agents).reshape((1, self.max_agents, self.max_agents-1, self.shared_obs_size)) # state_copy = state.copy() # state_copy[0] = state[0].reshape(1, state[0].shape[0], state[0].shape[1]) # if n_aircraft==1: # state_copy[1] = state[1].reshape(1, n_aircraft, state[1].shape[-1]) # else: # state_copy[1] = state[1].reshape(1, n_aircraft*(n_aircraft-1), state[1].shape[-1]) # state_copy[1] = state[1].reshape(1, n_aircraft * (n_aircraft - 1), state[1].shape[-1]) self.action = self.actor.predict(state_copy) # state[0] = state[0].reshape(1, state[0].shape[0], state[0].shape[1]) # data = state # model = self.target.model # print_intermediate_layer_output(model, data, 'merged_mask') # print_intermediate_layer_output(model, data, 'pre_brnn') # print_intermediate_layer_output(model, data, 'brnn') # print_intermediate_layer_output(model, data, 'post_brnn') qdr, dist = qdrdist(traf.lat, traf.lon, traf.actwp.lat, traf.actwp.lon) # [deg][nm]) # check which aircraft have reached their destination by checkwaypoint type = 3 (destination) and the relative # heading to this waypoint is exceeding 150 dest = np.asarray([traf.ap.route[i].wptype[traf.ap.route[i].iactwp] for i in range(traf.ntraf)]) == 3 away = np.abs(degto180(traf.trk % 360. - qdr % 360.)) > 100. d = dist < 2 done_idx = np.where(away * dest * d)[0] tas = np.delete(traf.tas, done_idx, 0) bank = np.delete(traf.bank, done_idx, 0) hdg = np.delete(traf.hdg, done_idx, 0) traflat = np.delete(traf.lat, done_idx, 0) traflon = np.delete(traf.lon, done_idx, 0) # data = [state[0], state[1], self.action] # model = self.critic.model # print_intermediate_layer_output(model, data, 'input_actions') # print_intermediate_layer_output(model, data, 'max_pool') # print_intermediate_layer_output(model, data, 'concatenate_inputs') # print_intermediate_layer_output(model, data, 'input_mask') # print_intermediate_layer_output(model, data, 'pre_brnn') # print_intermediate_layer_output(model, data, 'brnn') # print_intermediate_layer_output(model, data, 'post_brnn') # Exploration noise is added only when no test episode is running # print('OU', self.OU()) annealing_factor = 1. if self.summary_counter > 1000: annealing_factor = 1 - 0.0003 * self.summary_counter annealing_factor = np.max([annealing_factor, 0.15]) noise = self.OU()[0:n_aircraft].reshape(self.action.shape) # print(not self.summary_counter % CONF.test_freq, not self.train_indicator) # print('action', self.action) # print('noise', noise) # if not self.summary_counter % CONF.test_freq == 0 or not self.train_indicator: if self.train_indicator: if not self.summary_counter % CONF.test_freq == 0: # Add exploration noise and clip to range [-1, 1] for action space self.action = self.action + noise * annealing_factor # print('noise', noise, self.action) self.action = np.maximum(-1*np.ones(self.action.shape), self.action) self.action = np.minimum(np.ones(self.action.shape), self.action) # print('action', self.action) # Apply Bell curve here to sample from to get action values # mu = 0 # sigma = 0.5 # y_offset = 0.107982 + 6.69737e-08 # action = bell_curve(self.action[0], mu=mu, sigma=sigma, y_offset=y_offset, scaled=True) dist_limit = 5 # nm # dist = state[0][:,:, 4] * 110 """ dist = (obs[:,:,4]+1)/2 * 110 # denormalize # dist = dist.reshape(action.shape) mul_factor = 80*np.ones(dist.shape) """ # Compute multiplier for angle difference in action command turnrad = np.square(tas) / (np.maximum(0.01 * np.ones(tas.shape), np.tan(bank)) * g0) # [m] max_angle = (tas * CONF.update_interval) / (2 * np.pi * turnrad) * 360 dheading = self.action.reshape(max_angle.shape) * max_angle destidx = navdb.getaptidx('EHAM') lat, lon = navdb.aptlat[destidx], navdb.aptlon[destidx] qdr, dist = qdrdist(traflat, traflon, lat*np.ones(traflat.shape), lon*np.ones(traflon.shape)) # qdr, dist = np.asarray(qdr)[0], np.asarray(dist)[0] # print('qdr', qdr) qdr = (qdr+360)%360 # qdr = degtoqdr180(traf.hdg + dheading, qdr) heading = degtoqdr180(hdg + dheading, qdr) # traf.hdg + dheading + 360 # heading = np.maximum(-90*np.ones(heading.shape), heading) # heading = np.minimum(+90*np.ones(heading.shape), heading) # print("maxa {},\n dhdg {},\n qdr {},\n, , \n hdg1 {}\n, hdg2{}, \ntarget_hdg\n{}".format(max_angle, dheading, qdr, traf.hdg, heading, qdr180todeg(heading, qdr))) heading = qdr180todeg(heading, qdr) # print('HEADING', heading) return heading.ravel()[0:n_aircraft] wheredist = np.where(dist<dist_limit) mul_factor[wheredist] = dist[wheredist] / dist_limit * 80 minus = np.where(self.action.reshape(self.action.shape[1])<0)[0] plus = np.where(self.action.reshape(self.action.shape[1])>=0)[0] # dheading = np.zeros(action.shape) # dheading[minus] = (action[minus] - 1) * mul_factor[minus] # dheading[plus] = np.abs(action[plus]-1) * mul_factor[plus] # action[minus] = -1*action[minus] # qdr = state[0][:,:,3].transpose()*360-180 # Denormalize qdr = obs[:,:,3].transpose() * 180 #- 180 dheading = self.action * mul_factor.reshape(self.action.shape) # dheading = 90*np.ones(dheading.shape) # print('heading', dheading, self.action) heading = qdr + dheading print(qdr, dheading, heading, mul_factor, wheredist, dist) # print('action', self.action.ravel()) return heading.ravel()[0:n_aircraft]
def update(self): # FMS LNAV mode: # qdr[deg],distinnm[nm] qdr, distinnm = geo.qdrdist(bs.traf.lat, bs.traf.lon, bs.traf.actwp.lat, bs.traf.actwp.lon) # [deg][nm]) self.qdr2wp = qdr self.dist2wp = distinnm * nm # Conversion to meters # FMS route update and possibly waypoint shift. Note: qdr, dist2wp will be updated accordingly in case of wp switch self.update_fms(qdr, self.dist2wp) # Updates self.qdr2wp when necessary #================= Continuous FMS guidance ======================== # Note that the code below is vectorized, with traffic arrays, so for all aircraft # ComputeVNAV and inside waypoint loop of update_fms, it was scalar (per a/c with index i) # VNAV guidance logic (using the variables prepared by ComputeVNAV when activating waypoint) # # Central question is: # - Can we please we start to descend or to climb? # # Well, when Top of Descent (ToD) switch is on, descend as late as possible, # But when Top of Climb switch is on or off, climb as soon as possible, only difference is steepness used in ComputeVNAV # to calculate bs.traf.actwp.vs # Only use this logic if there is a valid next altitude constraint (nextaltco) startdescorclimb = (bs.traf.actwp.nextaltco>=-0.1) * \ np.logical_or(self.swtod * (bs.traf.alt>bs.traf.actwp.nextaltco) * (self.dist2wp < self.dist2vs), bs.traf.alt<bs.traf.actwp.nextaltco) #print("self.dist2vs =",self.dist2vs) # If not lnav:Climb/descend if doing so before lnav/vnav was switched off # (because there are no more waypoints). This is needed # to continue descending when you get into a conflict # while descending to the destination (the last waypoint) # Use 0.1 nm (185.2 m) circle in case turndist might be zero self.swvnavvs = bs.traf.swvnav * np.where( bs.traf.swlnav, startdescorclimb, self.dist2wp <= np.maximum(0.1 * nm, bs.traf.actwp.turndist)) # Recalculate V/S based on current altitude and distance to next alt constraint # How much time do we have before we need to descend? # Now done in ComputeVNAV # See ComputeVNAV for bs.traf.actwp.vs calculation self.vnavvs = np.where(self.swvnavvs, bs.traf.actwp.vs, self.vnavvs) #was: self.vnavvs = np.where(self.swvnavvs, self.steepness * bs.traf.gs, self.vnavvs) # self.vs = np.where(self.swvnavvs, self.vnavvs, self.vsdef * bs.traf.limvs_flag) # for VNAV use fixed V/S and change start of descent selvs = np.where(abs(bs.traf.selvs) > 0.1, bs.traf.selvs, self.vsdef) # m/s self.vs = np.where(self.swvnavvs, self.vnavvs, selvs) self.alt = np.where(self.swvnavvs, bs.traf.actwp.nextaltco, bs.traf.selalt) # When descending or climbing in VNAV also update altitude command of select/hold mode bs.traf.selalt = np.where(self.swvnavvs, bs.traf.actwp.nextaltco, bs.traf.selalt) # LNAV commanded track angle self.trk = np.where(bs.traf.swlnav, self.qdr2wp, self.trk) # FMS speed guidance: anticipate accel/decel distance for next leg or turn # Calculate actual distance it takes to decelerate/accelerate based on two cases: turning speed (decel) # Normally next leg speed (actwp.spd) but in case we fly turns with a specified turn speed # use the turn speed # Is turn speed specified and are we not already slow enough? We only decelerate for turns, not accel. turntas = np.where(bs.traf.actwp.nextturnspd > 0.0, vcas2tas(bs.traf.actwp.nextturnspd, bs.traf.alt), -1.0 + 0. * bs.traf.tas) # Switch is now whether the aircraft has any turn waypoints swturnspd = bs.traf.actwp.nextturnidx > 0 turntasdiff = np.maximum(0., (bs.traf.tas - turntas) * (turntas > 0.0)) # t = (v1-v0)/a ; x = v0*t+1/2*a*t*t => dx = (v1*v1-v0*v0)/ (2a) dxturnspdchg = distaccel(turntas, bs.traf.perf.vmax, bs.traf.perf.axmax) # dxturnspdchg = 0.5*np.abs(turntas*turntas-bs.traf.tas*bs.traf.tas)/(np.sign(turntas-bs.traf.tas)*np.maximum(0.01,np.abs(ax))) # dxturnspdchg = np.where(swturnspd, np.abs(turntasdiff)/np.maximum(0.01,ax)*(bs.traf.tas+0.5*np.abs(turntasdiff)), # 0.0*bs.traf.tas) # Decelerate or accelerate for next required speed because of speed constraint or RTA speed # Note that because nextspd comes from the stack, and can be either a mach number or # a calibrated airspeed, it can only be converted from Mach / CAS [kts] to TAS [m/s] # once the altitude is known. nexttas = vcasormach2tas(bs.traf.actwp.nextspd, bs.traf.alt) # tasdiff = (nexttas - bs.traf.tas)*(bs.traf.actwp.spd>=0.) # [m/s] # t = (v1-v0)/a ; x = v0*t+1/2*a*t*t => dx = (v1*v1-v0*v0)/ (2a) dxspdconchg = distaccel(bs.traf.tas, nexttas, bs.traf.perf.axmax) qdrturn, dist2turn = geo.qdrdist(bs.traf.lat, bs.traf.lon, bs.traf.actwp.nextturnlat, bs.traf.actwp.nextturnlon) self.qdrturn = qdrturn dist2turn = dist2turn * nm # Where we don't have a turn waypoint, as in turn idx is negative, then put distance # as Earth circumference. self.dist2turn = np.where(bs.traf.actwp.nextturnidx > 0, dist2turn, 40075000) # Check also whether VNAVSPD is on, if not, SPD SEL has override for next leg # and same for turn logic usenextspdcon = (self.dist2wp < dxspdconchg)*(bs.traf.actwp.nextspd>-990.) * \ bs.traf.swvnavspd*bs.traf.swvnav*bs.traf.swlnav useturnspd = np.logical_or(bs.traf.actwp.turntonextwp, (self.dist2turn < (dxturnspdchg+bs.traf.actwp.turndist))) * \ swturnspd*bs.traf.swvnavspd*bs.traf.swvnav*bs.traf.swlnav # Hold turn mode can only be switched on here, cannot be switched off here (happeps upon passing wp) bs.traf.actwp.turntonextwp = bs.traf.swlnav * np.logical_or( bs.traf.actwp.turntonextwp, useturnspd) # Which CAS/Mach do we have to keep? VNAV, last turn or next turn? oncurrentleg = (abs(degto180(bs.traf.trk - qdr)) < 2.0) # [deg] inoldturn = (bs.traf.actwp.oldturnspd > 0.) * np.logical_not(oncurrentleg) # Avoid using old turning speeds when turning of this leg to the next leg # by disabling (old) turningspd when on leg bs.traf.actwp.oldturnspd = np.where( oncurrentleg * (bs.traf.actwp.oldturnspd > 0.), -998., bs.traf.actwp.oldturnspd) # turnfromlastwp can only be switched off here, not on (latter happens upon passing wp) bs.traf.actwp.turnfromlastwp = np.logical_and( bs.traf.actwp.turnfromlastwp, inoldturn) # Select speed: turn sped, next speed constraint, or current speed constraint bs.traf.selspd = np.where( useturnspd, bs.traf.actwp.nextturnspd, np.where( usenextspdcon, bs.traf.actwp.nextspd, np.where((bs.traf.actwp.spdcon >= 0) * bs.traf.swvnavspd, bs.traf.actwp.spd, bs.traf.selspd))) # Temporary override when still in old turn bs.traf.selspd = np.where( inoldturn * (bs.traf.actwp.oldturnspd > 0.) * bs.traf.swvnavspd * bs.traf.swvnav * bs.traf.swlnav, bs.traf.actwp.oldturnspd, bs.traf.selspd) self.inturn = np.logical_or(useturnspd, inoldturn) #debug if inoldturn[0]: #debug print("inoldturn bs.traf.trk =",bs.traf.trk[0],"qdr =",qdr) #debug elif usenextspdcon[0]: #debug print("usenextspdcon") #debug elif useturnspd[0]: #debug print("useturnspd") #debug elif bs.traf.actwp.spdcon>0: #debug print("using current speed constraint") #debug else: #debug print("no speed given") # Below crossover altitude: CAS=const, above crossover altitude: Mach = const self.tas = vcasormach2tas(bs.traf.selspd, bs.traf.alt)