2011-12-02 00:13:50 -04:00
|
|
|
#!/usr/bin/env python
|
|
|
|
|
2016-07-31 07:22:06 -03:00
|
|
|
import errno
|
|
|
|
import socket
|
|
|
|
import sys
|
|
|
|
import time
|
|
|
|
|
2013-08-29 01:20:16 -03:00
|
|
|
from pymavlink import fgFDM
|
2011-12-02 00:13:50 -04:00
|
|
|
|
2016-07-31 07:22:06 -03:00
|
|
|
|
2011-12-02 00:13:50 -04:00
|
|
|
class udp_socket(object):
|
2016-07-31 07:22:06 -03:00
|
|
|
"""A UDP socket."""
|
|
|
|
def __init__(self, device, blocking=True, is_input=True):
|
2011-12-02 00:13:50 -04:00
|
|
|
a = device.split(':')
|
|
|
|
if len(a) != 2:
|
|
|
|
print("UDP ports must be specified as host:port")
|
|
|
|
sys.exit(1)
|
|
|
|
self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
2016-07-31 07:22:06 -03:00
|
|
|
if is_input:
|
2011-12-02 00:13:50 -04:00
|
|
|
self.port.bind((a[0], int(a[1])))
|
|
|
|
self.destination_addr = None
|
|
|
|
else:
|
|
|
|
self.destination_addr = (a[0], int(a[1]))
|
|
|
|
if not blocking:
|
|
|
|
self.port.setblocking(0)
|
|
|
|
self.last_address = None
|
|
|
|
|
2016-07-31 07:22:06 -03:00
|
|
|
def recv(self, n=1000):
|
2011-12-02 00:13:50 -04:00
|
|
|
try:
|
|
|
|
data, self.last_address = self.port.recvfrom(n)
|
|
|
|
except socket.error as e:
|
2016-07-31 07:22:06 -03:00
|
|
|
if e.errno in [errno.EAGAIN, errno.EWOULDBLOCK]:
|
2011-12-02 00:13:50 -04:00
|
|
|
return ""
|
|
|
|
raise
|
|
|
|
return data
|
|
|
|
|
|
|
|
def write(self, buf):
|
|
|
|
try:
|
|
|
|
if self.destination_addr:
|
|
|
|
self.port.sendto(buf, self.destination_addr)
|
|
|
|
else:
|
|
|
|
self.port.sendto(buf, self.last_addr)
|
|
|
|
except socket.error:
|
|
|
|
pass
|
|
|
|
|
|
|
|
|
|
|
|
udp = udp_socket("127.0.0.1:5123")
|
2016-07-31 07:22:06 -03:00
|
|
|
fgout = udp_socket("127.0.0.1:5124", is_input=False)
|
2011-12-02 00:13:50 -04:00
|
|
|
|
|
|
|
tlast = time.time()
|
|
|
|
count = 0
|
|
|
|
|
|
|
|
fg = fgFDM.fgFDM()
|
|
|
|
|
|
|
|
while True:
|
2016-07-31 07:22:06 -03:00
|
|
|
udp_buffer = udp.recv(1000)
|
|
|
|
fg.parse(udp_buffer)
|
2011-12-02 00:13:50 -04:00
|
|
|
fgout.write(fg.pack())
|
|
|
|
count += 1
|
|
|
|
if time.time() - tlast > 1.0:
|
2016-07-31 07:22:06 -03:00
|
|
|
print("%u FPS len=%u" % (count, len(udp_buffer)))
|
2011-12-02 00:13:50 -04:00
|
|
|
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'))
|