autotest: useful script for testing gyro and accel calculations

Tools/autotest/fakepos.py

@@ -0,0 +1,121 @@
+#!/usr/bin/env python
+
+import socket, struct, time
+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 [ 11, 35 ]:
+                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 = 0x4c56414d
+
+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 = m2ft(9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel)))
+    xAccel *= scale;
+    yAccel *= scale;
+    zAccel *= scale;
+
+    buf = struct.pack('>ddddddddddddddddI',
+                      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