Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13:56 -04:00
Code Issues 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): class Rover(Aircraft):
'''a simple rover''' '''a simple rover'''
def __init__(self, def __init__(self,
max_speed=13, max_speed=20,
max_accel=9, max_accel=30,
wheelbase=0.335, wheelbase=0.335,
wheeltrack=0.296, wheeltrack=0.296,
max_wheel_turn=35, max_wheel_turn=35,
turning_circle=1.8,
skid_steering=False): 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.turning_circle = turning_circle
self.wheelbase = wheelbase self.wheelbase = wheelbase
self.wheeltrack = wheeltrack self.wheeltrack = wheeltrack
self.max_wheel_turn = max_wheel_turn self.max_wheel_turn = max_wheel_turn
self.last_time = time.time() self.last_time = time.time()
self.skid_steering = skid_steering self.skid_steering = skid_steering
def turn_circle(self, steering_angle): def turn_circle(self, steering):
'''return turning circle (diameter) in meters for steering_angle in degrees '''return turning circle (diameter) in meters for steering angle proportion in degrees
''' '''
theta = radians(steering_angle) if abs(steering) < 1.0e-6:
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:
return 0 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 c = pi * d
t = c / speed t = c / speed
rate = 360.0 / t rate = 360.0 / t
@ -84,7 +87,7 @@ class Rover(Aircraft):
speed = velocity_body.x speed = velocity_body.x
# yaw rate in degrees/s # 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 with current throttle
target_speed = throttle * self.max_speed target_speed = throttle * self.max_speed