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)
