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)