You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
144 lines
4.2 KiB
144 lines
4.2 KiB
#!/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() |
|
|
|
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 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) |
|
parser.add_option("--skid-steering", action='store_true', default=False, help="Use skid steering") |
|
|
|
(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 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" % ( |
|
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)
|
|
|