autotest: first version of a rover simulator in python

Tools/autotest/pysim/rover.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+'''
+simple rover simulator core
+'''
+
+from aircraft import Aircraft
+import util, time, math
+from math import degrees, radians
+from rotmat import Vector3, Matrix3
+
+class Rover(Aircraft):
+    '''a simple rover'''
+    def __init__(self,
+                 max_speed=10,
+                 max_accel=10,
+                 max_turn_rate=45):
+        Aircraft.__init__(self)
+        self.max_speed = max_speed
+        self.max_accel = max_accel
+        self.max_turn_rate = max_turn_rate
+        self.last_time = time.time()
+
+    def update(self, state):
+        # how much time has passed?
+        t = time.time()
+        delta_time = t - self.last_time
+        self.last_time = t
+
+        # speed in m/s in body frame
+        velocity_body = self.dcm.transposed() * self.velocity
+
+        # speed along x axis, +ve is forward
+        speed = velocity_body.x
+
+        # yaw rate in degrees/s
+        yaw_rate = self.max_turn_rate * state.steering * (speed / self.max_speed)
+
+        # target speed with current throttle
+        target_speed = state.throttle * self.max_speed
+
+        # linear acceleration in m/s/s - very crude model
+        accel = self.max_accel * (target_speed - speed) / self.max_speed
+
+#        print('speed=%f throttle=%f steering=%f yaw_rate=%f accel=%f' % (speed, state.throttle, state.steering, yaw_rate, accel))
+        
+        self.gyro = Vector3(0,0,radians(yaw_rate))
+
+        # update attitude
+        self.dcm.rotate(self.gyro * delta_time)
+        self.dcm.normalize()
+
+        # accel in body frame due to motor
+        accel_body = Vector3(accel, 0, 0)
+
+        # add in accel due to direction change
+        accel_body.y += radians(yaw_rate) * speed
+
+        # now in earth frame
+        accel_earth = self.dcm * accel_body
+        accel_earth += Vector3(0, 0, self.gravity)
+
+        # if we're on the ground, then our vertical acceleration is limited
+        # to zero. This effectively adds the force of the ground on the aircraft
+        accel_earth.z = 0
+
+        # work out acceleration as seen by the accelerometers. It sees the kinematic
+        # acceleration (ie. real movement), plus gravity
+        self.accel_body = self.dcm.transposed() * (accel_earth + Vector3(0, 0, -self.gravity))
+
+        # new velocity vector
+        self.velocity += accel_earth * delta_time
+
+        # new position vector
+        old_position = self.position.copy()
+        self.position += self.velocity * delta_time
+
+        # update lat/lon/altitude
+        self.update_position(delta_time)

Tools/autotest/pysim/sim_rover.py

@@ -0,0 +1,145 @@
+#!/usr/bin/env python
+'''
+simple rover simulator
+'''
+
+from rover import Rover
+import util, time, os, sys, math
+import socket, struct
+import select, errno
+
+sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', '..', '..', '..', 'mavlink', 'pymavlink'))
+
+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('<16dI',
+                      a.latitude, a.longitude, a.altitude, degrees(yaw),
+                      a.velocity.x, a.velocity.y,
+                      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),
+                      0x4c56414e)
+    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 steering and throttle to -1/1
+    state.steering = (pwm[0]-1500)/500.0
+    state.throttle = (pwm[2]-1500)/500.0
+
+#    print("steering=%f throttle=%f pwm=%s" % (state.steering, state.throttle, str(pwm)))
+    
+
+
+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):
+        # steering from -1 to 1, where -1 is left, 1 is right
+        self.steering = 0
+        # throttle from -1 to 1, where -1 is full reverse, 1 is full forward
+        self.throttle = 0
+
+##################
+# main program
+from optparse import OptionParser
+parser = OptionParser("sim_rover.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)
+
+(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)
+
+parent_pid = os.getppid()
+
+# 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 quadcopter model
+a = Rover()
+
+# 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
+
+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/sim_rover.sh

@@ -0,0 +1,25 @@
+#!/bin/bash
+
+set -x
+
+killall -q APMrover2.elf
+pkill -f sim_rover.py
+set -e
+
+autotest=$(dirname $(readlink -e $0))
+pushd $autotest/../../APMrover2
+make clean sitl
+
+tfile=$(tempfile)
+(
+echo r
+) > $tfile
+#gnome-terminal -e "gdb -x $tfile --args /tmp/APMrover2.build/APMrover2.elf"
+gnome-terminal -e /tmp/APMrover2.build/APMrover2.elf
+#gnome-terminal -e "valgrind --db-attach=yes -q /tmp/APMrover2.build/APMrover2.elf"
+sleep 2
+rm -f $tfile
+gnome-terminal -e "../Tools/autotest/pysim/sim_rover.py --home=-35.362938,149.165085,584,270 --rate=400"
+sleep 2
+popd
+mavproxy.py --aircraft=test --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --out 127.0.0.1:14550 --out 127.0.0.1:14551