def __init__(self, filename):
self.root = Tkinter.Tk()
self.filesize = os.path.getsize(filename)
self.filepos = 0.0
self.mlog = mavutil.mavlink_connection(filename, planner_format=opts.planner,
robust_parsing=True)
self.mout = []
for m in opts.out:
self.mout.append(mavutil.mavudp(m, input=False))
self.fgout = []
for f in opts.fgout:
self.fgout.append(mavutil.mavudp(f, input=False))
self.fdm = fgFDM.fgFDM()
self.msg = self.mlog.recv_match(condition=opts.condition)
if self.msg is None:
sys.exit(1)
self.last_timestamp = getattr(self.msg, '_timestamp')
self.paused = False
self.topframe = Tkinter.Frame(self.root)
self.topframe.pack(side=Tkinter.TOP)
self.frame = Tkinter.Frame(self.root)
self.frame.pack(side=Tkinter.LEFT)
self.slider = Tkinter.Scale(self.topframe, from_=0, to=1.0, resolution=0.01,
orient=Tkinter.HORIZONTAL, command=self.slew)
self.slider.pack(side=Tkinter.LEFT)
self.clock = Tkinter.Label(self.topframe,text="")
self.clock.pack(side=Tkinter.RIGHT)
self.playback = Tkinter.Spinbox(self.topframe, from_=0, to=20, increment=0.1, width=3)
self.playback.pack(side=Tkinter.BOTTOM)
self.playback.delete(0, "end")
self.playback.insert(0, 1)
self.buttons = {}
self.button('quit', 'gtk-quit.gif', self.frame.quit)
self.button('pause', 'media-playback-pause.gif', self.pause)
self.button('rewind', 'media-seek-backward.gif', self.rewind)
self.button('forward', 'media-seek-forward.gif', self.forward)
self.button('status', 'Status', self.status)
self.flightmode = Tkinter.Label(self.frame,text="")
self.flightmode.pack(side=Tkinter.RIGHT)
self.next_message()
self.root.mainloop()
fg_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
fg_out.connect(fg_out_address)
fg_out.setblocking(0)
# setup input from SITL
sim_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sim_in.bind(sim_in_address)
sim_in.setblocking(0)
# setup output to SITL
sim_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sim_out.connect(sim_out_address)
sim_out.setblocking(0)
# FG FDM object
fdm = fgFDM.fgFDM()
# create the quadcopter model
a = MultiCopter(frame=opts.frame)
print("Simulating %u motors for frame %s" % (len(a.motors), opts.frame))
# motors initially off
m = [0.0] * 11
lastt = time.time()
frame_count = 0
# parse home
v = opts.home.split(',')
if len(v) != 4:
def m2ft(x):
return x / 0.3048
def kt2mps(x):
return x * 0.514444444
def mps2kt(x):
return x / 0.514444444
udp = udp_socket("127.0.0.1:5123")
fgout = udp_socket("127.0.0.1:5124", input=False)
tlast = time.time()
count = 0
fg = fgFDM.fgFDM()
while True:
buf = udp.recv(1000)
fg.parse(buf)
fgout.write(fg.pack())
count += 1
if time.time() - tlast > 1.0:
print("%u FPS len=%u" % (count, len(buf)))
count = 0
tlast = time.time()
print(fg.get('latitude', units='degrees'),
fg.get('longitude', units='degrees'),
fg.get('altitude', units='meters'),
fg.get('vcas', units='mps'))
sim_out.setblocking(0)
# setup possible output to FlightGear for display
fg_out = None
if opts.fgout:
fg_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
fg_out.connect(interpret_address(opts.fgout))
# setup wind generator
wind = util.Wind(opts.wind)
setup_home(opts.home)
fdm = fgFDM.fgFDM()
jsb_console.send("info\n")
jsb_console.send("resume\n")
jsb.expect("trim computation time")
time.sleep(1.5)
jsb_console.logfile = None
print("Simulator ready to fly")
def main_loop():
"""run main loop"""
tnow = time.time()
last_report = tnow
last_sim_input = tnow
def kt2mps(x):
return x * 0.514444444
def mps2kt(x):
return x / 0.514444444
udp = udp_socket("127.0.0.1:5123")
fgout = udp_socket("127.0.0.1:5124", input=False)
tlast = time.time()
count = 0
fg = fgFDM.fgFDM()
while True:
buf = udp.recv(1000)
fg.parse(buf)
fgout.write(fg.pack())
count += 1
if time.time() - tlast > 1.0:
print("%u FPS len=%u" % (count, len(buf)))
count = 0
tlast = time.time()
print(fg.get('latitude', units='degrees'),
fg.get('longitude', units='degrees'),
fg.get('altitude', units='meters'), fg.get('vcas', units='mps'))
def __init__(self): self.state_estimation = self.StateEstimation() self.K = np.zeros((2,12)) self.K[0,6] = 5.0 self.K[1,7] = 11.0 self.Kphi = np.zeros((1,3)) self.Kphi[0,0] = 1.0 self.Kphi[0,1] = 1.0 self.Kphi[0,2] = -0.5*1.5*self.Kphi[0,1]*math.atan(200.0*math.pi/180.0) self.udpgen = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.udpgen.connect(interpret_address(fg_control_ip_port)) self.udpgen.setblocking(0) self.fg_dynamics = fgFDM.fgFDM() self.fg_dynamics_udp = udp_socket(fg_dynamics_ip_port) #self.fg_control = fgCNTRL.fgCNTRL() self.tcp_loop = False self.tcp_ip = julia_ip self.tcp_port = julia_port self.tcp_socket = None self.tcp_connection = None self.julia_connected = False self.tcp_vis_loop = False self.tcp_vis_ip = vis_ip self.tcp_vis_port = vis_port self.tcp_vis_socket = None self.tcp_vis_connections = [] self.control_loop = False self.tape_init = False self.tape = None self.tape_reset = False self.tape_t0 = time.time() self.tape_count = 0 self.file_write = False self.file_handle = None self.cur_idx = 0 self.nxt_idx = self.cur_idx + 1 self.phi_desired_tape = 0 self.phi_desired = 0.0 self.error_tape = None self.error_wall = None self.side_tape = None self.side_wall = None self.psi_delta = 0 self.cur_wpt_x = 0 self.cur_wpt_y = 0 self.nxt_wpt_x = 0 self.nxt_wpt_y = 0 self.wall_l_x = 0 self.wall_l_y = 0 self.wall_r_x = 0 self.wall_r_y = 0 self.wind_vel = 0 self.trigger_wall_l_x = 0 self.trigger_wall_l_y = 0 self.trigger_wall_r_x = 0 self.trigger_wall_r_y = 0
