ardupilot/Tools/autotest/pysim/fg_display.py

76 lines
1.9 KiB
Python
Executable File

#!/usr/bin/env python
import socket, struct, time, math, errno
from pymavlink 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'))