def get_pilot_data(self):
log_file = open("log.txt", "r")
first_line = True
list_of_pilots = {}
for line in log_file:
if first_line:
first_line = not first_line
continue
log_line = LogLine(line)
if log_line.get_pilot_code() in list_of_pilots:
pilot = list_of_pilots.get(log_line.get_pilot_code())
else:
pilot = Pilot(log_line.get_pilot_code(),
log_line.get_pilot_name())
list_of_pilots[pilot.get_code()] = pilot
lap = Lap(log_line.get_lap_number(), log_line.get_lap_time(),
log_line.get_lap_average_speed(),
log_line.get_lap_hour())
pilot.add_lap(lap)
log_file.close()
return list_of_pilots
def test_types():
class A(object):
pass
class B(object):
pass
class C(A, B):
pass
object_list = [
"test", u'test', 1, 1L, 1.0, [], {},
A(),
B(),
C(),
type("NewClass", (object, ), {})(), False, None
]
def node_check(node):
assert node.val.__node__.__class__.__name__ is 'Node'
#cb = Callback(node_check)
pilot = Pilot(node_check)
pilot.fly(object_list)
return 1
def test_types():
class A(object): pass
class B(object): pass
class C(A, B): pass
object_list = [
"test",
u'test',
1,
1L,
1.0,
[],
{},
A(),
B(),
C(),
type("NewClass", (object,), {})(),
False,
None
]
def node_check(node):
assert node.val.__node__.__class__.__name__ is 'Node'
#cb = Callback(node_check)
pilot = Pilot(node_check)
pilot.fly(object_list)
return 1
def __init__(self):
# Contains all upgrades
self.upgrades = [
# All Talent Upgrades
Upgrade('Composure', 1, 'Talent'),
Upgrade('Deadeye Shot', 1, 'Talent'),
Upgrade('Heroic', 1, 'Talent', ['Resistance']),
Upgrade('Marg Sabl Closure', 1, 'Talent')
]
self.ships = [
### REBEL SHIPS ###
Ship(Pilot('Norra Wexley', 55, ['Talent', 'Crew', 'Astromech', 'Torpedo', 'Gunner', 'Modification'], 'While you defend, if there is an enemy ship at range 0-1, add 1 evade result to your dice results.')
,'ARC-170 Starfighter', ['Rebel'], '', [5, '^3', 'v2', 1, 6, 3, '0', '0'], ['Focus', 'Target Lock', 'Red Boost', 'Red Rotate']),
### RESISTANCE SHIPS ###
# Scavenged YT-1300 Pilots
Ship(Pilot('Rey', 68, ['Force', 'Crew', 'Gunner', 'Modification', 'Missle', 'Crew', 'Ilicit', 'Title'], 'While you defend or perform an attack, if the enemy ship is in your forward arc, you my spend 1 force to change 1 of your blank results to an evade or hit result.')
,'Scavenged YT-1300', ['Resistance'], '', [5, '^3', 1, 8, 3, '0', '^2'], ['Focus', 'Target Lock', 'Red Boost', 'Red Rotate']),
# T-70 X-Wing Pilots
Ship(Pilot('Poe Dameron [HoH]', 60, ['Talent', 'Tech', 'Configuration', 'Hardpoint', 'Astromech', 'Modification', 'Title'], 'After a friendly ship at range 0-2 performs an action during its activation, you may spend 2 charge. If you do, that ship may perform a white action, treating it as red.')
,'T-70 X-Wing', ['Resistance'], 'Weapon Hardpoint: You can equip 1 cannon, torpedo, or missle upgrade', [6, '^3', 2, 4, 3, '^2', '0'], ['Focus', 'Target Lock', 'Boost']),
Ship(Pilot('Poe Dameron', 62, ['Talent', 'Tech', 'Configuration', 'Hardpoint', 'Astromech', 'Modification', 'Title'], 'After you perform an action, you may spend 1 charge to perform a white action, treating it as red')
,'T-70 X-Wing', ['Resistance'], 'Weapon Hardpoint: You can equip 1 cannon, torpedo, or missle upgrade', [6, '^3', 2, 4, 3, '^1', '0'], ['Focus', 'Target Lock', 'Boost']),
Ship(Pilot('Nien Nunb', 55, ['Talent', 'Tech', 'Configuration', 'Hardpoint', 'Astromech', 'Modification', 'Title'], 'After you gain a stress token, if there is an enemy ship in your forward arc at range 0-1, you may remove that stress token.')
,'T-70 X-Wing', ['Resistance'], 'Weapon Hardpoint: You can equip 1 cannon, torpedo, or missle upgrade', [6, '^3', 2, 4, 3, '0', '0'], ['Focus', 'Target Lock', 'Boost'])
]
def main():
military_airplane = AV8B()
control_system = ControlSystem(military_airplane)
crew = [Pilot("Kolya Stepanov", 10), Pilot("Taras Lopasov", 7), "Natalya Kolesnikova"]
boeing737_airplane = Boeing737(crew)
show_pilot_info(military_airplane.pilot)
print("\nMilitary airplane pilot focusing on fly...\n")
military_airplane.pilot.focus_on_flying()
show_pilot_info(military_airplane.pilot)
print("\nAirplane status:")
show_airplane_info(military_airplane)
print("\nAirplane status:")
show_airplane_info(boeing737_airplane)
print("\nMilitary airplane pilot moving helm 'Up' and pull lever to '6000'...")
military_airplane.pilot.move_helm("Up", control_system)
military_airplane.pilot.pull_lever(6000, control_system)
print("\nAirplane start flying...")
for _ in range(10):
print("\nAirplane status (main):")
show_airplane_info(military_airplane, show_main=True)
iteration([military_airplane, boeing737_airplane])
sleep(1)
print("\nMilitary airplane pilot enabling autopilot...")
autopilot_status = military_airplane.pilot.enable_autopilot(control_system, 10, debug=True)
print("\nAutopilot finished work with status", autopilot_status)
def run(self):
print("Start Listening")
while True:
# set all pilots at the first "race" at the server starts
if self.status is None and self.totalLaps is not None:
self.status = [Pilot(self.totalLaps) for _ in range(20)]
udp_packet = self.udp_socket.recv(2048)
packet = unpack_udp_packet(udp_packet)
if isinstance(packet, PacketEventData_V1):
if packet.eventStringCode == b"SSTA":
# reset fastest player at start, else keep data from previoous race
self.update_fastest_pilot()
if packet.eventStringCode == b'FTLP':
self.update_fastest_pilot()
if packet.eventStringCode == b"SEND":
pass # for now
if isinstance(packet, PacketSessionData_V1):
self.race_info["track"] = TrackIDs[packet.trackId]
self.race_info["weather"] = self.weatherID[packet.weather]
self.race_info["trackTemperature"] = packet.trackTemperature
self.race_info["airTemperature"] = packet.airTemperature
self.race_info[
"sessionDuration"] = packet.sessionDuration - packet.sessionTimeLeft
self.race_info["sessionType"] = self.SessType[
packet.sessionType]
if self.totalLaps != packet.totalLaps:
# reset pilots when the number of lap changes (usually due to map change)
self.totalLaps = packet.totalLaps
self.status = [Pilot(self.totalLaps) for _ in range(20)]
if isinstance(packet, PacketLapData_V1):
for i, lap in enumerate(packet.lapData):
self.status[i].lap = lap.currentLapNum
self.status[i].position = lap.carPosition # starts at 0
self.status[i].fastest_lap = lap.bestLapTime
self.status[i].status = lap.resultStatus
if self.race_info["sessionType"] == "Race":
self.status[
i].gridPosition = lap.gridPosition + 1 # starts at 1
else:
self.status[i].gridPosition = -1
if isinstance(packet, PacketParticipantsData_V1):
for i, participant in enumerate(packet.participants):
self.status[i].name = self.DriverIDs[participant.driverId]
self.status[i].team = TeamIDs[participant.teamId]
self.status[i].is_bot = bool(participant.aiControlled)
if isinstance(packet, PacketCarStatusData_V1):
for i, car in enumerate(packet.carStatusData):
self.status[i].tyre = self.tyresID[car.tyreVisualCompound]
self.status[i].wear = sum(car.tyresWear) / 4
def test_tree():
pilot = Pilot()
data = pilot.fly(data_small)
assert len(data.__node__.children()) is 8
assert data.__node__.is_orphan is True
family = data['family']
assert len(family.__node__.parents()) is 1
assert len(family.__node__.children()) is 4
for child in family.__node__.children(as_generator=True):
if 'name' in child.val and child.val['name'] == "Jeri Bowen":
nest = child.val['nested'].__node__.children()[0].children()[0].val
assert nest.val is True
assert len(nest.__node__.ancestors()) is 5
break
assert len(data.__node__.descendants()) is 58
assert len(data.__node__.descendants(filters={'type': 'person'})) is 13
return 1
def test_loop():
process_count = 200
nl = {}
def cb(node):
if not str(node.id) in nl:
nl[str(node.id)] = 1
else:
nl[str(node.id)] += 1
a = {}
b = {'a':a}
a['b'] = b
pilot = Pilot(cb)
pilot.config.node_visit_limit = process_count
data = pilot.fly(a)
assert data.__node__.processed is process_count
assert data['b'].__node__.processed is process_count
return 1
def make(self, position=(0,0)): arena = self.arena ship = Ship( arena=arena, pilot=Pilot(), blueprint=self.blueprinter.make(), position=position) return ship
def test_tree():
pilot = Pilot()
data = pilot.fly(data_small)
assert len(data.__node__.children()) is 8
assert data.__node__.is_orphan is True
family = data['family']
assert len(family.__node__.parents()) is 1
assert len(family.__node__.children()) is 4
for child in family.__node__.children(as_generator=True):
if 'name' in child.val and child.val['name'] == "Jeri Bowen":
nest = child.val['nested'].__node__.children()[0].children()[0].val
assert nest.val is True
assert len(nest.__node__.ancestors()) is 5
break
assert len(data.__node__.descendants()) is 58
assert len(data.__node__.descendants(filters={'type': 'person'})) is 13
return 1
def execute(self):
logger.info("executing RunAppJob %s on device %s" %
(self.jobId, self.device))
backendJobData = self.backend.get_job(self.jobId)
## set job running
backendJobData['state'] = Job.STATE.RUNNING
self.backend.post_job(backendJobData)
pilot = Pilot(self.device.base_url())
try:
installDone = self._install_app(pilot)
if not self.appId:
raise JobExecutionError("No appId present")
jobInfo = self.jobDict['jobInfo']
bundleId = jobInfo['bundleId']
if self.device.ios_version()[0] > 8:
logger.debug(
"skipping app archiving since device is running iOS 9 or later"
)
else:
if not self.backend.has_app_archive(self.appId):
self._archive_app_binary(bundleId)
executionStrategy = None
if 'executionStrategy' in jobInfo:
executionStrategy = jobInfo['executionStrategy']
logger.debug('post_run')
## add run to backend
runId = self.backend.post_run(self.appId,
self.backend.RUN_STATE.RUNNING)
logger.info('starting app pilot execution')
self._execute_app(pilot, bundleId, runId, executionStrategy)
if installDone:
logger.info("uninstalling app (%s)" % bundleId)
self.device.uninstall(bundleId)
# # save the results and install the app if not previously installed
# self._save_run_results(runId, bundleId, uninstallApp=installDone)
## set run finished
self.backend.post_run(self.appId,
self.backend.RUN_STATE.FINISHED,
runId=runId,
executionStrategy=executionStrategy)
except JobExecutionError, e:
logger.error("Job execution failed: %s" % str(e))
backendJobData['state'] = Job.STATE.FAILED
self.backend.post_job(backendJobData)
return False
def test_loop():
process_count = 200
nl = {}
def cb(node):
if not str(node.id) in nl:
nl[str(node.id)] = 1
else:
nl[str(node.id)] += 1
a = {}
b = {'a': a}
a['b'] = b
pilot = Pilot(cb)
pilot.config.node_visit_limit = process_count
data = pilot.fly(a)
assert data.__node__.processed is process_count
assert data['b'].__node__.processed is process_count
return 1
def __init__(self):
# Init Tello object that interacts with the Tello drone
tello = Tello(logging=False)
self.tello = tello
if not self.tello.connect():
raise Exception("Tello not connected")
if not self.tello.set_speed(10):
raise Exception("Not set speed to lowest possible")
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
raise Exception("Could not stop video stream")
if not self.tello.streamon():
raise Exception("Could not start video stream")
self.frame_read = tello.get_frame_read()
self.pilot = Pilot(tello, self)
self.key_ctrl = KeyboardController(tello)
def read_and_process_data(filename):
file = open(filename, "r")
data = collections.defaultdict(dict)
for line in file:
if not line.startswith("Hora"):
line_splitted = line.split()
code = line_splitted[1]
name = line_splitted[3]
lap = line_splitted[4]
time = line_splitted[5]
speed = line_splitted[6]
data.setdefault(code, Pilot(name, code)).add_lap(lap, time, speed)
return data
def main():
"""Main function for the app. Handles creation and destruction of every element of the application."""
# Check and generate configuration
config_data = check_args(sys.argv)
app_configuration = Config(config_data['config'])
# Create controller of model-view
controller = Controller()
# If there's no config, configure the app through the GUI
if app_configuration.empty and config_data['gui']:
conf_window(app_configuration)
# Launch the simulation
if app_configuration.current_world:
logger.debug('Launching Simulation... please wait...')
environment.launch_env(app_configuration.current_world)
if config_data['tui']:
rows, columns = os.popen('stty size', 'r').read().split()
if rows < 34 and columns < 115:
logger.error(
"Terminal window too small: {}x{}, please resize it to at least 35x115"
.format(rows, columns))
sys.exit(1)
else:
t = TUI(controller)
ttui = threading.Thread(target=t.run)
ttui.start()
# Launch control
pilot = Pilot(app_configuration, controller)
pilot.daemon = True
pilot.start()
logger.info('Executing app')
# If GUI specified, launch it. Otherwise don't
if config_data['gui']:
main_win(app_configuration, controller)
else:
pilot.join()
# When window is closed or keypress for quit is detected, quit gracefully.
logger.info('closing all processes...')
pilot.kill_event.set()
environment.close_gazebo()
logger.info('DONE! Bye, bye :)')
def test_get_interval_from_pilot(self):
lap1 = Lap("1", "1:02.852", "44,275", "23:49:08.277")
lap2 = Lap("2", "1:02.851", "63,235", "23:49:08.277")
pilot2 = Pilot("037", "R.MASSA")
self.assertEqual("N/D", self.__pilot.get_interval_from_pilot(pilot2))
self.__pilot.add_lap(lap2)
self.__pilot.add_lap(lap1)
lap1 = Lap("1", "1:02.852", "44,275", "23:49:08.277")
lap2 = Lap("2", "1:02.851", "63,235", "23:49:08.277")
pilot2.add_lap(lap2)
pilot2.add_lap(lap1)
pilot3 = Pilot("037", "R.MASSA")
lap1 = Lap("1", "1:02.852", "44,275", "23:49:08.277")
pilot3.add_lap(lap1)
self.assertEqual(timedelta(),
self.__pilot.get_interval_from_pilot(pilot2))
self.assertEqual("N/D", self.__pilot.get_interval_from_pilot(pilot3))
def execute(self):
logger.info("executing InstallAppJob %s on device %s" %
(self.jobId, self.device))
# allow InstallAppJobs to exist/run without a corresponding backendJob
backendJobData = {}
if self.jobId:
backendJobData = self.backend.get_job(self.jobId)
## set job running
backendJobData['state'] = Job.STATE.RUNNING
self.backend.post_job(backendJobData)
pilot = Pilot(self.device.base_url())
result = True
try:
self.appJustInstalled = self._install_app(pilot)
if not self.appId:
raise JobExecutionError("No appId present")
jobInfo = self.jobDict['jobInfo']
bundleId = jobInfo['bundleId']
if self.device.ios_version()[0] > 8:
logger.debug("try archiving app with tweak")
if not self.backend.has_app_archive(self.appId):
self._archive_app_binary(bundleId)
else:
logger.debug("check if backend already has an app ipa")
if not self.backend.has_app_archive(self.appId):
self._archive_app_binary(bundleId)
backendJobData['state'] = Job.STATE.FINISHED
except JobExecutionError, e:
logger.error("Job execution failed: %s" % str(e))
backendJobData['state'] = Job.STATE.FAILED
result = False
#!/usr/bin/env python3
# Author: Kari Lavikka
from ev3dev.ev3 import *
from time import sleep, time
from pilot import Pilot
sensor_distance = 5.5 # Distance from wheel axis
pilot = Pilot(4.25, 14.2, LargeMotor("outA"), LargeMotor("outB"))
col = ColorSensor()
col.mode = 'COL-REFLECT'
def interpolate(a, b, ratio):
return a + (b - a) * ratio
def measure_line_reflections():
print("Measuring reflections:")
pilot.max_speed = 150
pilot.travel_indefinitely()
measurements = list()
while pilot.get_travelled_distance() < 13:
measurements.append((pilot.get_travelled_distance(), col.value()))
sleep(0.05)
def execute(self):
if self.process and self.execute:
pilot = Pilot(self.device.base_url())
pilot.inject(self.process, self.command)
else:
raise JobExecutionError("Process or command missing")
from pilot import Pilot
if __name__ == '__main__':
p = Pilot()
p.drone.land()
p.drone.halt()
def setUp(self):
self.__pilot = Pilot("038", "F.MASSA")
class TestPilot(unittest.TestCase):
@classmethod
def setUp(self):
self.__pilot = Pilot("038", "F.MASSA")
def test_get_code(self):
self.assertEqual("038", self.__pilot.get_code())
def test_get_name(self):
self.assertEqual("F.MASSA", self.__pilot.get_name())
def test_add_lap(self):
lap = Lap("1", "1:02.852", "44,275", "23:49:08.277")
self.__pilot.add_lap(lap)
self.assertEqual(lap, self.__pilot.get_last_lap())
def test_get_last_lap(self):
lap1 = Lap("1", "1:02.852", "44,275", "23:49:08.277")
lap2 = Lap("2", "1:02.852", "44,275", "23:49:08.277")
self.assertIsNone(self.__pilot.get_last_lap())
self.__pilot.add_lap(lap2)
self.__pilot.add_lap(lap1)
self.assertEqual(lap2, self.__pilot.get_last_lap())
def test_sum_lap_time(self):
lap1 = Lap("1", "1:02.852", "44,275", "23:49:08.277")
lap2 = Lap("2", "1:02.851", "44,275", "23:49:08.277")
self.__pilot.add_lap(lap2)
self.__pilot.add_lap(lap1)
self.assertEqual(lap2.get_time() + lap1.get_time(),
self.__pilot.sum_lap_time())
def test_best_lap_time(self):
lap1 = Lap("1", "1:02.852", "44,275", "23:49:08.277")
lap2 = Lap("2", "1:02.851", "63,235", "23:49:08.277")
self.__pilot.add_lap(lap2)
self.__pilot.add_lap(lap1)
self.assertEqual(lap2.get_time(), self.__pilot.get_best_lap_time())
def test_get_average_match_speed(self):
lap1 = Lap("1", "1:02.852", "44,275", "23:49:08.277")
lap2 = Lap("2", "1:02.851", "63,235", "23:49:08.277")
self.__pilot.add_lap(lap2)
self.__pilot.add_lap(lap1)
self.assertEqual(53.755, self.__pilot.get_average_match_speed())
def test_get_interval_from_pilot(self):
lap1 = Lap("1", "1:02.852", "44,275", "23:49:08.277")
lap2 = Lap("2", "1:02.851", "63,235", "23:49:08.277")
pilot2 = Pilot("037", "R.MASSA")
self.assertEqual("N/D", self.__pilot.get_interval_from_pilot(pilot2))
self.__pilot.add_lap(lap2)
self.__pilot.add_lap(lap1)
lap1 = Lap("1", "1:02.852", "44,275", "23:49:08.277")
lap2 = Lap("2", "1:02.851", "63,235", "23:49:08.277")
pilot2.add_lap(lap2)
pilot2.add_lap(lap1)
pilot3 = Pilot("037", "R.MASSA")
lap1 = Lap("1", "1:02.852", "44,275", "23:49:08.277")
pilot3.add_lap(lap1)
self.assertEqual(timedelta(),
self.__pilot.get_interval_from_pilot(pilot2))
self.assertEqual("N/D", self.__pilot.get_interval_from_pilot(pilot3))
class Traffic(DynamicArrays):
"""
Traffic class definition : Traffic data
Methods:
Traffic() : constructor
reset() : Reset traffic database w.r.t a/c data
create(acid,actype,aclat,aclon,achdg,acalt,acspd) : create aircraft
delete(acid) : delete an aircraft from traffic data
deletall() : delete all traffic
update(sim) : do a numerical integration step
id2idx(name) : return index in traffic database of given call sign
engchange(i,engtype) : change engine type of an aircraft
setNoise(A) : Add turbulence
Members: see create
Created by : Jacco M. Hoekstra
"""
def __init__(self):
self.wind = WindSim()
# Define the periodic loggers
# ToDo: explain what these line sdo in comments (type of logs?)
datalog.definePeriodicLogger('SNAPLOG', 'SNAPLOG logfile.', settings.snapdt)
datalog.definePeriodicLogger('INSTLOG', 'INSTLOG logfile.', settings.instdt)
datalog.definePeriodicLogger('SKYLOG', 'SKYLOG logfile.', settings.skydt)
with RegisterElementParameters(self):
# Register the following parameters for logging
with datalog.registerLogParameters('SNAPLOG', self):
# Aircraft Info
self.id = [] # identifier (string)
self.type = [] # aircaft type (string)
# Positions
self.lat = np.array([]) # latitude [deg]
self.lon = np.array([]) # longitude [deg]
self.alt = np.array([]) # altitude [m]
self.hdg = np.array([]) # traffic heading [deg]
self.trk = np.array([]) # track angle [deg]
# Velocities
self.tas = np.array([]) # true airspeed [m/s]
self.gs = np.array([]) # ground speed [m/s]
self.gsnorth = np.array([]) # ground speed [m/s]
self.gseast = np.array([]) # ground speed [m/s]
self.cas = np.array([]) # calibrated airspeed [m/s]
self.M = np.array([]) # mach number
self.vs = np.array([]) # vertical speed [m/s]
# Atmosphere
self.p = np.array([]) # air pressure [N/m2]
self.rho = np.array([]) # air density [kg/m3]
self.Temp = np.array([]) # air temperature [K]
self.dtemp = np.array([]) # delta t for non-ISA conditions
# Traffic autopilot settings
self.aspd = np.array([]) # selected spd(CAS) [m/s]
self.aptas = np.array([]) # just for initializing
self.ama = np.array([]) # selected spd above crossover altitude (Mach) [-]
self.apalt = np.array([]) # selected alt[m]
self.avs = np.array([]) # selected vertical speed [m/s]
# Whether to perform LNAV and VNAV
self.swlnav = np.array([], dtype=np.bool)
self.swvnav = np.array([], dtype=np.bool)
# Flight Models
self.asas = ASAS()
self.ap = Autopilot()
self.pilot = Pilot()
self.adsb = ADSB()
self.trails = Trails()
self.actwp = ActiveWaypoint()
# Traffic performance data
self.avsdef = np.array([]) # [m/s]default vertical speed of autopilot
self.aphi = np.array([]) # [rad] bank angle setting of autopilot
self.ax = np.array([]) # [m/s2] absolute value of longitudinal accelleration
self.bank = np.array([]) # nominal bank angle, [radian]
self.hdgsel = np.array([], dtype=np.bool) # determines whether aircraft is turning
# Crossover altitude
self.abco = np.array([])
self.belco = np.array([])
# limit settings
self.limspd = np.array([]) # limit speed
self.limspd_flag = np.array([], dtype=np.bool) # flag for limit spd - we have to test for max and min
self.limalt = np.array([]) # limit altitude
self.limvs = np.array([]) # limit vertical speed due to thrust limitation
self.limvs_flag = np.array([])
# Display information on label
self.label = [] # Text and bitmap of traffic label
# Miscallaneous
self.coslat = np.array([]) # Cosine of latitude for computations
self.eps = np.array([]) # Small nonzero numbers
# Default bank angles per flight phase
self.bphase = np.deg2rad(np.array([15, 35, 35, 35, 15, 45]))
self.reset()
def reset(self):
# This ensures that the traffic arrays (which size is dynamic)
# are all reset as well, so all lat,lon,sdp etc but also objects adsb
super(Traffic, self).reset()
self.ntraf = 0
# Reset models
self.wind.clear()
# Build new modules for area and turbulence
self.area = Area()
self.Turbulence = Turbulence()
# Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)
self.setNoise(False)
# Default: BlueSky internal performance model.
# Insert your BADA files to the folder "BlueSky/data/coefficients/BADA"
# for working with EUROCONTROL`s Base of Aircraft Data revision 3.12
self.perf = Perf()
self.trails.reset()
def mcreate(self, count, actype=None, alt=None, spd=None, dest=None):
""" Create multiple random aircraft in a specified area """
area = bs.scr.getviewlatlon()
idbase = chr(randint(65, 90)) + chr(randint(65, 90))
if actype is None:
actype = 'B744'
n = count
super(Traffic, self).create(n)
# Increase number of aircraft
self.ntraf = self.ntraf + count
acids = []
aclats = []
aclons = []
achdgs = []
acalts = []
acspds = []
for i in xrange(count):
acids.append((idbase + '%05d' % i).upper())
aclats.append(random() * (area[1] - area[0]) + area[0])
aclons.append(random() * (area[3] - area[2]) + area[2])
achdgs.append(float(randint(1, 360)))
acalts.append((randint(2000, 39000) * ft) if alt is None else alt)
acspds.append((randint(250, 450) * kts) if spd is None else spd)
# Aircraft Info
self.id[-n:] = acids
self.type[-n:] = [actype] * n
# Positions
self.lat[-n:] = aclats
self.lon[-n:] = aclons
self.alt[-n:] = acalts
self.hdg[-n:] = achdgs
self.trk[-n:] = achdgs
# Velocities
self.tas[-n:], self.cas[-n:], self.M[-n:] = vcasormach(acspds, acalts)
self.gs[-n:] = self.tas[-n:]
self.gsnorth[-n:] = self.tas[-n:] * np.cos(np.radians(self.hdg[-n:]))
self.gseast[-n:] = self.tas[-n:] * np.sin(np.radians(self.hdg[-n:]))
# Atmosphere
self.p[-n:], self.rho[-n:], self.Temp[-n:] = vatmos(acalts)
# Wind
if self.wind.winddim > 0:
vnwnd, vewnd = self.wind.getdata(self.lat[-n:], self.lon[-n:], self.alt[-n:])
self.gsnorth[-n:] = self.gsnorth[-n:] + vnwnd
self.gseast[-n:] = self.gseast[-n:] + vewnd
self.trk[-n:] = np.degrees(np.arctan2(self.gseast[-n:], self.gsnorth[-n:]))
self.gs[-n:] = np.sqrt(self.gsnorth[-n:]**2 + self.gseast[-n:]**2)
# Traffic performance data
#(temporarily default values)
self.avsdef[-n:] = 1500. * fpm # default vertical speed of autopilot
self.aphi[-n:] = np.radians(25.) # bank angle setting of autopilot
self.ax[-n:] = kts # absolute value of longitudinal accelleration
self.bank[-n:] = np.radians(25.)
# Crossover altitude
self.abco[-n:] = 0 # not necessary to overwrite 0 to 0, but leave for clarity
self.belco[-n:] = 1
# Traffic autopilot settings
self.aspd[-n:] = self.cas[-n:]
self.aptas[-n:] = self.tas[-n:]
self.apalt[-n:] = self.alt[-n:]
# Display information on label
self.label[-n:] = ['', '', '', 0]
# Miscallaneous
self.coslat[-n:] = np.cos(np.radians(aclats)) # Cosine of latitude for flat-earth aproximations
self.eps[-n:] = 0.01
# ----- Submodules of Traffic -----
self.ap.create(n)
self.actwp.create(n)
self.pilot.create(n)
self.adsb.create(n)
self.area.create(n)
self.asas.create(n)
self.perf.create(n)
self.trails.create(n)
def create(self, acid=None, actype="B744", aclat=None, aclon=None, achdg=None, acalt=None, casmach=None):
"""Create an aircraft"""
# Check if not already exist
if self.id.count(acid.upper()) > 0:
return False, acid + " already exists." # already exists do nothing
# Catch missing acid, replace by a default
if acid is None or acid == "*":
acid = "KL204"
flno = 204
while self.id.count(acid) > 0:
flno = flno + 1
acid = "KL" + str(flno)
# Check for (other) missing arguments
if actype is None or aclat is None or aclon is None or achdg is None \
or acalt is None or casmach is None:
return False, "CRE: Missing one or more arguments:"\
"acid,actype,aclat,aclon,achdg,acalt,acspd"
super(Traffic, self).create()
# Increase number of aircraft
self.ntraf = self.ntraf + 1
# Aircraft Info
self.id[-1] = acid.upper()
self.type[-1] = actype
# Positions
self.lat[-1] = aclat
self.lon[-1] = aclon
self.alt[-1] = acalt
self.hdg[-1] = achdg
self.trk[-1] = achdg
# Velocities
self.tas[-1], self.cas[-1], self.M[-1] = casormach(casmach, acalt)
self.gs[-1] = self.tas[-1]
self.gsnorth[-1] = self.tas[-1] * cos(radians(self.hdg[-1]))
self.gseast[-1] = self.tas[-1] * sin(radians(self.hdg[-1]))
# Atmosphere
self.p[-1], self.rho[-1], self.Temp[-1] = vatmos(acalt)
# Wind
if self.wind.winddim > 0:
vnwnd, vewnd = self.wind.getdata(self.lat[-1], self.lon[-1], self.alt[-1])
self.gsnorth[-1] = self.gsnorth[-1] + vnwnd
self.gseast[-1] = self.gseast[-1] + vewnd
self.trk[-1] = np.degrees(np.arctan2(self.gseast[-1], self.gsnorth[-1]))
self.gs[-1] = np.sqrt(self.gsnorth[-1]**2 + self.gseast[-1]**2)
# Traffic performance data
#(temporarily default values)
self.avsdef[-1] = 1500. * fpm # default vertical speed of autopilot
self.aphi[-1] = radians(25.) # bank angle setting of autopilot
self.ax[-1] = kts # absolute value of longitudinal accelleration
self.bank[-1] = radians(25.)
# Crossover altitude
self.abco[-1] = 0 # not necessary to overwrite 0 to 0, but leave for clarity
self.belco[-1] = 1
# Traffic autopilot settings
self.aspd[-1] = self.cas[-1]
self.aptas[-1] = self.tas[-1]
self.apalt[-1] = self.alt[-1]
# Display information on label
self.label[-1] = ['', '', '', 0]
# Miscallaneous
self.coslat[-1] = cos(radians(aclat)) # Cosine of latitude for flat-earth aproximations
self.eps[-1] = 0.01
# ----- Submodules of Traffic -----
self.ap.create()
self.actwp.create()
self.pilot.create()
self.adsb.create()
self.area.create()
self.asas.create()
self.perf.create()
self.trails.create()
return True
def creconfs(self, acid, actype, targetidx, dpsi, cpa, tlosh, dH=None, tlosv=None, spd=None):
latref = self.lat[targetidx] # deg
lonref = self.lon[targetidx] # deg
altref = self.alt[targetidx] # m
trkref = radians(self.trk[targetidx])
gsref = self.gs[targetidx] # m/s
vsref = self.vs[targetidx] # m/s
cpa = cpa * nm
pzr = settings.asas_pzr * nm
pzh = settings.asas_pzh * ft
trk = trkref + radians(dpsi)
gs = gsref if spd is None else spd
if dH is None:
acalt = altref
acvs = 0.0
else:
acalt = altref + dH
tlosv = tlosh if tlosv is None else tlosv
acvs = vsref - np.sign(dH) * (abs(dH) - pzh) / tlosv
# Horizontal relative velocity vector
gsn, gse = gs * cos(trk), gs * sin(trk)
vreln, vrele = gsref * cos(trkref) - gsn, gsref * sin(trkref) - gse
# Relative velocity magnitude
vrel = sqrt(vreln * vreln + vrele * vrele)
# Relative travel distance to closest point of approach
drelcpa = tlosh * vrel + (0 if cpa > pzr else sqrt(pzr * pzr - cpa * cpa))
# Initial intruder distance
dist = sqrt(drelcpa * drelcpa + cpa * cpa)
# Rotation matrix diagonal and cross elements for distance vector
rd = drelcpa / dist
rx = cpa / dist
# Rotate relative velocity vector to obtain intruder bearing
brn = degrees(atan2(-rx * vreln + rd * vrele,
rd * vreln + rx * vrele))
# Calculate intruder lat/lon
aclat, aclon = geo.qdrpos(latref, lonref, brn, dist / nm)
# convert groundspeed to CAS, and track to heading
wn, we = self.wind.getdata(aclat, aclon, acalt)
tasn, tase = gsn - wn, gse - we
acspd = vtas2cas(sqrt(tasn * tasn + tase * tase), acalt)
achdg = degrees(atan2(tase, tasn))
# Create and, when necessary, set vertical speed
self.create(acid, actype, aclat, aclon, achdg, acalt, acspd)
self.ap.selalt(len(self.lat) - 1, altref, acvs)
self.vs[-1] = acvs
def delete(self, acid):
"""Delete an aircraft"""
# Look up index of aircraft
idx = self.id2idx(acid)
# Do nothing if not found
if idx < 0:
return False
# Decrease number of aircraft
self.ntraf = self.ntraf - 1
# Delete all aircraft parameters
super(Traffic, self).delete(idx)
# ----- Submodules of Traffic -----
self.perf.delete(idx)
self.area.delete(idx)
return True
def update(self, simt, simdt):
# Update only if there is traffic ---------------------
if self.ntraf == 0:
return
#---------- Atmosphere --------------------------------
self.p, self.rho, self.Temp = vatmos(self.alt)
#---------- ADSB Update -------------------------------
self.adsb.update(simt)
#---------- Fly the Aircraft --------------------------
self.ap.update(simt)
self.asas.update(simt)
self.pilot.FMSOrAsas()
#---------- Limit Speeds ------------------------------
self.pilot.FlightEnvelope()
#---------- Kinematics --------------------------------
self.UpdateAirSpeed(simdt, simt)
self.UpdateGroundSpeed(simdt)
self.UpdatePosition(simdt)
#---------- Performance Update ------------------------
self.perf.perf(simt)
#---------- Simulate Turbulence -----------------------
self.Turbulence.Woosh(simdt)
#---------- Aftermath ---------------------------------
self.trails.update(simt)
self.area.check(simt)
return
def UpdateAirSpeed(self, simdt, simt):
# Acceleration
self.delspd = self.pilot.spd - self.tas
swspdsel = np.abs(self.delspd) > 0.4 # <1 kts = 0.514444 m/s
ax = self.perf.acceleration(simdt)
# Update velocities
self.tas = self.tas + swspdsel * ax * np.sign(self.delspd) * simdt
self.cas = vtas2cas(self.tas, self.alt)
self.M = vtas2mach(self.tas, self.alt)
# Turning
turnrate = np.degrees(g0 * np.tan(self.bank) / np.maximum(self.tas, self.eps))
delhdg = (self.pilot.hdg - self.hdg + 180.) % 360 - 180. # [deg]
self.hdgsel = np.abs(delhdg) > np.abs(2. * simdt * turnrate)
# Update heading
self.hdg = (self.hdg + simdt * turnrate * self.hdgsel * np.sign(delhdg)) % 360.
# Update vertical speed
delalt = self.pilot.alt - self.alt
self.swaltsel = np.abs(delalt) > np.maximum(10 * ft, np.abs(2. * simdt * np.abs(self.vs)))
self.vs = self.swaltsel * np.sign(delalt) * np.abs(self.pilot.vs)
def UpdateGroundSpeed(self, simdt):
# Compute ground speed and track from heading, airspeed and wind
if self.wind.winddim == 0: # no wind
self.gsnorth = self.tas * np.cos(np.radians(self.hdg))
self.gseast = self.tas * np.sin(np.radians(self.hdg))
self.gs = self.tas
self.trk = self.hdg
else:
windnorth, windeast = self.wind.getdata(self.lat, self.lon, self.alt)
self.gsnorth = self.tas * np.cos(np.radians(self.hdg)) + windnorth
self.gseast = self.tas * np.sin(np.radians(self.hdg)) + windeast
self.gs = np.sqrt(self.gsnorth**2 + self.gseast**2)
self.trk = np.degrees(np.arctan2(self.gseast, self.gsnorth)) % 360.
def UpdatePosition(self, simdt):
# Update position
self.alt = np.where(self.swaltsel, self.alt + self.vs * simdt, self.pilot.alt)
self.lat = self.lat + np.degrees(simdt * self.gsnorth / Rearth)
self.coslat = np.cos(np.deg2rad(self.lat))
self.lon = self.lon + np.degrees(simdt * self.gseast / self.coslat / Rearth)
def id2idx(self, acid):
"""Find index of aircraft id"""
try:
return self.id.index(acid.upper())
except:
return -1
def setNoise(self, noise=None):
"""Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)"""
if noise is None:
return True, "Noise is currently " + ("on" if self.Turbulence.active else "off")
self.Turbulence.SetNoise(noise)
self.adsb.SetNoise(noise)
return True
def engchange(self, acid, engid):
"""Change of engines"""
self.perf.engchange(acid, engid)
return
def move(self, idx, lat, lon, alt=None, hdg=None, casmach=None, vspd=None):
self.lat[idx] = lat
self.lon[idx] = lon
if alt:
self.alt[idx] = alt
self.apalt[idx] = alt
if hdg:
self.hdg[idx] = hdg
self.ap.trk[idx] = hdg
if casmach:
self.tas[idx], self.aspd[-1], dummy = casormach(casmach, alt)
if vspd:
self.vs[idx] = vspd
self.swvnav[idx] = False
def nom(self, idx):
""" Reset acceleration back to nominal (1 kt/s^2): NOM acid """
self.ax[idx] = kts
def poscommand(self, idxorwp):# Show info on aircraft(int) or waypoint or airport (str)
"""POS command: Show info or an aircraft, airport, waypoint or navaid"""
# Aircraft index
if type(idxorwp)==int and idxorwp >= 0:
idx = idxorwp
acid = self.id[idx]
actype = self.type[idx]
latlon = latlon2txt(self.lat[idx], self.lon[idx])
alt = round(self.alt[idx] / ft)
hdg = round(self.hdg[idx])
trk = round(self.trk[idx])
cas = round(self.cas[idx] / kts)
tas = round(self.tas[idx] / kts)
gs = round(self.gs[idx]/kts)
M = self.M[idx]
VS = round(self.vs[idx]/ft*60.)
route = self.ap.route[idx]
# Position report
lines = "Info on %s %s index = %d\n" %(acid, actype, idx) \
+ "Pos: "+latlon+ "\n" \
+ "Hdg: %03d Trk: %03d\n" %(hdg, trk) \
+ "Alt: %d ft V/S: %d fpm\n" %(alt,VS) \
+ "CAS/TAS/GS: %d/%d/%d kts M: %.3f\n"%(cas,tas,gs,M)
# FMS AP modes
if self.swlnav[idx] and route.nwp > 0 and route.iactwp >= 0:
if self.swvnav[idx]:
lines = lines + "VNAV, "
lines += "LNAV to " + route.wpname[route.iactwp] + "\n"
# Flight info: Destination and origin
if self.ap.orig[idx] != "" or self.ap.dest[idx] != "":
lines = lines + "Flying"
if self.ap.orig[idx] != "":
lines = lines + " from " + self.ap.orig[idx]
if self.ap.dest[idx] != "":
lines = lines + " to " + self.ap.dest[idx]
# Show a/c info and highlight route of aircraft in radar window
# and pan to a/c (to show route)
return bs.scr.showacinfo(acid,lines)
# Waypoint: airport, navaid or fix
else:
wp = idxorwp.upper()
# Reference position for finding nearest
reflat = bs.scr.ctrlat
reflon = bs.scr.ctrlon
lines = "Info on "+wp+":\n"
# First try airports (most used and shorter, hence faster list)
iap = bs.navdb.getaptidx(wp)
if iap>=0:
aptypes = ["large","medium","small"]
lines = lines + bs.navdb.aptname[iap]+"\n" \
+ "is a "+ aptypes[max(-1,bs.navdb.aptype[iap]-1)] \
+" airport at:\n" \
+ latlon2txt(bs.navdb.aptlat[iap], \
bs.navdb.aptlon[iap]) + "\n" \
+ "Elevation: " \
+ str(int(round(bs.navdb.aptelev[iap]/ft))) \
+ " ft \n"
# Show country name
try:
ico = bs.navdb.cocode2.index(bs.navdb.aptco[iap].upper())
lines = lines + "in "+bs.navdb.coname[ico]+" ("+ \
bs.navdb.aptco[iap]+")"
except:
ico = -1
lines = lines + "Country code: "+bs.navdb.aptco[iap]
try:
rwytxt = str(bs.navdb.rwythresholds[bs.navdb.aptid[iap]].keys())
lines = lines + "\nRunways: " +rwytxt.strip("[]").replace("'","")
except:
pass
# Not found as airport, try waypoints & navaids
else:
iwps = bs.navdb.getwpindices(wp,reflat,reflon)
if iwps[0]>=0:
typetxt = ""
desctxt = ""
lastdesc = "XXXXXXXX"
for i in iwps:
# One line type text
if typetxt == "":
typetxt = typetxt+bs.navdb.wptype[i]
else:
typetxt = typetxt+" and "+bs.navdb.wptype[i]
# Description: multi-line
samedesc = bs.navdb.wpdesc[i]==lastdesc
if desctxt == "":
desctxt = desctxt +bs.navdb.wpdesc[i]
lastdesc = bs.navdb.wpdesc[i]
elif not samedesc:
desctxt = desctxt +"\n"+bs.navdb.wpdesc[i]
lastdesc = bs.navdb.wpdesc[i]
# Navaid: frequency
if bs.navdb.wptype[i] in ["VOR","DME","TACAN"] and not samedesc:
desctxt = desctxt + " "+ str(bs.navdb.wpfreq[i])+" MHz"
elif bs.navdb.wptype[i]=="NDB" and not samedesc:
desctxt = desctxt+ " " + str(bs.navdb.wpfreq[i])+" kHz"
iwp = iwps[0]
# Basic info
lines = lines + wp +" is a "+ typetxt \
+ " at\n"\
+ latlon2txt(bs.navdb.wplat[iwp], \
bs.navdb.wplon[iwp])
# Navaids have description
if len(desctxt)>0:
lines = lines+ "\n" + desctxt
# VOR give variation
if bs.navdb.wptype[iwp]=="VOR":
lines = lines + "\nVariation: "+ \
str(bs.navdb.wpvar[iwp])+" deg"
# How many others?
nother = bs.navdb.wpid.count(wp)-len(iwps)
if nother>0:
verb = ["is ","are "][min(1,max(0,nother-1))]
lines = lines +"\nThere "+verb + str(nother) +\
" other waypoint(s) also named " + wp
# In which airways?
connect = bs.navdb.listconnections(wp, \
bs.navdb.wplat[iwp],
bs.navdb.wplon[iwp])
if len(connect)>0:
awset = set([])
for c in connect:
awset.add(c[0])
lines = lines+"\nAirways: "+"-".join(awset)
# Try airway id
else: # airway
awid = wp
airway = bs.navdb.listairway(awid)
if len(airway)>0:
lines = ""
for segment in airway:
lines = lines+"Airway "+ awid + ": " + \
" - ".join(segment)+"\n"
lines = lines[:-1] # cut off final newline
else:
return False,idxorwp+" not found as a/c, airport, navaid or waypoint"
# Show what we found on airport and navaid/waypoint
bs.scr.echo(lines)
return True
def airwaycmd(self,key=""):
# Show conections of a waypoint
reflat = bs.scr.ctrlat
reflon = bs.scr.ctrlon
if key=="":
return False,'AIRWAY needs waypoint or airway'
if bs.navdb.awid.count(key)>0:
return self.poscommand(key.upper())
else:
# Find connecting airway legs
wpid = key.upper()
iwp = bs.navdb.getwpidx(wpid,reflat,reflon)
if iwp<0:
return False,key + " not found."
wplat = bs.navdb.wplat[iwp]
wplon = bs.navdb.wplon[iwp]
connect = bs.navdb.listconnections(key.upper(),wplat,wplon)
if len(connect)>0:
lines = ""
for c in connect:
if len(c)>=2:
# Add airway, direction, waypoint
lines = lines+ c[0]+": to "+c[1]+"\n"
return True, lines[:-1] # exclude final newline
else:
return False,"No airway legs found for ",key
#!/usr/bin/env python3
# Author: Kari Lavikka
import math
from ev3dev.ev3 import *
from time import sleep
from pilot import Pilot
if __name__ == "__main__":
print("Kukkuu")
pilot = Pilot(4.25, 14.2, LargeMotor("outA"), LargeMotor("outB"))
ir = InfraredSensor()
assert ir.connected, "Connect a single infrared sensor to any sensor port"
ir.mode = 'IR-PROX'
btn = Button()
wall_distance = ir.value()
print("Initial wall distance: " + str(wall_distance))
pilot.travel_indefinitely()
last_blocked = pilot.get_travelled_distance()
previous_print = -10000000
while True:
wall_distance = ir.value()
class Traffic(DynamicArrays):
"""
Traffic class definition : Traffic data
Methods:
Traffic() : constructor
reset() : Reset traffic database w.r.t a/c data
create(acid,actype,aclat,aclon,achdg,acalt,acspd) : create aircraft
delete(acid) : delete an aircraft from traffic data
deletall() : delete all traffic
update(sim) : do a numerical integration step
id2idx(name) : return index in traffic database of given call sign
engchange(i,engtype) : change engine type of an aircraft
setNoise(A) : Add turbulence
Members: see create
Created by : Jacco M. Hoekstra
"""
def __init__(self, navdb):
self.wind = WindSim()
# Define the periodic loggers
# ToDo: explain what these line sdo in comments (type of logs?)
datalog.definePeriodicLogger('SNAPLOG', 'SNAPLOG logfile.',
settings.snapdt)
datalog.definePeriodicLogger('INSTLOG', 'INSTLOG logfile.',
settings.instdt)
datalog.definePeriodicLogger('SKYLOG', 'SKYLOG logfile.',
settings.skydt)
with RegisterElementParameters(self):
# Register the following parameters for logging
with datalog.registerLogParameters('SNAPLOG', self):
# Aircraft Info
self.id = [] # identifier (string)
self.type = [] # aircaft type (string)
# Positions
self.lat = np.array([]) # latitude [deg]
self.lon = np.array([]) # longitude [deg]
self.alt = np.array([]) # altitude [m]
self.hdg = np.array([]) # traffic heading [deg]
self.trk = np.array([]) # track angle [deg]
# Velocities
self.tas = np.array([]) # true airspeed [m/s]
self.gs = np.array([]) # ground speed [m/s]
self.gsnorth = np.array([]) # ground speed [m/s]
self.gseast = np.array([]) # ground speed [m/s]
self.cas = np.array([]) # calibrated airspeed [m/s]
self.M = np.array([]) # mach number
self.vs = np.array([]) # vertical speed [m/s]
# Atmosphere
self.p = np.array([]) # air pressure [N/m2]
self.rho = np.array([]) # air density [kg/m3]
self.Temp = np.array([]) # air temperature [K]
self.dtemp = np.array([]) # delta t for non-ISA conditions
# Traffic autopilot settings
self.aspd = np.array([]) # selected spd(CAS) [m/s]
self.aptas = np.array([]) # just for initializing
self.ama = np.array(
[]) # selected spd above crossover altitude (Mach) [-]
self.apalt = np.array([]) # selected alt[m]
self.avs = np.array([]) # selected vertical speed [m/s]
# Whether to perform LNAV and VNAV
self.swlnav = np.array([], dtype=np.bool)
self.swvnav = np.array([], dtype=np.bool)
# Flight Models
self.asas = ASAS(self)
self.ap = Autopilot(self)
self.pilot = Pilot(self)
self.adsb = ADSB(self)
self.trails = Trails(self)
self.actwp = ActiveWaypoint(self)
# Traffic performance data
self.avsdef = np.array(
[]) # [m/s]default vertical speed of autopilot
self.aphi = np.array([]) # [rad] bank angle setting of autopilot
self.ax = np.array(
[]) # [m/s2] absolute value of longitudinal accelleration
self.bank = np.array([]) # nominal bank angle, [radian]
self.hdgsel = np.array(
[], dtype=np.bool) # determines whether aircraft is turning
# Crossover altitude
self.abco = np.array([])
self.belco = np.array([])
# limit settings
self.limspd = np.array([]) # limit speed
self.limspd_flag = np.array(
[], dtype=np.bool
) # flag for limit spd - we have to test for max and min
self.limalt = np.array([]) # limit altitude
self.limvs = np.array(
[]) # limit vertical speed due to thrust limitation
self.limvs_flag = np.array([])
# Display information on label
self.label = [] # Text and bitmap of traffic label
# Miscallaneous
self.coslat = np.array([]) # Cosine of latitude for computations
self.eps = np.array([]) # Small nonzero numbers
# Default bank angles per flight phase
self.bphase = np.deg2rad(np.array([15, 35, 35, 35, 15, 45]))
self.reset(navdb)
def reset(self, navdb):
# This ensures that the traffic arrays (which size is dynamic)
# are all reset as well, so all lat,lon,sdp etc but also objects adsb
super(Traffic, self).reset()
self.ntraf = 0
# Reset models
self.wind.clear()
# Build new modules for area and turbulence
self.area = Area(self)
self.Turbulence = Turbulence(self)
# Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)
self.setNoise(False)
# Import navigation data base
self.navdb = navdb
# Default: BlueSky internal performance model.
# Insert your BADA files to the folder "BlueSky/data/coefficients/BADA"
# for working with EUROCONTROL`s Base of Aircraft Data revision 3.12
self.perf = Perf(self)
def mcreate(self,
count,
actype=None,
alt=None,
spd=None,
dest=None,
area=None):
""" Create multiple random aircraft in a specified area """
idbase = chr(randint(65, 90)) + chr(randint(65, 90))
if actype is None:
actype = 'B744'
for i in xrange(count):
acid = idbase + '%05d' % i
aclat = random() * (area[1] - area[0]) + area[0]
aclon = random() * (area[3] - area[2]) + area[2]
achdg = float(randint(1, 360))
acalt = (randint(2000, 39000) * ft) if alt is None else alt
acspd = (randint(250, 450) * kts) if spd is None else spd
self.create(acid, actype, aclat, aclon, achdg, acalt, acspd)
def create(self,
acid=None,
actype="B744",
aclat=None,
aclon=None,
achdg=None,
acalt=None,
casmach=None):
"""Create an aircraft"""
# Check if not already exist
if self.id.count(acid.upper()) > 0:
return False, acid + " already exists." # already exists do nothing
# Catch missing acid, replace by a default
if acid == None or acid == "*":
acid = "KL204"
flno = 204
while self.id.count(acid) > 0:
flno = flno + 1
acid = "KL" + str(flno)
# Check for (other) missing arguments
if actype == None or aclat == None or aclon == None or achdg == None \
or acalt == None or casmach == None:
return False,"CRE: Missing one or more arguments:"\
"acid,actype,aclat,aclon,achdg,acalt,acspd"
super(Traffic, self).create()
# Increase number of aircraft
self.ntraf = self.ntraf + 1
# Aircraft Info
self.id[-1] = acid.upper()
self.type[-1] = actype
# Positions
self.lat[-1] = aclat
self.lon[-1] = aclon
self.alt[-1] = acalt
self.hdg[-1] = achdg
self.trk[-1] = achdg
# Velocities
self.tas[-1], self.cas[-1], self.M[-1] = casormach(casmach, acalt)
self.gs[-1] = self.tas[-1]
self.gsnorth[-1] = self.tas[-1] * cos(radians(self.hdg[-1]))
self.gseast[-1] = self.tas[-1] * sin(radians(self.hdg[-1]))
# Atmosphere
self.Temp[-1], self.rho[-1], self.p[-1] = vatmos(acalt)
# Wind
if self.wind.winddim > 0:
vnwnd, vewnd = self.wind.getdata(self.lat[-1], self.lon[-1],
self.alt[-1])
self.gsnorth[-1] = self.gsnorth[-1] + vnwnd
self.gseast[-1] = self.gseast[-1] + vewnd
self.trk[-1] = np.degrees(
np.arctan2(self.gseast[-1], self.gsnorth[-1]))
self.gs[-1] = np.sqrt(self.gsnorth[-1]**2 + self.gseast[-1]**2)
# Traffic performance data
#(temporarily default values)
self.avsdef[-1] = 1500. * fpm # default vertical speed of autopilot
self.aphi[-1] = radians(25.) # bank angle setting of autopilot
self.ax[-1] = kts # absolute value of longitudinal accelleration
self.bank[-1] = radians(25.)
# Crossover altitude
self.abco[
-1] = 0 # not necessary to overwrite 0 to 0, but leave for clarity
self.belco[-1] = 1
# Traffic autopilot settings
self.aspd[-1] = self.cas[-1]
self.aptas[-1] = self.tas[-1]
self.apalt[-1] = self.alt[-1]
# Display information on label
self.label[-1] = ['', '', '', 0]
# Miscallaneous
self.coslat[-1] = cos(
radians(aclat)) # Cosine of latitude for flat-earth aproximations
self.eps[-1] = 0.01
# ----- Submodules of Traffic -----
self.ap.create()
self.actwp.create()
self.pilot.create()
self.adsb.create()
self.area.create()
self.asas.create()
self.perf.create()
self.trails.create()
return True
def delete(self, acid):
"""Delete an aircraft"""
# Look up index of aircraft
idx = self.id2idx(acid)
# Do nothing if not found
if idx < 0:
return False
# Decrease number of aircraft
self.ntraf = self.ntraf - 1
# Delete all aircraft parameters
super(Traffic, self).delete(idx)
# ----- Submodules of Traffic -----
self.perf.delete(idx)
self.area.delete(idx)
return True
def update(self, simt, simdt):
# Update only if there is traffic ---------------------
if self.ntraf == 0:
return
#---------- Atmosphere --------------------------------
self.p, self.rho, self.Temp = vatmos(self.alt)
#---------- ADSB Update -------------------------------
self.adsb.update(simt)
#---------- Fly the Aircraft --------------------------
self.ap.update(simt)
self.asas.update(simt)
self.pilot.FMSOrAsas()
#---------- Limit Speeds ------------------------------
self.pilot.FlightEnvelope()
#---------- Kinematics --------------------------------
self.UpdateAirSpeed(simdt, simt)
self.UpdateGroundSpeed(simdt)
self.UpdatePosition(simdt)
#---------- Performance Update ------------------------
self.perf.perf(simt)
#---------- Simulate Turbulence -----------------------
self.Turbulence.Woosh(simdt)
#---------- Aftermath ---------------------------------
self.trails.update(simt)
self.area.check(simt)
return
def UpdateAirSpeed(self, simdt, simt):
# Acceleration
self.delspd = self.pilot.spd - self.tas
swspdsel = np.abs(self.delspd) > 0.4 # <1 kts = 0.514444 m/s
ax = self.perf.acceleration(simdt)
# Update velocities
self.tas = self.tas + swspdsel * ax * np.sign(self.delspd) * simdt
self.cas = vtas2cas(self.tas, self.alt)
self.M = vtas2mach(self.tas, self.alt)
# Turning
turnrate = np.degrees(g0 * np.tan(self.bank) /
np.maximum(self.tas, self.eps))
delhdg = (self.pilot.hdg - self.hdg + 180.) % 360 - 180. # [deg]
self.hdgsel = np.abs(delhdg) > np.abs(2. * simdt * turnrate)
# Update heading
self.hdg = (self.hdg +
simdt * turnrate * self.hdgsel * np.sign(delhdg)) % 360.
# Update vertical speed
delalt = self.pilot.alt - self.alt
self.swaltsel = np.abs(delalt) > np.maximum(
10 * ft, np.abs(2 * simdt * np.abs(self.vs)))
self.vs = self.swaltsel * np.sign(delalt) * self.pilot.vs
def UpdateGroundSpeed(self, simdt):
# Compute ground speed and track from heading, airspeed and wind
if self.wind.winddim == 0: # no wind
self.gsnorth = self.tas * np.cos(np.radians(self.hdg))
self.gseast = self.tas * np.sin(np.radians(self.hdg))
self.gs = self.tas
self.trk = self.hdg
else:
windnorth, windeast = self.wind.getdata(self.lat, self.lon,
self.alt)
self.gsnorth = self.tas * np.cos(np.radians(self.hdg)) + windnorth
self.gseast = self.tas * np.sin(np.radians(self.hdg)) + windeast
self.gs = np.sqrt(self.gsnorth**2 + self.gseast**2)
self.trk = np.degrees(np.arctan2(self.gseast, self.gsnorth)) % 360.
def UpdatePosition(self, simdt):
# Update position
self.alt = np.where(self.swaltsel, self.alt + self.vs * simdt,
self.pilot.alt)
self.lat = self.lat + np.degrees(simdt * self.gsnorth / Rearth)
self.coslat = np.cos(np.deg2rad(self.lat))
self.lon = self.lon + np.degrees(
simdt * self.gseast / self.coslat / Rearth)
def id2idx(self, acid):
"""Find index of aircraft id"""
try:
return self.id.index(acid.upper())
except:
return -1
def setNoise(self, noise=None):
"""Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)"""
if noise is None:
return True, "Noise is currently " + (
"on" if self.Turbulence.active else "off")
self.Turbulence.SetNoise(noise)
self.adsb.SetNoise(noise)
return True
def engchange(self, acid, engid):
"""Change of engines"""
self.perf.engchange(acid, engid)
return
def move(self, idx, lat, lon, alt=None, hdg=None, casmach=None, vspd=None):
self.lat[idx] = lat
self.lon[idx] = lon
if alt:
self.alt[idx] = alt
self.apalt[idx] = alt
if hdg:
self.hdg[idx] = hdg
self.ap.trk[idx] = hdg
if casmach:
self.tas[idx], self.aspd[-1], dummy = casormach(casmach, alt)
if vspd:
self.vs[idx] = vspd
self.swvnav[idx] = False
def nom(self, idx):
""" Reset acceleration back to nominal (1 kt/s^2): NOM acid """
self.ax[idx] = kts
def acinfo(self, idx):
acid = self.id[idx]
actype = self.type[idx]
lat, lon = self.lat[idx], self.lon[idx]
alt, hdg, trk = self.alt[idx] / ft, self.hdg[idx], round(self.trk[idx])
cas = self.cas[idx] / kts
tas = self.tas[idx] / kts
route = self.ap.route[idx]
line = "Info on %s %s index = %d\n" % (acid, actype, idx) \
+ "Pos = %.2f, %.2f. Spd: %d kts CAS, %d kts TAS\n" % (lat, lon, cas, tas) \
+ "Alt = %d ft, Hdg = %d, Trk = %d\n" % (alt, hdg, trk)
if self.swlnav[idx] and route.nwp > 0 and route.iactwp >= 0:
if self.swvnav[idx]:
line += "VNAV, "
line += "LNAV to " + route.wpname[route.iactwp] + "\n"
if self.ap.orig[idx] != "" or self.ap.dest[idx] != "":
line += "Flying"
if self.ap.orig[idx] != "":
line += " from " + self.ap.orig[idx]
if self.ap.dest[idx] != "":
line += " to " + self.ap.dest[idx]
return acid, line
def __init__(self, navdb):
self.wind = WindSim()
# Define the periodic loggers
# ToDo: explain what these line sdo in comments (type of logs?)
datalog.definePeriodicLogger('SNAPLOG', 'SNAPLOG logfile.',
settings.snapdt)
datalog.definePeriodicLogger('INSTLOG', 'INSTLOG logfile.',
settings.instdt)
datalog.definePeriodicLogger('SKYLOG', 'SKYLOG logfile.',
settings.skydt)
with RegisterElementParameters(self):
# Register the following parameters for logging
with datalog.registerLogParameters('SNAPLOG', self):
# Aircraft Info
self.id = [] # identifier (string)
self.type = [] # aircaft type (string)
# Positions
self.lat = np.array([]) # latitude [deg]
self.lon = np.array([]) # longitude [deg]
self.alt = np.array([]) # altitude [m]
self.hdg = np.array([]) # traffic heading [deg]
self.trk = np.array([]) # track angle [deg]
# Velocities
self.tas = np.array([]) # true airspeed [m/s]
self.gs = np.array([]) # ground speed [m/s]
self.gsnorth = np.array([]) # ground speed [m/s]
self.gseast = np.array([]) # ground speed [m/s]
self.cas = np.array([]) # calibrated airspeed [m/s]
self.M = np.array([]) # mach number
self.vs = np.array([]) # vertical speed [m/s]
# Atmosphere
self.p = np.array([]) # air pressure [N/m2]
self.rho = np.array([]) # air density [kg/m3]
self.Temp = np.array([]) # air temperature [K]
self.dtemp = np.array([]) # delta t for non-ISA conditions
# Traffic autopilot settings
self.aspd = np.array([]) # selected spd(CAS) [m/s]
self.aptas = np.array([]) # just for initializing
self.ama = np.array(
[]) # selected spd above crossover altitude (Mach) [-]
self.apalt = np.array([]) # selected alt[m]
self.avs = np.array([]) # selected vertical speed [m/s]
# Whether to perform LNAV and VNAV
self.swlnav = np.array([], dtype=np.bool)
self.swvnav = np.array([], dtype=np.bool)
# Flight Models
self.asas = ASAS(self)
self.ap = Autopilot(self)
self.pilot = Pilot(self)
self.adsb = ADSB(self)
self.trails = Trails(self)
self.actwp = ActiveWaypoint(self)
# Traffic performance data
self.avsdef = np.array(
[]) # [m/s]default vertical speed of autopilot
self.aphi = np.array([]) # [rad] bank angle setting of autopilot
self.ax = np.array(
[]) # [m/s2] absolute value of longitudinal accelleration
self.bank = np.array([]) # nominal bank angle, [radian]
self.hdgsel = np.array(
[], dtype=np.bool) # determines whether aircraft is turning
# Crossover altitude
self.abco = np.array([])
self.belco = np.array([])
# limit settings
self.limspd = np.array([]) # limit speed
self.limspd_flag = np.array(
[], dtype=np.bool
) # flag for limit spd - we have to test for max and min
self.limalt = np.array([]) # limit altitude
self.limvs = np.array(
[]) # limit vertical speed due to thrust limitation
self.limvs_flag = np.array([])
# Display information on label
self.label = [] # Text and bitmap of traffic label
# Miscallaneous
self.coslat = np.array([]) # Cosine of latitude for computations
self.eps = np.array([]) # Small nonzero numbers
# Default bank angles per flight phase
self.bphase = np.deg2rad(np.array([15, 35, 35, 35, 15, 45]))
self.reset(navdb)
def execute(self):
if self.process and self.execute:
pilot = Pilot(self.device.base_url())
pilot.inject(self.process, self.command)
else:
raise JobExecutionError("Process or command missing")
class Traffic(DynamicArrays):
"""
Traffic class definition : Traffic data
Methods:
Traffic() : constructor
reset() : Reset traffic database w.r.t a/c data
create(acid,actype,aclat,aclon,achdg,acalt,acspd) : create aircraft
delete(acid) : delete an aircraft from traffic data
deletall() : delete all traffic
update(sim) : do a numerical integration step
id2idx(name) : return index in traffic database of given call sign
engchange(i,engtype) : change engine type of an aircraft
setNoise(A) : Add turbulence
Members: see create
Created by : Jacco M. Hoekstra
"""
def __init__(self, navdb):
self.wind = WindSim()
# Define the periodic loggers
# ToDo: explain what these line sdo in comments (type of logs?)
datalog.definePeriodicLogger('SNAPLOG', 'SNAPLOG logfile.', settings.snapdt)
datalog.definePeriodicLogger('INSTLOG', 'INSTLOG logfile.', settings.instdt)
datalog.definePeriodicLogger('SKYLOG', 'SKYLOG logfile.', settings.skydt)
with RegisterElementParameters(self):
# Register the following parameters for logging
with datalog.registerLogParameters('SNAPLOG', self):
# Aircraft Info
self.id = [] # identifier (string)
self.type = [] # aircaft type (string)
# Positions
self.lat = np.array([]) # latitude [deg]
self.lon = np.array([]) # longitude [deg]
self.alt = np.array([]) # altitude [m]
self.hdg = np.array([]) # traffic heading [deg]
self.trk = np.array([]) # track angle [deg]
# Velocities
self.tas = np.array([]) # true airspeed [m/s]
self.gs = np.array([]) # ground speed [m/s]
self.gsnorth = np.array([]) # ground speed [m/s]
self.gseast = np.array([]) # ground speed [m/s]
self.cas = np.array([]) # calibrated airspeed [m/s]
self.M = np.array([]) # mach number
self.vs = np.array([]) # vertical speed [m/s]
# Atmosphere
self.p = np.array([]) # air pressure [N/m2]
self.rho = np.array([]) # air density [kg/m3]
self.Temp = np.array([]) # air temperature [K]
self.dtemp = np.array([]) # delta t for non-ISA conditions
# Traffic autopilot settings
self.aspd = np.array([]) # selected spd(CAS) [m/s]
self.aptas = np.array([]) # just for initializing
self.ama = np.array([]) # selected spd above crossover altitude (Mach) [-]
self.apalt = np.array([]) # selected alt[m]
self.avs = np.array([]) # selected vertical speed [m/s]
# Speed offset
self.spdoffset = np.array([]) # speed offset [kts]
self.spdoffsetdev = np.array([]) # speed offset deviation [kts]
self.spd_onoff = np.array([]) # speed offset on/off
# Whether to perform LNAV and VNAV
self.swlnav = np.array([], dtype=np.bool)
self.swvnav = np.array([], dtype=np.bool)
# Flight Models
self.asas = ASAS(self)
self.ap = Autopilot(self)
self.pilot = Pilot(self)
self.adsb = ADSB(self)
self.trails = Trails(self)
self.actwp = ActiveWaypoint(self)
# Traffic performance data
self.avsdef = np.array([]) # [m/s]default vertical speed of autopilot
self.aphi = np.array([]) # [rad] bank angle setting of autopilot
self.ax = np.array([]) # [m/s2] absolute value of longitudinal accelleration
self.bank = np.array([]) # nominal bank angle, [radian]
self.hdgsel = np.array([], dtype=np.bool) # determines whether aircraft is turning
# Crossover altitude
self.abco = np.array([])
self.belco = np.array([])
# limit settings
self.limspd = np.array([]) # limit speed
self.limspd_flag = np.array([], dtype=np.bool) # flag for limit spd - we have to test for max and min
self.limalt = np.array([]) # limit altitude
self.limvs = np.array([]) # limit vertical speed due to thrust limitation
self.limvs_flag = np.array([])
# Display information on label
self.label = [] # Text and bitmap of traffic label
# Miscallaneous
self.coslat = np.array([]) # Cosine of latitude for computations
self.eps = np.array([]) # Small nonzero numbers
# Default bank angles per flight phase
self.bphase = np.deg2rad(np.array([15, 35, 35, 35, 15, 45]))
self.reset(navdb)
def reset(self, navdb):
# This ensures that the traffic arrays (which size is dynamic)
# are all reset as well, so all lat,lon,sdp etc but also objects adsb
super(Traffic, self).reset()
self.ntraf = 0
# Reset models
self.wind.clear()
# Build new modules for area and turbulence
self.area = Area(self)
self.Turbulence = Turbulence(self)
# Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)
self.setNoise(False)
# Import navigation data base
self.navdb = navdb
# Default: BlueSky internal performance model.
# Insert your BADA files to the folder "BlueSky/data/coefficients/BADA"
# for working with EUROCONTROL`s Base of Aircraft Data revision 3.12
self.perf = Perf(self)
self.AMAN=AMAN(AllFlights,unique_runways)
for rwy in unique_runways:
self.AMAN.initial_schedule_popupflightsonly(intarrtime_AMAN_runway,rwy,simulation_start,unique_runways)
def mcreate(self, count, actype=None, alt=None, spd=None, dest=None, area=None):
""" Create multiple random aircraft in a specified area """
idbase = chr(randint(65, 90)) + chr(randint(65, 90))
if actype is None:
actype = 'B744'
for i in xrange(count):
acid = idbase + '%05d' % i
aclat = random() * (area[1] - area[0]) + area[0]
aclon = random() * (area[3] - area[2]) + area[2]
achdg = float(randint(1, 360))
acalt = (randint(2000, 39000) * ft) if alt is None else alt
acspd = (randint(250, 450) * kts) if spd is None else spd
self.create(acid, actype, aclat, aclon, achdg, acalt, acspd)
def create(self, acid=None, actype="B744", aclat=None, aclon=None, achdg=None, acalt=None, casmach=None):
"""Create an aircraft"""
# Check if not already exist
if self.id.count(acid.upper()) > 0:
return False, acid + " already exists." # already exists do nothing
# Catch missing acid, replace by a default
if acid == None or acid =="*":
acid = "KL204"
flno = 204
while self.id.count(acid)>0:
flno = flno+1
acid ="KL"+str(flno)
# Check for (other) missing arguments
if actype == None or aclat == None or aclon == None or achdg == None \
or acalt == None or casmach == None:
return False,"CRE: Missing one or more arguments:"\
"acid,actype,aclat,aclon,achdg,acalt,acspd"
super(Traffic, self).create()
# Increase number of aircraft
self.ntraf = self.ntraf + 1
# Aircraft Info
self.id[-1] = acid.upper()
self.type[-1] = actype
# Positions
self.lat[-1] = aclat
self.lon[-1] = aclon
self.alt[-1] = acalt
self.hdg[-1] = achdg
self.trk[-1] = achdg
# Velocities
self.tas[-1], self.cas[-1], self.M[-1] = casormach(casmach, acalt)
self.gs[-1] = self.tas[-1]
self.gsnorth[-1] = self.tas[-1] * cos(radians(self.hdg[-1]))
self.gseast[-1] = self.tas[-1] * sin(radians(self.hdg[-1]))
# Speed offset
self.spd_onoff[-1] = 0. # Speed offset = 0 (off) on default
# Atmosphere
self.Temp[-1], self.rho[-1], self.p[-1] = vatmos(acalt)
# Wind
if self.wind.winddim > 0:
vnwnd, vewnd = self.wind.getdata(self.lat[-1], self.lon[-1], self.alt[-1])
self.gsnorth[-1] = self.gsnorth[-1] + vnwnd
self.gseast[-1] = self.gseast[-1] + vewnd
self.trk[-1] = np.degrees(np.arctan2(self.gseast[-1], self.gsnorth[-1]))
self.gs[-1] = np.sqrt(self.gsnorth[-1]**2 + self.gseast[-1]**2)
# Traffic performance data
#(temporarily default values)
self.avsdef[-1] = 1500. * fpm # default vertical speed of autopilot
self.aphi[-1] = radians(25.) # bank angle setting of autopilot
self.ax[-1] = kts # absolute value of longitudinal accelleration
self.bank[-1] = radians(25.)
# Crossover altitude
self.abco[-1] = 0 # not necessary to overwrite 0 to 0, but leave for clarity
self.belco[-1] = 1
# Traffic autopilot settings
self.aspd[-1] = self.cas[-1]
self.aptas[-1] = self.tas[-1]
self.apalt[-1] = self.alt[-1]
# Display information on label
self.label[-1] = ['', '', '', 0]
# Miscallaneous
self.coslat[-1] = cos(radians(aclat)) # Cosine of latitude for flat-earth aproximations
self.eps[-1] = 0.01
# ----- Submodules of Traffic -----
self.ap.create()
self.actwp.create()
self.pilot.create()
self.adsb.create()
self.area.create()
self.asas.create()
self.perf.create()
self.trails.create()
return True
def delete(self, acid):
"""Delete an aircraft"""
# Look up index of aircraft
idx = self.id2idx(acid)
# Do nothing if not found
if idx < 0:
return False
# Decrease number of aircraft
self.ntraf = self.ntraf - 1
# Delete all aircraft parameters
super(Traffic, self).delete(idx)
# ----- Submodules of Traffic -----
self.perf.delete(idx)
self.area.delete(idx)
return True
def update(self, simt, simdt):
# Update only if there is traffic ---------------------
if self.ntraf == 0:
return
#---------- Atmosphere --------------------------------
self.p, self.rho, self.Temp = vatmos(self.alt)
#---------- ADSB Update -------------------------------
self.adsb.update(simt)
#---------- Fly the Aircraft --------------------------
self.ap.update(simt)
self.asas.update(simt)
self.pilot.FMSOrAsas()
#---------- Limit Speeds ------------------------------
self.pilot.FlightEnvelope()
#---------- Kinematics --------------------------------
self.UpdateAirSpeed(simdt, simt)
self.UpdateGroundSpeed(simdt)
self.UpdatePosition(simdt)
#---------- Performance Update ------------------------
self.perf.perf(simt)
#---------- Simulate Turbulence -----------------------
self.Turbulence.Woosh(simdt)
#---------- Aftermath ---------------------------------
self.trails.update(simt)
self.area.check(simt)
#---------- AMAN --------------------------------------
i=0
while(i<self.ntraf):
temp1, temp2 = qdrdist(self.lat[i], self.lon[i], 52.309, 4.764) # Check distance towards EHAM
if temp2<2. and self.alt[i]<1.: # If aircraft within 2 nm from airport and below 1 meter, delete it
self.delete(self.id[i])
i=i+1
# Calculate energy cost per flight
self.AMAN.calculate_energy_cost(self.id,self.vs,self.tas,CD_0,CD_2,self.rho,WingSurface,self.hdg,self.trk,mass_nominal,simdt)
# Increase iteration counter
self.AMAN.IterationCounter=self.AMAN.IterationCounter+1
# Update trajectory predictor, scheduler and SARA every five seconds
if self.AMAN.IterationCounter%2==0:
# Check for speed offset
self.AMAN.speed_offset_switch(self.id,AMAN_horizon,self.spd_onoff)
# Update Trajectory Predictions
self.AMAN.update_TP(self.id,self.lat,self.lon,self.tas/kts,self.actwp.lat,self.actwp.lon,simt)
# Update schedule per runway
for rwy in unique_runways:
if '--node' in sys.argv:
if var_TP==str('ASAPBASIC'):
self.AMAN.scheduler_ASAP_basic(self.id,Take_into_account_schedule_horizon,rwy,intarrtime_AMAN_runway,Freeze_horizon,unique_runways)
elif var_TP==str('DYNAMIC'):
self.AMAN.scheduler_dynamic(self.id,Take_into_account_schedule_horizon,rwy,intarrtime_AMAN_runway)
elif var_TP==str('ASAPUPGRADE'):
self.AMAN.scheduler_ASAP_upgrade(self.id,Take_into_account_schedule_horizon,rwy,intarrtime_AMAN_runway,Freeze_horizon,unique_runways)
else:
self.AMAN.scheduler_ASAP_basic(self.id,Take_into_account_schedule_horizon,rwy,intarrtime_AMAN_runway,Freeze_horizon,unique_runways)
# #self.AMAN.scheduler_dynamic(self.id,Take_into_account_schedule_horizon,rwy,intarrtime_AMAN_runway)
# #self.AMAN.scheduler_ASAP_upgrade(self.id,Take_into_account_schedule_horizon,rwy,intarrtime_AMAN_runway,Freeze_horizon,unique_runways)
# Update SARA advisories
self.AMAN.update_SARA(self.id,self.alt,self.ap.route,SARA_horizon,simt,approach_margin)
# Save variables in logger
self.AMAN.AMAN_LOG_arrtimes_and_energycost(self.id,simulation_start)
self.AMAN.AMAN_LOG_STAhistory(self.id,simt)
self.AMAN.AMAN_LOG_lowleveldelay(self.id,simt)
self.AMAN.AMAN_LOG_seqhistory(self.id)
self.AMAN.AMAN_LOG_CBAShistory(self.id,simulation_start)
#self.AMAN.AMAN_LOG_ETA_CBAShistory(self.id,simt)
self.AMAN.AMAN_LOG_traffic_bunches(self.id,approach_margin,simt)
return
def UpdateAirSpeed(self, simdt, simt):
# Acceleration
self.delspd = self.pilot.spd - self.tas
swspdsel = np.abs(self.delspd) > 0.4 # <1 kts = 0.514444 m/s
ax = self.perf.acceleration(simdt)
# Update velocities
self.tas = self.tas + swspdsel * ax * np.sign(self.delspd) * simdt
self.cas = vtas2cas(self.tas, self.alt)
self.M = vtas2mach(self.tas, self.alt)
# Turning
turnrate = np.degrees(g0 * np.tan(self.bank) / np.maximum(self.tas, self.eps))
delhdg = (self.pilot.hdg - self.hdg + 180.) % 360 - 180. # [deg]
self.hdgsel = np.abs(delhdg) > np.abs(2. * simdt * turnrate)
# Update heading
self.hdg = (self.hdg + simdt * turnrate * self.hdgsel * np.sign(delhdg)) % 360.
# Update vertical speed
delalt = self.pilot.alt - self.alt
self.swaltsel = np.abs(delalt) > np.maximum(10 * ft, np.abs(2 * simdt * np.abs(self.vs)))
self.vs = self.swaltsel * np.sign(delalt) * self.pilot.vs
def UpdateGroundSpeed(self, simdt):
# Compute ground speed and track from heading, airspeed and wind
spd_random = self.spdoffset * kts + np.random.randn(self.ntraf) * self.spdoffsetdev * kts
#self.spd_onoff = array([0 0 0 0 0 0 1 0 0 0 etc.]) # Array whether speed offset + randomness is on/off per aircraft
tasx = self.tas + spd_random * self.spd_onoff
#print(tasx)
#tasx = self.tas + self.spdoffset * kts # np.random.randn(self.ntraf) * self.spdoffsetdev * kts
if self.wind.winddim == 0: # no wind
self.gsnorth = tasx * np.cos(np.radians(self.hdg))
self.gseast = tasx * np.sin(np.radians(self.hdg))
self.gs = self.tas
self.trk = self.hdg
else:
windnorth, windeast = self.wind.getdata(self.lat, self.lon, self.alt)
self.gsnorth = tasx * np.cos(np.radians(self.hdg)) + windnorth
self.gseast = tasx * np.sin(np.radians(self.hdg)) + windeast
self.gs = np.sqrt(self.gsnorth**2 + self.gseast**2)
self.trk = np.degrees(np.arctan2(self.gseast, self.gsnorth)) % 360.
def UpdatePosition(self, simdt):
# Update position
self.alt = np.where(self.swaltsel, self.alt + self.vs * simdt, self.pilot.alt)
self.lat = self.lat + np.degrees(simdt * self.gsnorth / Rearth)
self.coslat = np.cos(np.deg2rad(self.lat))
self.lon = self.lon + np.degrees(simdt * self.gseast / self.coslat / Rearth)
def id2idx(self, acid):
"""Find index of aircraft id"""
try:
return self.id.index(acid.upper())
except:
return -1
def setNoise(self, noise=None):
"""Noise (turbulence, ADBS-transmission noise, ADSB-truncated effect)"""
if noise is None:
return True, "Noise is currently " + ("on" if self.Turbulence.active else "off")
self.Turbulence.SetNoise(noise)
self.adsb.SetNoise(noise)
return True
def engchange(self, acid, engid):
"""Change of engines"""
self.perf.engchange(acid, engid)
return
def move(self, idx, lat, lon, alt=None, hdg=None, casmach=None, vspd=None):
self.lat[idx] = lat
self.lon[idx] = lon
if alt:
self.alt[idx] = alt
self.apalt[idx] = alt
if hdg:
self.hdg[idx] = hdg
self.ap.trk[idx] = hdg
if casmach:
self.tas[idx], self.aspd[-1], dummy = casormach(casmach, alt)
if vspd:
self.vs[idx] = vspd
self.swvnav[idx] = False
def nom(self, idx):
""" Reset acceleration back to nominal (1 kt/s^2): NOM acid """
self.ax[idx] = kts
def setdeltaspeed(self, idx, offset=0.):
self.spdoffset[idx] = offset
def setdeltaspeeddev(self, idx, offset=0.):
self.spdoffsetdev[idx] = offset
def poscommand(self, scr, idxorwp):# Show info on aircraft(int) or waypoint or airport (str)
"""POS command: Show info or an aircraft, airport, waypoint or navaid"""
# Aircraft index
if type(idxorwp)==int and idxorwp >= 0:
idx = idxorwp
acid = self.id[idx]
actype = self.type[idx]
lat, lon = self.lat[idx], self.lon[idx]
alt, hdg, trk = self.alt[idx] / ft, self.hdg[idx], round(self.trk[idx])
cas = self.cas[idx] / kts
tas = self.tas[idx] / kts
route = self.ap.route[idx]
# Position report
lines = "Info on %s %s index = %d\n" % (acid, actype, idx) \
+ "Pos = %.2f, %.2f. Spd: %d kts CAS, %d kts TAS\n" % (lat, lon, cas, tas) \
+ "Alt = %d ft, Hdg = %d, Trk = %d\n" % (alt, hdg, trk)
# FMS AP modes
if self.swlnav[idx] and route.nwp > 0 and route.iactwp >= 0:
if self.swvnav[idx]:
lines = lines + "VNAV, "
lines += "LNAV to " + route.wpname[route.iactwp] + "\n"
# Flight info: Destination and origin
if self.ap.orig[idx] != "" or self.ap.dest[idx] != "":
lines = lines + "Flying"
if self.ap.orig[idx] != "":
lines = lines + " from " + self.ap.orig[idx]
if self.ap.dest[idx] != "":
lines = lines + " to " + self.ap.dest[idx]
# Show a/c info and highlight route of aircraft in radar window
# and pan to a/c (to show route)
return scr.showacinfo(acid,lines)
# Waypoint: airport, navaid or fix
else:
wp = idxorwp.upper()
# Reference position for finding nearest
reflat = scr.ctrlat
reflon = scr.ctrlon
lines = "Info on "+wp+":\n"
# First try airports (most used and shorter, hence faster list)
iap = self.navdb.getaptidx(wp)
if iap>=0:
aptypes = ["large","medium","small"]
lines = lines + self.navdb.aptname[iap]+"\n" \
+ "is a "+ aptypes[max(-1,self.navdb.aptype[iap]-1)] \
+" airport at:\n" \
+ latlon2txt(self.navdb.aptlat[iap], \
self.navdb.aptlon[iap]) + "\n" \
+ "Elevation: " \
+ str(int(round(self.navdb.aptelev[iap]/ft))) \
+ " ft \n"
# Show country name
try:
ico = self.navdb.cocode2.index(self.navdb.aptco[iap].upper())
lines = lines + "in "+self.navdb.coname[ico]+" ("+ \
self.navdb.aptco[iap]+")"
except:
ico = -1
lines = lines + "Country code: "+self.navdb.aptco[iap]
try:
rwytxt = str(self.navdb.rwythresholds[self.navdb.aptid[iap]].keys())
lines = lines + "\nRunways: " +rwytxt.strip("[]").replace("'","")
except:
pass
# Not found as airport, try waypoints & navaids
else:
iwps = self.navdb.getwpindices(wp,reflat,reflon)
if iwps[0]>=0:
typetxt = ""
desctxt = ""
lastdesc = "XXXXXXXX"
for i in iwps:
# One line type text
if typetxt == "":
typetxt = typetxt+self.navdb.wptype[i]
else:
typetxt = typetxt+" and "+self.navdb.wptype[i]
# Description: multi-line
samedesc = self.navdb.wpdesc[i]==lastdesc
if desctxt == "":
desctxt = desctxt +self.navdb.wpdesc[i]
lastdesc = self.navdb.wpdesc[i]
elif not samedesc:
desctxt = desctxt +"\n"+self.navdb.wpdesc[i]
lastdesc = self.navdb.wpdesc[i]
# Navaid: frequency
if self.navdb.wptype[i] in ["VOR","DME","TACAN"] and not samedesc:
desctxt = desctxt + " "+ str(self.navdb.wpfreq[i])+" MHz"
elif self.navdb.wptype[i]=="NDB" and not samedesc:
desctxt = desctxt+ " " + str(self.navdb.wpfreq[i])+" kHz"
iwp = iwps[0]
# Basic info
lines = lines + wp +" is a "+ typetxt \
+ " at\n"\
+ latlon2txt(self.navdb.wplat[iwp], \
self.navdb.wplon[iwp])
# Navaids have description
if len(desctxt)>0:
lines = lines+ "\n" + desctxt
# VOR give variation
if self.navdb.wptype[iwp]=="VOR":
lines = lines + "\nVariation: "+ \
str(self.navdb.wpvar[iwp])+" deg"
# How many others?
nother = self.navdb.wpid.count(wp)-len(iwps)
if nother>0:
verb = ["is ","are "][min(1,max(0,nother-1))]
lines = lines +"\nThere "+verb + str(nother) +\
" other waypoint(s) also named " + wp
else:
return False,idxorwp+" not found as a/c, airport, navaid or waypoint"
# Show what we found on airport and navaid/waypoint
scr.echo(lines)
return True
def __init__(self, navdb):
self.wind = WindSim()
# Define the periodic loggers
# ToDo: explain what these line sdo in comments (type of logs?)
datalog.definePeriodicLogger('SNAPLOG', 'SNAPLOG logfile.', settings.snapdt)
datalog.definePeriodicLogger('INSTLOG', 'INSTLOG logfile.', settings.instdt)
datalog.definePeriodicLogger('SKYLOG', 'SKYLOG logfile.', settings.skydt)
with RegisterElementParameters(self):
# Register the following parameters for logging
with datalog.registerLogParameters('SNAPLOG', self):
# Aircraft Info
self.id = [] # identifier (string)
self.type = [] # aircaft type (string)
# Positions
self.lat = np.array([]) # latitude [deg]
self.lon = np.array([]) # longitude [deg]
self.alt = np.array([]) # altitude [m]
self.hdg = np.array([]) # traffic heading [deg]
self.trk = np.array([]) # track angle [deg]
# Velocities
self.tas = np.array([]) # true airspeed [m/s]
self.gs = np.array([]) # ground speed [m/s]
self.gsnorth = np.array([]) # ground speed [m/s]
self.gseast = np.array([]) # ground speed [m/s]
self.cas = np.array([]) # calibrated airspeed [m/s]
self.M = np.array([]) # mach number
self.vs = np.array([]) # vertical speed [m/s]
# Atmosphere
self.p = np.array([]) # air pressure [N/m2]
self.rho = np.array([]) # air density [kg/m3]
self.Temp = np.array([]) # air temperature [K]
self.dtemp = np.array([]) # delta t for non-ISA conditions
# Traffic autopilot settings
self.aspd = np.array([]) # selected spd(CAS) [m/s]
self.aptas = np.array([]) # just for initializing
self.ama = np.array([]) # selected spd above crossover altitude (Mach) [-]
self.apalt = np.array([]) # selected alt[m]
self.avs = np.array([]) # selected vertical speed [m/s]
# Speed offset
self.spdoffset = np.array([]) # speed offset [kts]
self.spdoffsetdev = np.array([]) # speed offset deviation [kts]
self.spd_onoff = np.array([]) # speed offset on/off
# Whether to perform LNAV and VNAV
self.swlnav = np.array([], dtype=np.bool)
self.swvnav = np.array([], dtype=np.bool)
# Flight Models
self.asas = ASAS(self)
self.ap = Autopilot(self)
self.pilot = Pilot(self)
self.adsb = ADSB(self)
self.trails = Trails(self)
self.actwp = ActiveWaypoint(self)
# Traffic performance data
self.avsdef = np.array([]) # [m/s]default vertical speed of autopilot
self.aphi = np.array([]) # [rad] bank angle setting of autopilot
self.ax = np.array([]) # [m/s2] absolute value of longitudinal accelleration
self.bank = np.array([]) # nominal bank angle, [radian]
self.hdgsel = np.array([], dtype=np.bool) # determines whether aircraft is turning
# Crossover altitude
self.abco = np.array([])
self.belco = np.array([])
# limit settings
self.limspd = np.array([]) # limit speed
self.limspd_flag = np.array([], dtype=np.bool) # flag for limit spd - we have to test for max and min
self.limalt = np.array([]) # limit altitude
self.limvs = np.array([]) # limit vertical speed due to thrust limitation
self.limvs_flag = np.array([])
# Display information on label
self.label = [] # Text and bitmap of traffic label
# Miscallaneous
self.coslat = np.array([]) # Cosine of latitude for computations
self.eps = np.array([]) # Small nonzero numbers
# Default bank angles per flight phase
self.bphase = np.deg2rad(np.array([15, 35, 35, 35, 15, 45]))
self.reset(navdb)
Created on Mar 7, 2013
@author: mllofriu
'''
import roslib; roslib.load_manifest('visionTests')
import rospy
from pilot import Pilot
from affordances import Affordances
import random
import math
if __name__ == '__main__':
rospy.init_node('pilotAffordancesDriver')
p = Pilot()
affCalc = Affordances()
while not rospy.is_shutdown():
aff = affCalc.getAffordances()
print "Affordances", aff
validAngles = []
for i in range(len(aff)):
if aff[i]:
validAngles.append(affCalc.possibleAngles[i])
if len(validAngles) > 0:
i = random.randint(0,len(validAngles)-1)
rot = validAngles[i]
p.rotateRad(rot)
class FrontEnd:
def __init__(self):
# Init Tello object that interacts with the Tello drone
tello = Tello(logging=False)
self.tello = tello
if not self.tello.connect():
raise Exception("Tello not connected")
if not self.tello.set_speed(10):
raise Exception("Not set speed to lowest possible")
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
raise Exception("Could not stop video stream")
if not self.tello.streamon():
raise Exception("Could not start video stream")
self.frame_read = tello.get_frame_read()
self.pilot = Pilot(tello, self)
self.key_ctrl = KeyboardController(tello)
def run(self):
self.tello.get_battery()
while 1:
self.draw()
self.exit()
def draw(self):
self.tello.send_rc_control()
if self.frame_read.stopped:
self.frame_read.stop()
frame = self.frame_read.frame
np.save('frame', frame)
depth = predict_depth(frame)
print(depth.min()/25.5)
# TODO add some delay here to allow user to control
if not self.key_ctrl.check_key():
self.pilot.on_frame(frame, depth)
# Draw the center of screen circle
cv2.circle(depth, (220//2, 220//2), 10, (0, 0, 255), 2)
# Display the resulting frame
cv2.imshow('Hide n Seek', depth)
cv2.imshow('Real', frame)
def draw_box(self, frame, x, y, end_cord_x, end_cord_y, target_x, target_y):
# Draw the face bounding box
cv2.rectangle(frame, (x, y), (end_cord_x,
end_cord_y), fbCol, fbStroke)
def exit(self):
# On exit, print the battery
self.tello.get_battery()
# When everything done, release the capture
cv2.destroyAllWindows()
# Call it always before finishing. I deallocate resources.
self.tello.end()
answ1 = input('try again.. (pass/mil)')
if answ1 == 'pass' and answ1 == 'mil':
break
answ2 = input(
'what color of plain will you select ?') # user select color of plain
num += 1 # generation number of plain
if answ1 == 'pass': # info collected from user is used to create objects
p = PassPlain(num, answ2) # p is instance of PassPlain class
print(p)
status = p.status
else:
m = MilPlain(num, answ2) # m is instance of MIlPlain class
print(m)
status = m.status
Pilot(status)
writer(num, answ1, answ2, status) # preparing data to save in files
check = input('One more plain ? (y/n)')
if check == 'n':
break
write_to_json(full) # writing attr in JSON-file
write_to_xml(num) # writing attr in XML-file
print('Good-bye!')
def __init__(self, pid):
Pilot.__init__(self, pid)
