SITL: Initial improvements to tradheli simulation

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