mirror of https://github.com/ArduPilot/ardupilot
122 lines
2.7 KiB
Python
Executable File
122 lines
2.7 KiB
Python
Executable File
#!/usr/bin/env python
|
|
|
|
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:
|
|
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
|
|
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 = 0x4c56414e
|
|
|
|
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)) * cos(radians(rollDeg))
|
|
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('<16dI',
|
|
latitude, longitude, altitude, heading,
|
|
speedN, speedE,
|
|
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
|