From ca3fa1a469872847455587783a4bae24f1893c58 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Nov 2012 00:07:15 +1100 Subject: [PATCH] autotest: first version of a rover simulator in python --- Tools/autotest/pysim/rover.py | 78 ++++++++++++++++ Tools/autotest/pysim/sim_rover.py | 145 ++++++++++++++++++++++++++++++ Tools/autotest/sim_rover.sh | 25 ++++++ 3 files changed, 248 insertions(+) create mode 100644 Tools/autotest/pysim/rover.py create mode 100755 Tools/autotest/pysim/sim_rover.py create mode 100755 Tools/autotest/sim_rover.sh diff --git a/Tools/autotest/pysim/rover.py b/Tools/autotest/pysim/rover.py new file mode 100644 index 0000000000..bb1e42dfb5 --- /dev/null +++ b/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) diff --git a/Tools/autotest/pysim/sim_rover.py b/Tools/autotest/pysim/sim_rover.py new file mode 100755 index 0000000000..695d79489e --- /dev/null +++ b/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) diff --git a/Tools/autotest/sim_rover.sh b/Tools/autotest/sim_rover.sh new file mode 100755 index 0000000000..bfb007356a --- /dev/null +++ b/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