autotest: initial version of a antenna tracker simulator

Tools/autotest/pysim/sim_tracker.py

@@ -0,0 +1,142 @@
+#!/usr/bin/env python
+'''
+simple antenna tracker simulator
+'''
+
+from tracker import Tracker
+import util, time, os, sys, math
+import socket, struct
+import select, errno
+
+def sim_send(a):
+    '''send flight information to mavproxy'''
+    from math import degrees
+
+    earth_rates = util.BodyRatesToEarthRates(a.dcm, a.gyro)
+    (roll, pitch, yaw) = a.dcm.to_euler()
+
+    buf = struct.pack('<17dI',
+                      a.latitude, a.longitude, a.altitude, degrees(yaw),
+                      a.velocity.x, a.velocity.y, a.velocity.z,
+                      a.accelerometer.x, a.accelerometer.y, a.accelerometer.z,
+                      degrees(earth_rates.x), degrees(earth_rates.y), degrees(earth_rates.z),
+                      degrees(roll), degrees(pitch), degrees(yaw),
+                      math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y),
+                      0x4c56414f)
+    try:
+        sim_out.send(buf)
+    except socket.error as e:
+        if not e.errno in [ errno.ECONNREFUSED ]:
+            raise
+
+
+def sim_recv(state):
+    '''receive control information from SITL'''
+    try:
+        buf = sim_in.recv(28)
+    except socket.error as e:
+        if not e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
+            raise
+        return
+        
+    if len(buf) != 28:
+        print('len=%u' % len(buf))
+        return
+    control = list(struct.unpack('<14H', buf))
+    pwm = control[0:11]
+
+    # map yaw and pitch control to -1/1
+    state.yaw_input = (pwm[0]-1500)/500.0
+    state.pitch_input = (pwm[1]-1500)/500.0
+    
+
+
+def interpret_address(addrstr):
+    '''interpret a IP:port string'''
+    a = addrstr.split(':')
+    a[1] = int(a[1])
+    return tuple(a)
+
+class ControlState:
+    def __init__(self):
+        # yaw control from -1 to 1, where -1 is full left, 1 is full right
+        self.yaw_input = 0
+        # pitch control from -1 to 1, where -1 is full down, 1 is full up
+        self.pitch_input = 0
+
+##################
+# main program
+from optparse import OptionParser
+parser = OptionParser("sim_tracker.py [options]")
+parser.add_option("--simin",  dest="simin",   help="SIM input (IP:port)",       default="127.0.0.1:5502")
+parser.add_option("--simout", dest="simout",  help="SIM output (IP:port)",      default="127.0.0.1:5501")
+parser.add_option("--home", dest="home",  type='string', default=None, help="home lat,lng,alt,hdg (required)")
+parser.add_option("--rate", dest="rate", type='int', help="SIM update rate", default=100)
+parser.add_option("--rate-controlled", action='store_true', default=False, help="Use rate controlled servos")
+
+(opts, args) = parser.parse_args()
+
+for m in [ 'home' ]:
+    if not opts.__dict__[m]:
+        print("Missing required option '%s'" % m)
+        parser.print_help()
+        sys.exit(1)
+
+# UDP socket addresses
+sim_out_address = interpret_address(opts.simout)
+sim_in_address  = interpret_address(opts.simin)
+
+# setup input from SITL
+sim_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+sim_in.bind(sim_in_address)
+sim_in.setblocking(0)
+
+# setup output to SITL
+sim_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+sim_out.connect(sim_out_address)
+sim_out.setblocking(0)
+
+# create the antenna tracker  model
+a = Tracker(rate_controlled=opts.rate_controlled)
+
+# initial controls state
+state = ControlState()
+
+
+# parse home
+v = opts.home.split(',')
+if len(v) != 4:
+    print("home should be lat,lng,alt,hdg")
+    sys.exit(1)
+a.home_latitude  = float(v[0])
+a.home_longitude = float(v[1])
+a.home_altitude  = float(v[2])
+a.altitude       = a.home_altitude
+a.yaw            = float(v[3])
+a.latitude = a.home_latitude
+a.longitude = a.home_longitude
+
+a.set_yaw_degrees(a.yaw)
+
+print("Starting at lat=%f lon=%f alt=%f heading=%.1f" % (
+    a.home_latitude,
+    a.home_longitude,
+    a.altitude,
+    a.yaw))
+
+frame_time = 1.0/opts.rate
+sleep_overhead = 0
+
+while True:
+    frame_start = time.time()
+    sim_recv(state)
+    a.update(state)
+    sim_send(a)
+    t = time.time()
+    frame_end = time.time()
+    if frame_end - frame_start < frame_time:
+        dt = frame_time - (frame_end - frame_start)
+        dt -= sleep_overhead
+        if dt > 0:
+            time.sleep(dt)
+        sleep_overhead = 0.99*sleep_overhead + 0.01*(time.time() - frame_end)

Tools/autotest/pysim/tracker.py

@@ -0,0 +1,99 @@
+#!/usr/bin/env python
+'''
+simple antenna tracker simulator core
+'''
+
+from aircraft import Aircraft
+import util, time, math
+from math import degrees, radians
+from rotmat import Vector3
+
+class Tracker(Aircraft):
+    '''a simple antenna tracker'''
+    def __init__(self,
+                 rate_controlled=False,
+                 pitch_range = 45,
+                 yaw_range = 180,
+                 zero_yaw = 270, # yaw direction at startup
+                 zero_pitch = 10, # pitch at startup
+                 turn_rate=20 # servo max turn rate in degrees/sec
+                 ):
+        Aircraft.__init__(self)
+        self.rate_controlled = rate_controlled
+        self.turn_rate = turn_rate
+        self.last_time = time.time()
+        self.pitch_range = pitch_range
+        self.yaw_range = yaw_range
+        self.zero_yaw = zero_yaw
+        self.zero_pitch = zero_pitch
+        self.verbose = False
+        self.last_debug = time.time()
+        self.pitch_current = 0
+        self.yaw_current = 0
+
+    def slew_limit(self, current, target, range, delta_time):
+        '''limit speed of servo movement'''
+        dangle = self.turn_rate * delta_time
+        dv = dangle / range
+        if target - current > dv:
+            return current + dv
+        if target - current < -dv:
+            return current - dv
+        return target
+        
+
+    def update(self, state):
+        # how much time has passed?
+        t = time.time()
+        delta_time = t - self.last_time
+        self.last_time = t
+
+        self.pitch_current = self.slew_limit(self.pitch_current, state.pitch_input, self.pitch_range, delta_time)
+        self.yaw_current = self.slew_limit(self.yaw_current, state.yaw_input, self.yaw_range, delta_time)
+
+        pitch_target = self.zero_pitch + self.pitch_current*self.pitch_range
+        yaw_target = self.zero_yaw + self.yaw_current*self.yaw_range
+        while yaw_target > 180:
+            yaw_target -= 360
+
+        (r,p,y) = self.dcm.to_euler()
+        pitch_current = degrees(p)
+        yaw_current = degrees(y)
+        roll_current = degrees(r)
+
+        pitch_rate = pitch_target - pitch_current
+        pitch_rate = min(self.turn_rate, pitch_rate)
+        pitch_rate = max(-self.turn_rate, pitch_rate)
+
+        yaw_diff = yaw_target - yaw_current
+        if yaw_diff > 180:
+            yaw_diff -= 360
+        if yaw_diff < -180:
+            yaw_diff += 360            
+        yaw_rate = yaw_diff
+        yaw_rate = min(self.turn_rate, yaw_rate)
+        yaw_rate = max(-self.turn_rate, yaw_rate)
+
+        # keep it level
+        roll_rate = 0 - roll_current
+
+        if time.time() - self.last_debug > 2:
+            self.last_debug = time.time()
+            print("roll=%.1f/%.1f pitch=%.1f/%.1f yaw=%.1f/%.1f rates=%.1f/%.1f/%.1f" % (
+                roll_current, 0,
+                pitch_current, pitch_target,
+                yaw_current, yaw_target,
+                roll_rate, pitch_rate, yaw_rate))
+
+        self.gyro = Vector3(radians(roll_rate),radians(pitch_rate),radians(yaw_rate))
+
+        # update attitude
+        self.dcm.rotate(self.gyro * delta_time)
+        self.dcm.normalize()
+
+        accel_earth = Vector3(0, 0, -self.gravity)
+        self.accel_body = self.dcm.transposed() * accel_earth
+
+        # new velocity vector
+        self.velocity = Vector3()
+        self.update_position(delta_time)