def expect_callback(self, 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 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(self): signal.signal(signal.SIGINT, self.exit_handler) signal.signal(signal.SIGTERM, self.exit_handler) opts = self.opts jsb_in = self.jsb_in jsb_console = self.jsb_console fdm = self.fdm jsb = self.jsb 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(), 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()) self.process_jsb_input(buf, simtime) frame_count += 1 new_frame = True #if sim_in.fileno() in rin: #simbuf = sim_in.recv(28) self.update_input() 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: self.update_wind(self.wind) last_wind_update = tnow if tnow - last_report > 0.1: 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