diff --git a/Tools/autotest/pysim/helicopter.py b/Tools/autotest/pysim/helicopter.py index 2ac528f512..a355015844 100644 --- a/Tools/autotest/pysim/helicopter.py +++ b/Tools/autotest/pysim/helicopter.py @@ -8,10 +8,10 @@ from rotmat import Vector3, Matrix3 class HeliCopter(Aircraft): '''a HeliCopter simulator''' def __init__(self, frame='+', - hover_throttle=0.35, - terminal_velocity=15.0, + hover_throttle=0.65, + terminal_velocity=40.0, frame_height=0.1, - mass=1.5): + mass=2.5): Aircraft.__init__(self) self.mass = mass # Kg self.hover_throttle = hover_throttle @@ -19,10 +19,10 @@ class HeliCopter(Aircraft): self.terminal_rotation_rate = 4*radians(360.0) self.frame_height = frame_height self.last_time = time.time() - self.roll_rate_max = radians(360) - self.pitch_rate_max = radians(360) + self.roll_rate_max = radians(720) + self.pitch_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 def update(self, servos): @@ -48,6 +48,10 @@ class HeliCopter(Aircraft): #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, pitch_rate*self.pitch_rate_max, yaw_rate*self.yaw_rate_max) @@ -55,7 +59,7 @@ class HeliCopter(Aircraft): # rotational air resistance 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.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 self.gyro += rot_accel * delta_time