-
Notifications
You must be signed in to change notification settings - Fork 0
/
Functions.py
172 lines (159 loc) · 6.59 KB
/
Functions.py
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
# -*- coding: utf-8 -*-
"""
Created on Thu Apr 6 10:49:00 2017
@author: uqjchai2
"""
import math
import numpy as n
from Constants import *
#-----------------------------------------------------------------------------
# Module: Pitch Rate Control Law
# No actuator dynamics modelled (assume instant from command to deflection)
#-----------------------------------------------------------------------------
def pitchrate_control(theta,thetac,thetadot,Kp,Kq):
#-- ENTER CODE HERE
de = Kp*(thetac-theta) + Kq*thetadot;
return de
#----------------------------------------------------------------------------
# Module: 1st order time lag, discretised using forward euler
# May be used to simulate actuator dynamics
# Y/U = 1/(1+Ts)
#----------------------------------------------------------------------------
def time_lag_1(T,uo,yo,dt):
y1 = (uo-yo)*dt/T + yo
return y1
#-----------------------------------------------------------------------------
# Module: State Space Dynamics Lateral x = [phidot psidot beta phi psi ]
# Returns state dynamics xdot = Ax + Bu
#-----------------------------------------------------------------------------
def state_space_lat(xo,dar):
phido, psido, betao, phio, psio = xo
x = n.matrix([[phido],[psido],[betao],[phio],[psio]])
A = n.matrix([[b_2U*Clp/Ix_Sqb, b_2U*Clr/Ix_Sqb, Clb/Ix_Sqb, 0, 0],
[b_2U*Cnp/Iz_Sqb, b_2U*Cnr/Iz_Sqb, Cnb/Iz_Sqb, 0, 0],
[b_2U*Cyp/mU_Sq, (b_2U*Cyr-mU_Sq)/mU_Sq, Cyb/mU_Sq, Cyph/mU_Sq, Cyps/mU_Sq],
[1, 0, 0, 0, 0],
[0, 1, 0, 0, 0]])
B = n.matrix([[Cldar/Ix_Sqb], [Cndar/Iz_Sqb], [Cydar/mU_Sq], [0], [0]])
xdot = n.dot(A,x) + B*dar
return float(xdot[0]), float(xdot[1]), float(xdot[2]), float(xdot[3]), float(xdot[4])
#-----------------------------------------------------------------------------
# Module: State Space Dynamics Longitudinal x = [u alpha thetadot theta]
# Assumes no downwash lag for Cx and Cm
# returns state dynamics xdot = Ax + Bu
#-----------------------------------------------------------------------------
def state_space(xo,de):
uo, ao, thetadoto, thetao = xo
x = n.matrix([[uo],[ao],[thetadoto],[thetao]])
A = n.matrix([[Cxu/mU_Sq,Cxa/mU_Sq,(c/(2*U))*Cxq/mU_Sq,Cw*math.cos(Theta)/mU_Sq],
[Czu/(mU_Sq+c*Czad/(2*U)),Cza/(mU_Sq+c*Czad/(2*U)),(mU_Sq+(c/(2*U))*Czq)/(mU_Sq+c*Czad/(2*U)),Cw*math.sin(Theta)/(mU_Sq+c*Czad/(2*U))],
[Cmu/I_Sqc,Cma/I_Sqc,(c/(2*U))*Cmq/I_Sqc,0/I_Sqc],
[0,0,1,0]])
B = n.matrix([[0],[Czde/mU_Sq],[Cmde/I_Sqc],[0]])
xdot = n.dot(A,x) + B*de
return float(xdot[0]), float(xdot[1]), float(xdot[2]), float(xdot[3])
#-----------------------------------------------------------------------------
# Module: Kinematics
# Takes state dynamics, current state and time step size
# returns new state
#-----------------------------------------------------------------------------
def kinematics(xdot,xo,dt):
# xdot = state dynamics
# dt = timestep
# unpack state dynamics
if len(xdot) == 4:
# longitudinal
udot, adot, thetadotdot, thetadot = xdot
uo, ao, thetadoto, thetao = xo
# use euler 1st order
u1 = udot*dt + uo
a1 = adot*dt + ao
thetadot1 = thetadotdot*dt + thetadoto
theta1 = thetadot*dt + thetao
x = u1, a1, thetadot1, theta1
if len(xdot) == 5:
# lateral
phidotdot, psidotdot, bdot, phidot, psidot = xdot
phidoto, psidoto, bo, phio, psio = xo
# euler 1st order
phidot1 = phidotdot*dt + phidoto
psidot1 = psidotdot*dt + psidoto
b1 = bdot*dt + bo
phi1 = phidot*dt + phio
psi1 = psidot*dt + psio
x = phidot1, psidot1, b1, phi1, psi1
return x
#------------------------------------------------------------------------------
# Finite difference: central difference tool
#
#-----------------------------------------------------------------------------
def central_diff(f,x1,x2,dx,independent='x1'):
if independent == 'x1':
yn = f(x1+dx,x2)
yo = f(x1-dx,x2)
return (yn-yo)/(2*dx)
if independent == 'x2':
yn = f(x1,x2+dx)
yo = f(x1,x2-dx)
return (yn-yo)/(2*dx)
#------------------------------------------------------------------------------
# gravity
#------------------------------------------------------------------------------
def gravity(y):
return 9.81*(6371000/(6371000+y))**2
#------------------------------------------------------------------------------
# Atmosphere Model
#------------------------------------------------------------------------------
def rhoinf(y): # Atmospheric density
# y is altitude
# US standard atmosphere 1976, NASA
if y <= 11000:
return 1.225*(1-y/44329)**4.255876
if 11000 < y <= 20000:
return 1.225*(0.297076)*math.exp((10999-y)/6341.4)
if 20000 < y <= 32000:
return 1.225*(0.978261 + y/201010)**(-35.16319)
if 32000 < y <= 47000:
return 1.225*(0.857003 + y/57944)**(-13.20114)
if 47000 < y <= 51000:
return 1.225*(0.00116533)*math.exp((46998-y)/7922)
if 51000 < y <= 71000:
return 1.225*(0.79899 - y/184800)**(11.20114)
else:
return 1.225*(0.79899 - y/184800)**(11.20114)
def Tinf(y): # Atmospheric Temperature
# y is altitude
# US standard atmosphere 1976, NASA
if y <= 11000:
return 288.15*(1-y/44329)
if 11000 < y <= 20000:
return 0.751865*288.75
if 20000 < y <= 32000:
return 288.15*(0.682457 + y/288136)
if 32000 < y <= 47000:
return 288.15*(0.482561+y/102906)
if 47000 < y <= 51000:
return 288.15*0.939268
if 51000 < y <= 71000:
return 288.15*(1.434843 - y/102906)
else:
print("Error: altitude not supported")
def Pinf(y): # Atmospheric Pressure
# y is altitude
# US standard atmosphere 1976, NASA
if y <= 11000:
return 101325*(1-y/44329)**5.255876
if 11000 < y <= 20000:
return 101325*(0.223361)*math.exp((10999-y)/6341.4)
if 20000 < y <= 32000:
yl = [20000,25000,30000]
Pl = [5473.75,2549.0,1197.0]
return n.interp(y,yl,Pl)
if 32000 < y <= 47000:
return 101325*(0.898309 + y/55280)**(-12.20114)
if 47000 < y <= 51000:
return 101325*(0.00109456)*math.exp((46998-y)/7922)
if 51000 < y <= 71000:
return 101325*(0.838263 - y/176142)**12.20114
else:
print("Error: altitude not supported")