aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

autotest: added skid steering support to rover simulator

Browse Source 
Very rough initial implementation. Needs to be tweaked for more
realistic behaviour later.

thanks to Greg Brill for pointers on this.
This commit is contained in:
Andrew Tridgell 2013-03-15 09:52:32 +11:00
parent 5ddbad6ccb
commit 6ea2130342
2 changed files with 19 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Tools/autotest/pysim/rover.py
View File

@ -13,14 +13,27 @@ class Rover(Aircraft):
def __init__(self, def __init__(self,
max_speed=10, max_speed=10,
max_accel=10, max_accel=10,
max_turn_rate=45): max_turn_rate=45,
skid_steering=False):
Aircraft.__init__(self) Aircraft.__init__(self)
self.max_speed = max_speed self.max_speed = max_speed
self.max_accel = max_accel self.max_accel = max_accel
self.max_turn_rate = max_turn_rate self.max_turn_rate = max_turn_rate
self.last_time = time.time() self.last_time = time.time()
self.skid_steering = skid_steering
def update(self, state): def update(self, state):
# if in skid steering mode the steering and throttle values are used for motor1 and motor2
if self.skid_steering:
motor1 = state.steering
motor2 = state.throttle
steering = motor2 - motor1
throttle = 0.5*(motor1 + motor2)
else:
steering = state.steering
throttle = state.throttle
# how much time has passed? # how much time has passed?
t = time.time() t = time.time()
delta_time = t - self.last_time delta_time = t - self.last_time
@ -33,10 +46,10 @@ class Rover(Aircraft):
speed = velocity_body.x speed = velocity_body.x
# yaw rate in degrees/s # yaw rate in degrees/s
yaw_rate = self.max_turn_rate * state.steering * (speed / self.max_speed) yaw_rate = self.max_turn_rate * steering * (speed / self.max_speed)
# target speed with current throttle # target speed with current throttle
target_speed = state.throttle * self.max_speed target_speed = throttle * self.max_speed
# linear acceleration in m/s/s - very crude model # linear acceleration in m/s/s - very crude model
accel = self.max_accel * (target_speed - speed) / self.max_speed accel = self.max_accel * (target_speed - speed) / self.max_speed
@ -76,3 +89,4 @@ class Rover(Aircraft):
# update lat/lon/altitude # update lat/lon/altitude
self.update_position(delta_time) self.update_position(delta_time)

3
Tools/autotest/pysim/sim_rover.py
View File

@ -76,6 +76,7 @@ parser.add_option("--simin", dest="simin", help="SIM input (IP:port)",
parser.add_option("--simout", dest="simout", help="SIM output (IP:port)", default="127.0.0.1:5501") 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("--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("--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() (opts, args) = parser.parse_args()
@ -102,7 +103,7 @@ sim_out.connect(sim_out_address)
sim_out.setblocking(0) sim_out.setblocking(0)
# create the quadcopter model # create the quadcopter model
a = Rover() a = Rover(skid_steering=opts.skid_steering)
# initial controls state # initial controls state
state = ControlState() state = ControlState()