-
Notifications
You must be signed in to change notification settings - Fork 3
/
Copy pathSimpleHel.jl
260 lines (245 loc) · 8.08 KB
/
SimpleHel.jl
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
# DESCRIPTION
# Simple model of a helicopter using static aerodynamic model for rotor,
# tail rotor, horzontal and vertical tail, and fuselage.
#
# INPUT
# - state: state vector
# - ctrls: stick control inputs
#
# OUTPUT
# - dstate: state derivative
# - FM: total forces and moments
#
# ------------------------------------------------------------------------------
function SimpleHel!(state,sctrls)
# unpack states
u=state[1]
v=state[2]
w=state[3]
p=state[4]
q=state[5]
r=state[6]
phi=state[7]
theta=state[8]
psi=state[9]
x=state[10]
y=state[11]
z=state[12]
# convert to stick inputs to swashplate and TR collective inputs
ctrls = control_mixing(sctrls)
# unpack controls
theta1c=ctrls[1]*pi/180
theta1s=ctrls[2]*pi/180
theta0=ctrls[3]*pi/180
theta0tr=ctrls[4]*pi/180
# ----------------------------- MAIN ROTOR -------------------------------
# hub velocities
Vh=[u;v;w]+cross([p;q;r],rMR)
uh=Vh[1]
vh=Vh[2]
wh=Vh[3]
# advance ratios
Vinplane=sqrt(uh^2+vh^2)
mu=Vinplane/(OMEGA*RADIUS)
muz=wh/(OMEGA*RADIUS)
mu2=mu*mu
# get wind axis transformation
psiw=atan2(vh,uh)
cpsiw=cos(psiw)
spsiw=sin(psiw)
Th2w=[ cpsiw spsiw
-spsiw cpsiw]
Tw2h=Th2w'
# non-dimensional angular rates in wind axes
temp=Th2w*[p; q]
phw=temp[1]/OMEGA
qhw=temp[2]/OMEGA
# cyclic pitch translated to wind axes
temp=Th2w*[theta1s;theta1c]
theta1sw=temp[1]
theta1cw=temp[2]
# CT and lambda0 iteration, solves for inflow and thrust coefficient
# first guess
lambda0=0.05
# set tolerance greater than error
delta_lambda=1
iter=0
function inflow_MR()
while (abs(delta_lambda)>1e-10 && iter<200)
CT=0.5*A0*SOL*((1/3+0.5*mu2)*theta0+0.5*mu*(theta1sw+0.5*phw)+
0.5*(muz-lambda0)+0.25*(1+mu2)*TWIST)
lamtot2=mu2+(lambda0-muz)^2
delta_lambda=-(2.0*lambda0*sqrt(lamtot2)-CT)*lamtot2/(2.0*lamtot2^1.5+0.25*
A0*SOL*lamtot2-CT*(muz-lambda0))
lambda0=lambda0+0.5*delta_lambda
iter=iter+1
end
return CT
end
CT=inflow_MR()
# thrust
T=CT*RHO*OMEGA^2*pi*RADIUS^4
# uniform inflow assumption in this model
lambda1cw=0.0
lambda1sw=0.0
# quasi-Steady flapping in wind axes (Padfield p. 107)
Abt = [-8/3*mu*(1+0.5*mu2) -2*mu*(1+0.5*mu2) -(1+2*mu^2) 0.0
-GAMMA/6*mu*(1+0.5*mu2) -2*GAMMA*mu/15*(1+mu2/3) -2*GAMMA*mu2/9.0 (1-0.5*mu2*mu2)]
Abl = [-2*mu*(1+0.5*mu2) (1+0.5*mu^2) 0.0
-2*GAMMA*mu/9.0*(1-0.5*mu2) GAMMA/9.0*mu^2+0.5*GAMMA/9.0*mu2 -(1.0-0.5*mu2) ]
Abo = [-(1+0.5*mu^2) 16/GAMMA*(1+0.5*mu2)
16/GAMMA*(1.0-0.5*mu2)+GAMMA/9.0*mu^2 (1.0-0.5*mu2)]
#
temp=Abt*[theta0; TWIST; theta1sw; theta1cw]+Abl*
[muz-lambda0; lambda1sw; lambda1cw]+Abo*[phw; qhw]
beta1cw=temp[1]
beta1sw=temp[2]
# convert back to hub system
temp=Tw2h*[beta1sw; beta1cw]
beta1s=temp[1]
beta1c=temp[2]
# torque
delta=DELTA0+DELTA2*CT*CT
CQ=-(muz-lambda0)*CT+0.125*delta*SOL*(1+(7/3)*mu^2)
Q=CQ*RHO*OMEGA^2*pi*RADIUS^5
# translate rotor forces and moments to CG
Xr=T*beta1c
Yr=-T*beta1s
Zr=-T
MvecMR=[0.0; 0.0; Q]+cross(rMR, [Xr; Yr; Zr])
Lr=MvecMR[1]
Mr=MvecMR[2]
Nr=MvecMR[3]
# ------------------------------ FUSELAGE --------------------------------
# velocity vector
Vf=[u; v; w]
# drag force vector
Ff=-0.5*RHO*sqrt(Vf'*Vf)*Vf*CDSF
# transform to body forces
Xf=Ff[1]
Yf=Ff[2]
Zf=Ff[3]
MvecF=cross(rFS,[Xf;Yf;Zf])
Lf=MvecF[1]
Mf=MvecF[2]
Nf=MvecF[3]
# ----------------------- HORIZONTAL STABILIZER --------------------------
# local angle of attack
Vht=[u; v; w]+cross([p; q; r], rHT)
uht=Vht[1]
wht=Vht[3]
alphaht=atan2(wht,uht)
# lift force
CLht=AHT*alphaht
LIFTht=0.5*RHO*(Vht'*Vht)*SHT*CLht
# transform to body forces at CG
Xht=0.0
Yht=0.0
Zht=-LIFTht
MvecHT=cross(rHT,[Xht;Yht;Zht])
Lht=MvecHT[1]
Mht=MvecHT[2]
Nht=MvecHT[3]
# ----------------------- VERTICAL STABILIZER --------------------------
# local angle of attack
Vvt=[u; v; w]+cross([p; q; r], rVT)
uvt=Vvt[1]
vvt=Vvt[2]
alphavt=atan2(vvt,uvt)
# lift force
CLvt=AVT*alphavt
LIFTvt=0.5*RHO*(Vvt'*Vvt)*SVT*CLvt
# transform to body forces at CG
Xvt=0.0
Yvt=LIFTvt
Zvt=0.0
MvecVT=cross(rVT,[Xvt;Yvt;Zvt])
Lvt=MvecVT[1]
Mvt=MvecVT[2]
Nvt=MvecVT[3]
# ----------------------------- TAIL ROTOR -------------------------------
# local velocities at tail rotor hub in body CS
Vtr=[u;v;w]+cross([p;q;r],rTR)
# transfrom to TR coordinate system
Ttrg=[1.0 0.0 0.0
0.0 0.0 1.0
0.0 -1.0 0.0]
Vtr_tr=Ttrg*Vtr
utr=Vtr_tr[1]
vtr=Vtr_tr[2]
wtr=Vtr_tr[3]
mutr=sqrt(utr^2+vtr^2)/(OMEGTR*RTR)
muztr=wtr/(OMEGTR*RTR)
# CT and lambda0 iteration for tail rotor
# first guess
lambda0tr=0.05
#set tolerance greater than error
delta_lambda=1
iter=0
function inflow_TR()
while(abs(delta_lambda)>1e-10 && iter<200)
CTtr=0.5*A0TR*SOLTR*((1/3+0.5*mutr^2)*theta0tr+0.5*(muztr-lambda0tr)+
0.25*(1+mutr^2)*TWISTTR)
lamtot2=mutr^2+(lambda0tr-muztr)^2
delta_lambda=-(2.0*lambda0tr*sqrt(lamtot2)-CTtr)*lamtot2/(2.0*lamtot2^1.5+
0.25*A0TR*SOLTR*lamtot2-CTtr*(muztr-lambda0tr))
lambda0tr=lambda0tr+0.5*delta_lambda
iter=iter+1
end
return CTtr
end
CTtr=inflow_TR()
# tail rotor thrust
Ttr=CTtr*RHO*OMEGTR^2*pi*RTR^4
# force vector in body coordinates
Ftr=Ttrg'*[0.0;0.0;-Ttr]
Xtr=Ftr[1]
Ytr=Ftr[2]
Ztr=Ftr[3]
# resolve force and moment about CG
MvecTR=cross(rTR,Ftr)
Ltr=MvecTR[1]
Mtr=MvecTR[2]
Ntr=MvecTR[3]
# ------------------------ EQUATIONS OF MOTION ---------------------------
# sum forces and moments
X=Xr+Xf+Xtr+Xht+Xvt
Y=Yr+Yf+Ytr+Yht+Yvt
Z=Zr+Zf+Ztr+Zht+Zvt
L=Lr+Lf+Ltr+Lht+Lvt
M=Mr+Mf+Mtr+Mht+Mvt
N=Nr+Nf+Ntr+Nht+Nvt
# total forces and moments
FM=zeros(6,1)
FM=[X;Y;Z;L;M;N]
# EQUATIONS OF MOTION
# trigonometric functions
cphi=cos(phi)
sphi=sin(phi)
cthe=cos(theta)
sthe=sin(theta)
cpsi=cos(psi)
spsi=sin(psi)
# declare state derivative vector
dstate=zeros(12,1)
# equations of motion
# translational dynamics
dstate[1]=X/mass-g*sthe-q*w+r*v
dstate[2]=Y/mass+g*cthe*sphi-r*u+p*w
dstate[3]=Z/mass+g*cthe*cphi-p*v+q*u
# rotational dynamics
gam=Ix*Iz-Ixz^2
dstate[4]=(Iz*L+Ixz*N+Ixz*(Ix-Iy+Iz)*p*q-(Iz^2-Iy*Iz+Ixz^2)*q*r)/gam
dstate[5]=(M+(Iz-Ix)*p*r-Ixz*(p^2-r^2))/Iy
dstate[6]=(Ix*N+Ixz*L-Ixz*(Ix-Iy+Iz)*q*r+(Ix^2-Ix*Iy+Ixz^2)*p*q)/gam
# rotational kinematics
dstate[7]=p+q*sphi*sthe/cthe+r*cphi*sthe/cthe
dstate[8]=q*cphi-r*sphi
dstate[9]=q*sphi/cthe+r*cphi/cthe
# position
dstate[10]=u*cthe*cpsi+v*(sphi*sthe*cpsi-cphi*spsi)+w*(cphi*sthe*cpsi+sphi*spsi)
dstate[11]=u*cthe*spsi+v*(sphi*sthe*spsi+cphi*cpsi)+w*(cphi*sthe*spsi-sphi*cpsi)
dstate[12]=-u*sthe+v*sphi*cthe+w*cphi*cthe
#
return dstate, FM
end