ardupilot/Tools/autotest/fakepos.py

123 lines
2.7 KiB
Python
Raw Normal View History

#!/usr/bin/env python
2011-12-22 21:07:32 -04:00
import socket, struct, time, errno
from math import *
class udp_out(object):
'''a UDP output socket'''
def __init__(self, device):
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)
self.destination_addr = (a[0], int(a[1]))
self.port.setblocking(0)
self.last_address = None
def recv(self,n=None):
try:
data, self.last_address = self.port.recvfrom(300)
except socket.error as e:
2011-12-22 21:07:32 -04:00
if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
return ""
raise
return data
def write(self, buf):
try:
self.port.sendto(buf, self.destination_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_out("127.0.0.1:5501")
latitude = -35
longitude = 149
altitude = 600.0
heading = 0.0
speedN = 0
speedE = 0.0
speedD = 0.0
xAccel = 0.0
yAccel = 0.0
zAccel = 0.0
rollRate = 0.0
pitchRate = 0.0
yawRate = 0.0
rollDeg = 0.0
pitchDeg = 0.0
yawDeg = 0.0
airspeed = 0
magic = 0x4c56414f
deltaT = 0.005
rollDeg = 45
pitchDeg = 0
pitchMax = None
rollMax = None
if True:
pitchRate = 1
pitchMax = 45
rollMax = 45
while True:
xAccel = sin(radians(pitchDeg))
yAccel = -sin(radians(rollDeg)) * cos(radians(pitchDeg))
zAccel = -cos(radians(rollDeg)) * cos(radians(pitchDeg))
scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel))
xAccel *= scale;
yAccel *= scale;
zAccel *= scale;
buf = struct.pack('<17dI',
latitude, longitude, altitude, heading,
speedN, speedE, speedD,
xAccel, yAccel, zAccel,
rollRate, pitchRate, yawRate,
rollDeg, pitchDeg, yawDeg,
airspeed, magic)
udp.write(buf)
time.sleep(deltaT)
yawDeg += yawRate * deltaT
if yawDeg > 180:
yawDeg -= 360
if yawDeg < -180:
yawDeg += 360
heading = yawDeg
if pitchMax is not None and fabs(pitchDeg) > pitchMax:
pitchRate = -pitchRate
if rollMax is not None and fabs(rollDeg) > rollMax:
rollRate = -rollRate
pitchDeg += pitchRate * deltaT
if pitchDeg > 180:
pitchDeg -= 360
if pitchDeg < -180:
pitchDeg += 360
rollDeg += rollRate * deltaT
if rollDeg > 180:
rollDeg -= 360
if rollDeg < -180:
rollDeg += 360