zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

SITL: Initial improvements to tradheli simulation

mission-4.1.18
Robert Lefebvre 10 years ago committed by Randy Mackay
parent
12724e9556
commit
45a12646be
1 changed files with 11 additions and 7 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      Tools/autotest/pysim/helicopter.py

18
Tools/autotest/pysim/helicopter.py
Unescape View File

@ -8,10 +8,10 @@ from rotmat import Vector3, Matrix3
class HeliCopter(Aircraft): class HeliCopter(Aircraft):
'''a HeliCopter simulator''' '''a HeliCopter simulator'''
def __init__(self, frame='+', def __init__(self, frame='+',
hover_throttle=0.35, hover_throttle=0.65,
terminal_velocity=15.0, terminal_velocity=40.0,
frame_height=0.1, frame_height=0.1,
mass=1.5): mass=2.5):
Aircraft.__init__(self) Aircraft.__init__(self)
self.mass = mass # Kg self.mass = mass # Kg
self.hover_throttle = hover_throttle self.hover_throttle = hover_throttle
@ -19,10 +19,10 @@ class HeliCopter(Aircraft):
self.terminal_rotation_rate = 4*radians(360.0) self.terminal_rotation_rate = 4*radians(360.0)
self.frame_height = frame_height self.frame_height = frame_height
self.last_time = time.time() self.last_time = time.time()
self.roll_rate_max = radians(360) self.roll_rate_max = radians(720)
self.pitch_rate_max = radians(360) self.pitch_rate_max = radians(720)
self.yaw_rate_max = radians(720) self.yaw_rate_max = radians(720)
self.rsc_setpoint = 0.5 self.rsc_setpoint = 0.8
self.thrust_scale = (self.mass * self.gravity) / self.hover_throttle self.thrust_scale = (self.mass * self.gravity) / self.hover_throttle
def update(self, servos): def update(self, servos):
@ -48,6 +48,10 @@ class HeliCopter(Aircraft):
#print(swash1, swash2, swash3, roll_rate, pitch_rate, yaw_rate, thrust, rsc) #print(swash1, swash2, swash3, roll_rate, pitch_rate, yaw_rate, thrust, rsc)
roll_rate *= rsc/self.rsc_setpoint
pitch_rate *= rsc/self.rsc_setpoint
yaw_rate *= rsc/self.rsc_setpoint
rot_accel = Vector3(roll_rate*self.roll_rate_max, rot_accel = Vector3(roll_rate*self.roll_rate_max,
pitch_rate*self.pitch_rate_max, pitch_rate*self.pitch_rate_max,
yaw_rate*self.yaw_rate_max) yaw_rate*self.yaw_rate_max)
@ -55,7 +59,7 @@ class HeliCopter(Aircraft):
# rotational air resistance # rotational air resistance
rot_accel.x -= self.gyro.x * radians(5000.0) / self.terminal_rotation_rate rot_accel.x -= self.gyro.x * radians(5000.0) / self.terminal_rotation_rate
rot_accel.y -= self.gyro.y * radians(5000.0) / self.terminal_rotation_rate rot_accel.y -= self.gyro.y * radians(5000.0) / self.terminal_rotation_rate
rot_accel.z -= self.gyro.z * radians(400.0) / self.terminal_rotation_rate rot_accel.z -= self.gyro.z * radians(5000.0) / self.terminal_rotation_rate
# update rotational rates in body frame # update rotational rates in body frame
self.gyro += rot_accel * delta_time self.gyro += rot_accel * delta_time

Write Preview
Loading…
Cancel
Save