Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

autotest: added some lateral thrust to helicopter simulator

Browse Source
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 from aircraft import Aircraft
import util, time, math import util, time, math
from math import degrees, radians from math import degrees, radians, sin, cos
from rotmat import Vector3, Matrix3 from rotmat import Vector3, Matrix3
class HeliCopter(Aircraft): class HeliCopter(Aircraft):
@ -11,7 +11,10 @@ class HeliCopter(Aircraft):
hover_throttle=0.65, hover_throttle=0.65,
terminal_velocity=40.0, terminal_velocity=40.0,
frame_height=0.1, 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) Aircraft.__init__(self)
self.mass = mass # Kg self.mass = mass # Kg
self.hover_throttle = hover_throttle self.hover_throttle = hover_throttle
@ -19,11 +22,15 @@ 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(720) self.roll_rate_max = radians(1400)
self.pitch_rate_max = radians(720) self.pitch_rate_max = radians(1400)
self.yaw_rate_max = radians(720) self.yaw_rate_max = radians(1400)
self.rsc_setpoint = 0.8 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
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): def update(self, servos):
# how much time has passed? # how much time has passed?
@ -48,9 +55,11 @@ 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 rsc_scale = rsc/self.rsc_setpoint
pitch_rate *= rsc/self.rsc_setpoint
yaw_rate *= 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, rot_accel = Vector3(roll_rate*self.roll_rate_max,
pitch_rate*self.pitch_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.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 -= 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 # update rotational rates in body frame
self.gyro += rot_accel * delta_time self.gyro += rot_accel * delta_time
@ -73,7 +84,7 @@ class HeliCopter(Aircraft):
thrust *= self.thrust_scale 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 = self.dcm * accel_body
accel_earth += Vector3(0, 0, self.gravity) accel_earth += Vector3(0, 0, self.gravity)
accel_earth += air_resistance accel_earth += air_resistance