You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
75 lines
1.9 KiB
75 lines
1.9 KiB
#!/usr/bin/env python |
|
|
|
import socket, struct, time, math, errno |
|
import fgFDM |
|
|
|
class udp_socket(object): |
|
'''a UDP socket''' |
|
def __init__(self, device, blocking=True, input=True): |
|
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) |
|
if input: |
|
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 |
|
|
|
def recv(self,n=1000): |
|
try: |
|
data, self.last_address = self.port.recvfrom(n) |
|
except socket.error as e: |
|
if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]: |
|
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 |
|
|
|
|
|
|
|
def ft2m(x): |
|
return x * 0.3048 |
|
|
|
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'))
|
|
|