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

autotest: make rover sim closer to Toms slash

Browse Source
This commit is contained in:
Andrew Tridgell 2013-05-27 11:08:07 +10:00
parent 761bd75a3a
commit 7456493b34
1 changed files with 15 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -11,32 +11,35 @@ from rotmat import Vector3, Matrix3
class Rover(Aircraft):
'''a simple rover'''
def __init__(self,
max_speed=13,
max_accel=9,
max_speed=20,
max_accel=30,
wheelbase=0.335,
wheeltrack=0.296,
max_wheel_turn=35,
turning_circle=1.8,
skid_steering=False):
Aircraft.__init__(self)
self.max_speed = max_speed
self.max_accel = max_accel
self.turning_circle = turning_circle
self.wheelbase = wheelbase
self.wheeltrack = wheeltrack
self.max_wheel_turn = max_wheel_turn
self.last_time = time.time()
self.skid_steering = skid_steering
def turn_circle(self, steering_angle):
'''return turning circle (diameter) in meters for steering_angle in degrees
def turn_circle(self, steering):
'''return turning circle (diameter) in meters for steering angle proportion in degrees
'''
theta = radians(steering_angle)
return (self.wheeltrack/2) + (self.wheelbase/sin(theta))
def yaw_rate(self, steering_angle, speed):
'''return yaw rate in degrees/second given steering_angle and speed'''
if abs(steering_angle) < 1.0e-6 or abs(speed) < 1.0e-6:
if abs(steering) < 1.0e-6:
return 0
d = self.turn_circle(steering_angle)
return self.turning_circle * sin(radians(35)) / sin(radians(steering*35))
def yaw_rate(self, steering, speed):
'''return yaw rate in degrees/second given steering_angle and speed'''
if abs(steering) < 1.0e-6 or abs(speed) < 1.0e-6:
return 0
d = self.turn_circle(steering)
c = pi * d
t = c / speed
rate = 360.0 / t
@ -84,7 +87,7 @@ class Rover(Aircraft):
speed = velocity_body.x
# yaw rate in degrees/s
yaw_rate = self.yaw_rate(self.max_wheel_turn * steering, speed)
yaw_rate = self.yaw_rate(steering, speed)
# target speed with current throttle
target_speed = throttle * self.max_speed