autotest: convert rover sim to use sim_wrapper.py
This commit is contained in:
Andrew Tridgell
parent
dbeaccc3d9
commit
ffd4662ec4
@ -11,6 +11,7 @@ from rotmat import Vector3, Matrix3
|
||||
class Rover(Aircraft):
|
||||
'''a simple rover'''
|
||||
def __init__(self,
|
||||
frame='',
|
||||
max_speed=20,
|
||||
max_accel=30,
|
||||
wheelbase=0.335,
|
||||
@ -72,17 +73,17 @@ class Rover(Aircraft):
|
||||
steer = 0.5 * lat_accel * mincircle / (speed**2)
|
||||
return steer * 35
|
||||
|
||||
def update(self, state):
|
||||
def update(self, servos):
|
||||
|
||||
# if in skid steering mode the steering and throttle values are used for motor1 and motor2
|
||||
if self.skid_steering:
|
||||
motor1 = state.steering # left motor
|
||||
motor2 = state.throttle # right motor
|
||||
motor1 = 2*(servos[0]-0.5)
|
||||
motor2 = 2*(servos[2]-0.5)
|
||||
steering = motor1 - motor2
|
||||
throttle = 0.5*(motor1 + motor2)
|
||||
else:
|
||||
steering = state.steering
|
||||
throttle = state.throttle
|
||||
steering = 2*(servos[0]-0.5)
|
||||
throttle = 2*(servos[2]-0.5)
|
||||
|
||||
# how much time has passed?
|
||||
t = self.time_now
|
||||
|
||||
@ -1,156 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
'''
|
||||
simple rover simulator
|
||||
'''
|
||||
|
||||
from rover import Rover
|
||||
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()
|
||||
|
||||
timestamp = int((a.time_now - a.time_base) * 1e6)
|
||||
|
||||
buf = struct.pack('<Q17dI',
|
||||
timestamp,
|
||||
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:
|
||||
print(e, e.error)
|
||||
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=1000)
|
||||
parser.add_option("--skid-steering", action='store_true', default=False, help="Use skid steering")
|
||||
parser.add_option("--speedup", type='float', default=1.0, help="speedup from realtime")
|
||||
|
||||
|
||||
(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(1)
|
||||
|
||||
# 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(skid_steering=opts.skid_steering)
|
||||
|
||||
# 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 rate=%.1f" % (
|
||||
a.home_latitude,
|
||||
a.home_longitude,
|
||||
a.altitude,
|
||||
a.yaw,
|
||||
opts.rate))
|
||||
|
||||
a.setup_frame_time(opts.rate, opts.speedup)
|
||||
|
||||
counter = 0
|
||||
|
||||
while True:
|
||||
# receive control input from SITL
|
||||
sim_recv(state)
|
||||
|
||||
# update the model
|
||||
a.update(state)
|
||||
|
||||
# send model state to SITL
|
||||
sim_send(a)
|
||||
|
||||
a.sync_frame_time()
|
||||
a.time_advance(a.frame_time)
|
||||
|
||||
counter += 1
|
||||
if counter == 1000:
|
||||
print("t=%f speedup=%.1f" % ((a.time_now - a.time_base), a.achieved_rate/a.rate))
|
||||
counter = 0
|
||||
@ -144,6 +144,10 @@ elif opts.frame.startswith('CRRCSim'):
|
||||
from crrcsim import CRRCSim
|
||||
a = CRRCSim(frame=opts.frame)
|
||||
frame_rate = 360
|
||||
elif opts.frame.startswith('rover'):
|
||||
from rover import Rover
|
||||
a = Rover(frame=opts.frame)
|
||||
frame_rate = 360
|
||||
else:
|
||||
from multicopter import MultiCopter
|
||||
a = MultiCopter(frame=opts.frame)
|
||||
|
||||
@ -173,6 +173,10 @@ set -x
|
||||
VEHICLE=$(basename $PWD)
|
||||
}
|
||||
|
||||
[ -z "$FRAME" -a "$VEHICLE" = "APMrover2" ] && {
|
||||
FRAME="rover"
|
||||
}
|
||||
|
||||
EXTRA_PARM=""
|
||||
EXTRA_SIM=""
|
||||
|
||||
@ -223,8 +227,8 @@ case $FRAME in
|
||||
EXTRA_PARM="param set VTAIL_OUTPUT 4;"
|
||||
EXTRA_SIM="$EXTRA_SIM --vtail"
|
||||
;;
|
||||
skid)
|
||||
EXTRA_SIM="$EXTRA_SIM --skid-steering"
|
||||
rover|rover-skid)
|
||||
EXTRA_SIM="$EXTRA_SIM --frame=$FRAME"
|
||||
;;
|
||||
obc)
|
||||
BUILD_TARGET="sitl-obc"
|
||||
@ -341,7 +345,7 @@ EOF
|
||||
PARMS="copter_params.parm"
|
||||
;;
|
||||
APMrover2)
|
||||
RUNSIM="nice $autotest/pysim/sim_rover.py --home=$SIMHOME --rate=400 $EXTRA_SIM"
|
||||
RUNSIM="nice $autotest/pysim/sim_wrapper.py --home=$SIMHOME --simin=$SIMIN_PORT --simout=$SIMOUT_PORT --fgout=$FG_PORT $EXTRA_SIM"
|
||||
PARMS="Rover.parm"
|
||||
;;
|
||||
*)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user