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 expect_callback(e): '''called when waiting for a expect pattern''' global expect_list for p in expect_list: if p == e: continue util.pexpect_drain(p)
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.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 run(self): # send something to simulator to get it running # self.sitl = mavutil.mavudp(self.sitl_address, input=False) self.sitl = mavutil.mavlink_connection('tcpin:localhost:4560') self.sitl.wait_heartbeat() print("Heartbeat from system (system %u component %u)" % (self.sitl.target_system, self.sitl.target_system)) self.sitl.write("hello") # setup output to flightgear if self.fg_address is not None: fg_address = (self.fg_address.split(':')[0], int(self.fg_address.split(':')[1])) self.fg_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.fg_out.connect(fg_address) print 'FG Connection Succeeded' self.init_JSBSim() util.pexpect_drain(self.jsb_console) self.jsb_console.send('resume\n') self.jsb_console.expect('Resuming') #self.jsb_set('simulation/reset', 1) self.update() # self.jsb.expect("\(Trim\) executed") while self.update(): pass
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 run(self): # send something to simulator to get it running self.sitl = mavutil.mavudp(self.sitl_address, input=False) self.sitl.write("hello") #setup output to flightgear if self.fg_address is not None: fg_address = (self.fg_address.split(':')[0], int(self.fg_address.split(':')[1])) self.fg_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.fg_out.connect(fg_address) self.init_JSBSim() util.pexpect_drain(self.jsb_console) self.jsb_console.send('resume\n') self.jsb_set('simulation/reset',1) self.update() self.jsb.expect("\(Trim\) executed") while self.update(): pass
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 run(self): # send something to simulator to get it running self.sitl = mavutil.mavudp(self.sitl_address, input=False) self.sitl.write("hello") #setup output to flightgear if self.fg_address is not None: fg_address = (self.fg_address.split(':')[0], int(self.fg_address.split(':')[1])) self.fg_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.fg_out.connect(fg_address) self.init_JSBSim() util.pexpect_drain(self.jsb_console) self.jsb_console.send('resume\n') self.jsb_set('simulation/reset', 1) self.update() self.jsb.expect("\(Trim\) executed") while self.update(): pass
def idle_hook(mav): '''called when waiting for a mavlink message''' global expect_list for p in expect_list: util.pexpect_drain(p)
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() 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() 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)