Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

autotest: added some lateral thrust to helicopter simulator

This commit is contained in:
Andrew Tridgell 2015-04-18 09:25:01 +10:00
parent 6b21aceed0
commit 633e7122e7
1 changed files with 20 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
Tools/autotest/pysim/helicopter.py
View File

@ -2,7 +2,7 @@
from aircraft import Aircraft
import util, time, math
from math import degrees, radians
from math import degrees, radians, sin, cos
from rotmat import Vector3, Matrix3
class HeliCopter(Aircraft):
@ -11,7 +11,10 @@ class HeliCopter(Aircraft):
hover_throttle=0.65,
terminal_velocity=40.0,
frame_height=0.1,
mass=2.5):
hover_lean=8.0,
yaw_zero=0.1,
rotor_rot_accel=radians(20),
mass=2.13):
Aircraft.__init__(self)
self.mass = mass # Kg
self.hover_throttle = hover_throttle
@ -19,11 +22,15 @@ 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(720)
self.pitch_rate_max = radians(720)
self.yaw_rate_max = radians(720)
self.roll_rate_max = radians(1400)
self.pitch_rate_max = radians(1400)
self.yaw_rate_max = radians(1400)
self.rsc_setpoint = 0.8
self.thrust_scale = (self.mass * self.gravity) / self.hover_throttle
self.rotor_rot_accel=rotor_rot_accel
# calculate lateral thrust ratio for tail rotor
self.tail_thrust_scale = sin(radians(hover_lean)) * self.thrust_scale / yaw_zero
def update(self, servos):
# how much time has passed?
@ -48,9 +55,11 @@ 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
rsc_scale = rsc/self.rsc_setpoint
roll_rate *= rsc_scale
pitch_rate *= rsc_scale
yaw_rate *= rsc_scale
rot_accel = Vector3(roll_rate*self.roll_rate_max,
pitch_rate*self.pitch_rate_max,
@ -61,6 +70,8 @@ class HeliCopter(Aircraft):
rot_accel.y -= self.gyro.y * radians(5000.0) / self.terminal_rotation_rate
rot_accel.z -= self.gyro.z * radians(5000.0) / self.terminal_rotation_rate
rot_accel.z += (rsc_scale+thrust) * self.rotor_rot_accel
# update rotational rates in body frame
self.gyro += rot_accel * delta_time
@ -73,7 +84,7 @@ class HeliCopter(Aircraft):
thrust *= self.thrust_scale
accel_body = Vector3(0, 0, -thrust / self.mass)
accel_body = Vector3(0, yaw_rate * rsc_scale * self.tail_thrust_scale, -thrust / self.mass)
accel_earth = self.dcm * accel_body
accel_earth += Vector3(0, 0, self.gravity)
accel_earth += air_resistance