ardupilot/Tools/autotest/pysim/sim_rover.py

157 lines
4.4 KiB
Python
Executable File

#!/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