def main_loop():
'''run main loop'''
tnow = time.time()
last_report = tnow
last_sim_input = tnow
paused = False
while True:
rin = [
jsb_in.fileno(),
sim_in.fileno(),
jsb_console.fileno(),
jsb.fileno()
]
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
continue
tnow = time.time()
if jsb_in.fileno() in rin:
buf = jsb_in.recv(fdm.packet_size())
process_jsb_input(buf)
if sim_in.fileno() in rin:
simbuf = sim_in.recv(28)
process_sitl_input(simbuf)
last_sim_input = tnow
# show any jsbsim console output
if jsb_console.fileno() in rin:
util.pexpect_drain(jsb_console)
if jsb.fileno() in rin:
util.pexpect_drain(jsb)
if tnow - last_sim_input > 0.2:
if not paused:
print("PAUSING SIMULATION")
paused = True
else:
if paused:
print("RESUMING SIMULATION")
paused = False
# only simulate wind above 5 meters, to prevent crashes while
# waiting for takeoff
if tnow - last_report > 3:
print("asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f)" %
(fdm.get('altitude',
units='meters'), fdm.get('agl', units='meters'),
fdm.get('phi',
units='degrees'), fdm.get('theta', units='degrees'),
fdm.get('A_X_pilot',
units='mpss'), fdm.get('A_Y_pilot', units='mpss'),
fdm.get('A_Z_pilot', units='mpss')))
last_report = time.time()
def update(self):
# watch files
rin = [self.jsb_in.fileno(), self.jsb_console.fileno(), self.jsb.fileno(), self.sitl.port.fileno()]
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
return
if self.jsb_in.fileno() in rin:
self.process_sensor_data()
if self.sitl.port.fileno() in rin:
msg = self.sitl.recv_msg()
self.Controls = aircraft.Controls.from_mavlink(msg)
self.Controls.send_to_jsbsim(self.jsb_console)
if self.jsb_console.fileno() in rin:
util.pexpect_drain(self.jsb_console)
if self.jsb.fileno() in rin:
util.pexpect_drain(self.jsb)
return True
def update(self):
# watch files
rin = [self.jsb_in.fileno(), self.jsb_console.fileno(), self.jsb.fileno(), self.sitl.port.fileno()]
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
return
if self.jsb_in.fileno() in rin:
self.process_sensor_data()
# print 'self.jsb_in.fileno()'
if self.sitl.port.fileno() in rin:
msg = self.sitl.recv_msg()
self.Controls = multicopter.Controls.from_mavlink(msg)
self.Controls.send_to_jsbsim(self.jsb_console)
# print 'self.sitl.port.fileno()'
if self.jsb_console.fileno() in rin:
util.pexpect_drain(self.jsb_console)
# print 'self.jsb_console.fileno()'
if self.jsb.fileno() in rin:
util.pexpect_drain(self.jsb)
# print 'self.jsb.fileno()'
return True
def update(self):
# watch files
rin = [
self.jsb_in.fileno(),
self.jsb_console.fileno(),
self.jsb.fileno(), self.gcs.fd
]
# receive messages on serial port
while self.master.port.inWaiting() > 0:
self.process_master()
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
return
# if new gcs input, process it
if self.gcs.fd in rin:
self.process_gcs()
# if new jsbsim input, process it
if self.jsb_in.fileno() in rin:
if self.mode == 'state':
if (time.time() - self.t_hil_state) > 1.0 / 50:
self.t_hil_state = time.time()
self.ac.send_state(self.master.mav)
elif self.mode == 'sensor':
self.ac.send_sensors(self.master.mav)
self.process_jsb_input()
# gcs not currently getting HIL_STATE message
#self.x.send_to_mav(self.gcs.mav)
self.frame_count += 1
# show any jsbsim console output
if self.jsb_console.fileno() in rin:
util.pexpect_drain(self.jsb_console)
if self.jsb.fileno() in rin:
util.pexpect_drain(self.jsb)
dt_report = time.time() - self.last_report
if dt_report > 5:
print '\nmode: {0:X}, JSBSim {1:5.0f} Hz, {2:d} sent, {3:d} received, {4:d} errors, bwin={5:.1f} kB/s, bwout={6:.1f} kB/s'.format(
self.master.base_mode, self.frame_count / dt_report,
self.master.mav.total_packets_sent,
self.master.mav.total_packets_received,
self.master.mav.total_receive_errors, 0.001 *
(self.master.mav.total_bytes_received - self.bytes_recv) /
dt_report,
0.001 * (self.master.mav.total_bytes_sent - self.bytes_sent) /
dt_report)
print self.counts
self.bytes_sent = self.master.mav.total_bytes_sent
self.bytes_recv = self.master.mav.total_bytes_received
self.frame_count = 0
self.last_report = time.time()
return True
def update(self):
# watch files
rin = [self.jsb_in.fileno(), self.jsb_console.fileno(), self.jsb.fileno(), self.gcs.fd]
# receive messages on serial port
while self.master.port.inWaiting() > 0:
self.process_master()
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
return
# if new gcs input, process it
if self.gcs.fd in rin:
self.process_gcs()
# if new jsbsim input, process it
if self.jsb_in.fileno() in rin:
if self.mode == "state":
if (time.time() - self.t_hil_state) > 1.0 / 50:
self.t_hil_state = time.time()
self.ac.send_state(self.master.mav)
elif self.mode == "sensor":
self.ac.send_sensors(self.master.mav)
self.process_jsb_input()
# gcs not currently getting HIL_STATE message
# self.x.send_to_mav(self.gcs.mav)
self.frame_count += 1
# show any jsbsim console output
if self.jsb_console.fileno() in rin:
util.pexpect_drain(self.jsb_console)
if self.jsb.fileno() in rin:
util.pexpect_drain(self.jsb)
dt_report = time.time() - self.last_report
if dt_report > 5:
print "\nmode: {0:X}, JSBSim {1:5.0f} Hz, {2:d} sent, {3:d} received, {4:d} errors, bwin={5:.1f} kB/s, bwout={6:.1f} kB/s".format(
self.master.base_mode,
self.frame_count / dt_report,
self.master.mav.total_packets_sent,
self.master.mav.total_packets_received,
self.master.mav.total_receive_errors,
0.001 * (self.master.mav.total_bytes_received - self.bytes_recv) / dt_report,
0.001 * (self.master.mav.total_bytes_sent - self.bytes_sent) / dt_report,
)
print self.counts
self.bytes_sent = self.master.mav.total_bytes_sent
self.bytes_recv = self.master.mav.total_bytes_received
self.frame_count = 0
self.last_report = time.time()
return True
def main_loop():
'''run main loop'''
tnow = time.time()
last_report = tnow
last_sim_input = tnow
paused = False
while True:
rin = [jsb_in.fileno(), sim_in.fileno(), jsb_console.fileno(), jsb.fileno()]
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
continue
tnow = time.time()
if jsb_in.fileno() in rin:
buf = jsb_in.recv(fdm.packet_size())
process_jsb_input(buf)
if sim_in.fileno() in rin:
simbuf = sim_in.recv(28)
process_sitl_input(simbuf)
last_sim_input = tnow
# show any jsbsim console output
if jsb_console.fileno() in rin:
util.pexpect_drain(jsb_console)
if jsb.fileno() in rin:
util.pexpect_drain(jsb)
if tnow - last_sim_input > 0.2:
if not paused:
print("PAUSING SIMULATION")
paused = True
else:
if paused:
print("RESUMING SIMULATION")
paused = False
# only simulate wind above 5 meters, to prevent crashes while
# waiting for takeoff
if tnow - last_report > 3:
print("asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f)" % (
fdm.get('altitude', units='meters'),
fdm.get('agl', units='meters'),
fdm.get('phi', units='degrees'),
fdm.get('theta', units='degrees'),
fdm.get('A_X_pilot', units='mpss'),
fdm.get('A_Y_pilot', units='mpss'),
fdm.get('A_Z_pilot', units='mpss')))
last_report = time.time()
def main_loop():
'''run main loop'''
tnow = time.time()
last_report = tnow
last_sim_input = tnow
last_wind_update = tnow
frame_count = 0
paused = False
timestamp = 0
while True:
rin = [ros_in.fileno(), sim_in.fileno()]
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
continue
tnow = time.time()
if ros_in.fileno() in rin:
buf = ros_in.recv(fdm.packet_size())
process_ros_input(buf, frame_count, timestamp)
frame_count += 1
timestamp += 1000
if sim_in.fileno() in rin:
simbuf = sim_in.recv(28)
print(len(simbuf))
process_sitl_input(simbuf)
last_sim_input = tnow
if tnow - last_report > 3:
print(
"FPS %u asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f)"
% (frame_count / (time.time() - last_report),
fdm.get('altitude',
units='meters'), fdm.get('agl', units='meters'),
fdm.get('phi',
units='degrees'), fdm.get('theta', units='degrees'),
fdm.get('A_X_pilot',
units='mpss'), fdm.get('A_Y_pilot', units='mpss'),
fdm.get('A_Z_pilot', units='mpss')))
frame_count = 0
last_report = time.time()
def main_loop():
'''run main loop'''
tnow = time.time()
last_report = tnow
last_sim_input = tnow
last_wind_update = tnow
frame_count = 0
paused = False
timestamp = 0
while True:
rin = [ros_in.fileno(), sim_in.fileno()]
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
continue
tnow = time.time()
if ros_in.fileno() in rin:
buf = ros_in.recv(fdm.packet_size())
process_ros_input(buf,frame_count, timestamp)
frame_count += 1
timestamp += 1000
if sim_in.fileno() in rin:
simbuf = sim_in.recv(28)
print(len(simbuf))
process_sitl_input(simbuf)
last_sim_input = tnow
if tnow - last_report > 3:
print("FPS %u asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f)" % (
frame_count / (time.time() - last_report),
fdm.get('altitude', units='meters'),
fdm.get('agl', units='meters'),
fdm.get('phi', units='degrees'),
fdm.get('theta', units='degrees'),
fdm.get('A_X_pilot', units='mpss'),
fdm.get('A_Y_pilot', units='mpss'),
fdm.get('A_Z_pilot', units='mpss')))
frame_count = 0
last_report = time.time()
def main_loop():
'''run main loop'''
last_report = time.time()
frame_count = 0
while True:
rin = [jsb_in.fileno(), sim_in.fileno(), jsb_console.fileno(), jsb.fileno()]
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
continue
if jsb_in.fileno() in rin:
buf = jsb_in.recv(fdm.packet_size())
process_jsb_input(buf)
frame_count += 1
if sim_in.fileno() in rin:
simbuf = sim_in.recv(22)
process_sitl_input(simbuf)
# show any jsbsim console output
if jsb_console.fileno() in rin:
util.pexpect_drain(jsb_console)
if jsb.fileno() in rin:
util.pexpect_drain(jsb)
if time.time() - last_report > 0.5:
print("FPS %u asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f)" % (
frame_count / (time.time() - last_report),
fdm.get('altitude', units='meters'),
fdm.get('agl', units='meters'),
fdm.get('phi', units='degrees'),
fdm.get('theta', units='degrees'),
fdm.get('A_X_pilot', units='mpss'),
fdm.get('A_Y_pilot', units='mpss'),
fdm.get('A_Z_pilot', units='mpss')))
frame_count = 0
last_report = time.time()
def main_loop():
'''run main loop'''
tnow = time.time()
last_sim_input = tnow
paused = False
while True:
rin = [sim_in.fileno(), fg.fileno()]
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
continue
tnow = time.time;
if sim_in.fileno() in rin:
buf = sim_in.recv(17*8+4)
process_input(buf) #take buf (info from ardupilot) and process it and send it back
if tnow - last_sim_input > 0.2:
if not paused:
print("PAUSING SIMULATION")
paused = True
else:
if paused:
print("RESUMING SIMULATION")
paused = False
if tnow - last_report > 3:
print("alt=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f)" % (
fdm.get('altitude', units='meters'),
fdm.get('roll', units='degrees'),
fdm.get('pitch', units='degrees'),
fdm.get('ax', units='mpss'),
fdm.get('ay', units='mpss'),
fdm.get('az', units='mpss')))
def main_loop():
'''run main loop'''
tnow = time.time()
last_report = tnow
last_sim_input = tnow
last_wind_update = tnow
frame_count = 0
paused = False
simstep = 1.0/opts.rate
simtime = simstep
frame_time = 1.0/opts.rate
scaled_frame_time = frame_time/opts.speedup
last_wall_time = time.time()
achieved_rate = opts.speedup
while True:
new_frame = False
rin = [jsb_in.fileno(), sim_in.fileno(), jsb_console.fileno(), jsb.fileno()]
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
continue
tnow = time.time()
if jsb_in.fileno() in rin:
buf = jsb_in.recv(fdm.packet_size())
process_jsb_input(buf, simtime)
frame_count += 1
new_frame = True
#if sim_in.fileno() in rin:
#simbuf = sim_in.recv(28)
simbuf = ""
process_sitl_input(simbuf)
simtime += simstep
last_sim_input = tnow
# show any jsbsim console output
if jsb_console.fileno() in rin:
util.pexpect_drain(jsb_console)
if jsb.fileno() in rin:
util.pexpect_drain(jsb)
# only simulate wind above 5 meters, to prevent crashes while
# waiting for takeoff
if tnow - last_wind_update > 0.1:
update_wind(wind)
last_wind_update = tnow
if tnow - last_report > 3:
print("FPS %u asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f) AR=%.1f" % (
frame_count / (time.time() - last_report),
fdm.get('altitude', units='meters'),
fdm.get('agl', units='meters'),
fdm.get('phi', units='degrees'),
fdm.get('theta', units='degrees'),
fdm.get('A_X_pilot', units='mpss'),
fdm.get('A_Y_pilot', units='mpss'),
fdm.get('A_Z_pilot', units='mpss'),
achieved_rate))
frame_count = 0
last_report = time.time()
if new_frame:
now = time.time()
if now < last_wall_time + scaled_frame_time:
dt = last_wall_time+scaled_frame_time - now
time.sleep(last_wall_time+scaled_frame_time - now)
now = time.time()
if now > last_wall_time and now - last_wall_time < 0.1:
rate = 1.0/(now - last_wall_time)
achieved_rate = (0.98*achieved_rate) + (0.02*rate)
if achieved_rate < opts.rate*opts.speedup:
scaled_frame_time *= 0.999
else:
scaled_frame_time *= 1.001
last_wall_time = now
def main_loop():
"""run main loop"""
tnow = time.time()
last_report = tnow
last_sim_input = tnow
last_wind_update = tnow
frame_count = 0
paused = False
while True:
rin = [jsb_in.fileno(), sim_in.fileno(), jsb_console.fileno(), jsb.fileno()]
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
continue
tnow = time.time()
if jsb_in.fileno() in rin:
buf = jsb_in.recv(fdm.packet_size())
process_jsb_input(buf)
frame_count += 1
if sim_in.fileno() in rin:
simbuf = sim_in.recv(22)
process_sitl_input(simbuf)
last_sim_input = tnow
# show any jsbsim console output
if jsb_console.fileno() in rin:
util.pexpect_drain(jsb_console)
if jsb.fileno() in rin:
util.pexpect_drain(jsb)
if tnow - last_sim_input > 0.2:
if not paused:
print("PAUSING SIMULATION")
paused = True
jsb_console.send("hold\n")
else:
if paused:
print("RESUMING SIMULATION")
paused = False
jsb_console.send("resume\n")
if tnow - last_wind_update > 0.1:
update_wind(wind)
last_wind_update = tnow
if tnow - last_report > 0.5:
print(
"FPS %u asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f)"
% (
frame_count / (time.time() - last_report),
fdm.get("altitude", units="meters"),
fdm.get("agl", units="meters"),
fdm.get("phi", units="degrees"),
fdm.get("theta", units="degrees"),
fdm.get("A_X_pilot", units="mpss"),
fdm.get("A_Y_pilot", units="mpss"),
fdm.get("A_Z_pilot", units="mpss"),
)
)
frame_count = 0
last_report = time.time()
def main_loop():
'''run main loop'''
tnow = time.time()
last_report = tnow
last_sim_input = tnow
last_wind_update = tnow
frame_count = 0
paused = False
while True:
rin = [
jsb_in.fileno(),
sim_in.fileno(),
jsb_console.fileno(),
jsb.fileno()
]
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
continue
tnow = time.time()
if jsb_in.fileno() in rin:
buf = jsb_in.recv(fdm.packet_size())
process_jsb_input(buf)
frame_count += 1
if sim_in.fileno() in rin:
simbuf = sim_in.recv(22)
process_sitl_input(simbuf)
last_sim_input = tnow
# show any jsbsim console output
if jsb_console.fileno() in rin:
util.pexpect_drain(jsb_console)
if jsb.fileno() in rin:
util.pexpect_drain(jsb)
if tnow - last_sim_input > 0.2:
if not paused:
print("PAUSING SIMULATION")
paused = True
jsb_console.send('hold\n')
else:
if paused:
print("RESUMING SIMULATION")
paused = False
jsb_console.send('resume\n')
if tnow - last_wind_update > 0.1:
update_wind(wind)
last_wind_update = tnow
if tnow - last_report > 0.5:
print(
"FPS %u asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f)"
% (frame_count / (time.time() - last_report),
fdm.get('altitude',
units='meters'), fdm.get('agl', units='meters'),
fdm.get('phi',
units='degrees'), fdm.get('theta', units='degrees'),
fdm.get('A_X_pilot',
units='mpss'), fdm.get('A_Y_pilot', units='mpss'),
fdm.get('A_Z_pilot', units='mpss')))
frame_count = 0
last_report = time.time()
def main_loop():
'''run main loop'''
tnow = time.time()
tFinal = 80
last_report = tnow
last_sim_input = tnow
last_wind_update = tnow
frame_count = 0
input_count = 0
paused = False
# Load attack definitions and set the desired attack
execfile('jsbsim/attackDefinitions.py', globals())
print L
attack1 = imuGyroNoise
attack2 = gainAccelX
global latFreq
global rNoiseVar
latFreq = attack1.nominal
rNoiseVar = attack2.nominal
resultKeys = ['flightFail', 'missionFail']
innerSize = len(attack1.attack_values)
outerSize = len(attack2.attack_values)
results = dict.fromkeys(resultKeys)
for key in results:
results[key] = numpy.empty(shape=(innerSize, outerSize))
for outerIndex, outerValue in enumerate(attack1.attack_values):
iterResults = defaultdict(list)
for innerIndex, innerValue in enumerate(attack2.attack_values):
latFreq = attack1.attack_values[outerIndex]
rNoiseVar = attack2.attack_values[innerIndex]
# Reset the vehicle state
tstart = reset_sim()
tstart_sim = time.time() #fdm.get('time', units='seconds'),
missionFailed = False
while True:
rin = [jsb_in.fileno(), sim_in.fileno(), jsb_console.fileno(), jsb.fileno()]
try:
(rin, win, xin) = select.select(rin, [], [], 1.0)
except select.error:
util.check_parent()
continue
tnow = time.time()
if jsb_in.fileno() in rin:
buf = jsb_in.recv(fdm.packet_size())
process_jsb_input(buf)
frame_count += 1
if sim_in.fileno() in rin:
simbuf = sim_in.makefile().readline()
process_sitl_input(simbuf)
last_sim_input = tnow
input_count +=1
# show any jsbsim console output
if jsb_console.fileno() in rin:
util.pexpect_drain(jsb_console)
if jsb.fileno() in rin:
util.pexpect_drain(jsb)
#if tnow - last_sim_input > 0.5:
#if not paused:
#print("PAUSING SIMULATION")
#paused = True
#jsb_console.send('hold\n')
#else:
#if paused:
#print("RESUMING SIMULATION")
#paused = False
#jsb_console.send('resume\n')
# only simulate wind above 5 meters, to prevent crashes while
# waiting for takeoff
if tnow - last_wind_update > 0.1:
update_wind(wind)
last_wind_update = tnow
if tnow - last_report > 3:
print("IPS %u FPS %u asl=%.1f agl=%.1f roll=%.1f pitch=%.1f a=(%.2f %.2f %.2f)" % (
input_count/(time.time() - last_report),
frame_count / (time.time() - last_report),
fdm.get('altitude', units='meters'),
fdm.get('agl', units='meters'),
fdm.get('phi', units='degrees'),
fdm.get('theta', units='degrees'),
fdm.get('A_X_pilot', units='mpss'),
fdm.get('A_Y_pilot', units='mpss'),
fdm.get('A_Z_pilot', units='mpss')))
print 'lat: %f lon: %f' % (fdm.get('latitude', units='degrees'),fdm.get('longitude', units='degrees'))
frame_count = 0
input_count = 0
last_report = time.time()
# mission/ flight envelope check
tsim = time.time()-tstart_sim #fdm.get('time', units='seconds') - tstart_sim,
#if not missionFailed:
#if not check_mission_env(fdm, tsim):
#print 'Mission Envelope Failure!'
#missionFailed = True
#iterResults['missionFail'].append(tsim)
#if not check_flight_env(fdm):
#print 'Flight Envelope Failure!'
#iterResults['flightFail'].append(tsim)
#if not missionFailed:
#iterResults['missionFail'].append(tsim)
#break
if tsim > tFinal:
print 'Time has ended.'
if not missionFailed:
iterResults['missionFail'].append(tsim)
iterResults['flightFail'].append(tsim)
print 'lat: %f' % fdm.get('latitude', units='degrees'),
print 'lon: %f' % fdm.get('longitude', units='degrees'),
break
for key in resultKeys:
results[key][outerIndex] = iterResults[key]
plotStructs = plot.generatePlotStructs(results, [attack1, attack2], os.getcwd()+'/')
write = open('plotStructs.pkl', 'wb')
pickle.dump(plotStructs, write)
write.close()
print plotStructs
plot.generatePlots(plotStructs)
