1
|
-- node __tan ( x : real )
|
2
|
-- returns ( out : real ) ;
|
3
|
|
4
|
-- let
|
5
|
-- out = if ( 0.000000 <= x and x <= 0.157080 ) then 0.000000 + ( 0.995893 * ( x - 0.000000 ) )
|
6
|
-- else if ( 0.157080 <= x and x <= 0.314159 ) then 0.156434 + ( 0.971371 * ( x - 0.157080 ) )
|
7
|
-- else if ( 0.314159 <= x and x <= 0.471239 ) then 0.309017 + ( 0.922930 * ( x - 0.314159 ) )
|
8
|
-- else if ( 0.471239 <= x and x <= 0.628319 ) then 0.453990 + ( 0.851764 * ( x - 0.471239 ) )
|
9
|
-- else if ( 0.628319 <= x and x <= 0.785398 ) then 0.587785 + ( 0.759624 * ( x - 0.628319 ) )
|
10
|
-- else if ( 0.785398 <= x and x <= 0.942478 ) then 0.707107 + ( 0.648781 * ( x - 0.785398 ) )
|
11
|
-- else if ( 0.942478 <= x and x <= 1.099557 ) then 0.809017 + ( 0.521962 * ( x - 0.942478 ) )
|
12
|
-- else if ( 1.099557 <= x and x <= 1.256637 ) then 0.891007 + ( 0.382290 * ( x - 1.099557 ) )
|
13
|
-- else if ( 1.256637 <= x and x <= 1.413717 ) then 0.951057 + ( 0.233205 * ( x - 1.256637 ) )
|
14
|
-- else if ( 1.413717 <= x and x <= 1.570796 ) then 0.987688 + ( 0.078378 * ( x - 1.413717 ) ) else - 1.000000;
|
15
|
-- tel
|
16
|
|
17
|
|
18
|
-- -- Interpolation for the tan function
|
19
|
-- node my_tan ( x : real ) returns ( out : real ) ;
|
20
|
-- let
|
21
|
-- out = if ( 0.000000 <= x and x <= 1.570796 ) then __tan ( x )
|
22
|
-- else if ( 1.570796 <= x and x <= 3.141593 ) then __tan ( 3.141593 - x )
|
23
|
-- else if ( 3.141593 <= x and x <= 4.712389 ) then - ( __tan ( x - 3.141593 ) )
|
24
|
-- else if ( 4.712389 <= x and x <= 6.283185 ) then - ( __tan ( 6.283185 - x ) ) else - 1.000000;
|
25
|
-- tel
|
26
|
|
27
|
-- An integrator with reset
|
28
|
node integrator_reset ( Fx : real ; ResetLevel : bool ; x0 : real ) returns (o : real ) ;
|
29
|
let
|
30
|
o = x0 -> if ( ResetLevel ) then x0 else ( Fx * 1.000000 ) + pre ( o );
|
31
|
tel
|
32
|
|
33
|
node ANGL_sign ( SigIn_Out1_137 : real ) returns ( Out2_In1_203 : real ) ;
|
34
|
var Switch1_Out1_182 : real ;
|
35
|
let
|
36
|
Switch1_Out1_182 = if 0.000000 = SigIn_Out1_137 then 0.000000 else - 1.000000;
|
37
|
Out2_In1_203 = if SigIn_Out1_137 > 0.000000 then 1.000000 else Switch1_Out1_182;
|
38
|
tel
|
39
|
|
40
|
|
41
|
node ANGL ( in_Out1_76 : real ) returns ( _180_In1_238 : real ) ;
|
42
|
var Abs_Out1_79 : real ;
|
43
|
Relational_Operator_ANGL_Out1_101 : bool ;
|
44
|
Sum6_Out1_111 : real ;
|
45
|
Switch1_ANGL_Out1_120 : real ;
|
46
|
sign_Out1_127 : real ;
|
47
|
times360_Out1_226 : real ;
|
48
|
Abs_In1_78 : real ;
|
49
|
Relational_Operator_ANGL_In1_99 : real ;
|
50
|
Sum6_In1_109 : real ;
|
51
|
Sum6_In2_110 : real ;
|
52
|
Switch1_ANGL_In1_117 : real ;
|
53
|
Switch1_ANGL_In2_118 : bool ;
|
54
|
Switch1_ANGL_In3_119 : real ;
|
55
|
sign_In1_126 : real ;
|
56
|
|
57
|
let
|
58
|
Abs_Out1_79 = if Abs_In1_78 < 0.000000 then - Abs_In1_78 else Abs_In1_78;
|
59
|
Relational_Operator_ANGL_Out1_101 = Relational_Operator_ANGL_In1_99 > 180.000000;
|
60
|
Sum6_Out1_111 = - Sum6_In1_109 + Sum6_In2_110;
|
61
|
Switch1_ANGL_Out1_120 = if Switch1_ANGL_In2_118 then Switch1_ANGL_In1_117 else Switch1_ANGL_In3_119;
|
62
|
sign_Out1_127 = ANGL_sign ( sign_In1_126 );
|
63
|
times360_Out1_226 = 360.000000 * sign_Out1_127;
|
64
|
Relational_Operator_ANGL_In1_99 = Abs_Out1_79;
|
65
|
Abs_In1_78 = in_Out1_76;
|
66
|
Switch1_ANGL_In3_119 = in_Out1_76;
|
67
|
Sum6_In2_110 = in_Out1_76;
|
68
|
sign_In1_126 = in_Out1_76;
|
69
|
_180_In1_238 = Switch1_ANGL_Out1_120;
|
70
|
Sum6_In1_109 = times360_Out1_226;
|
71
|
Switch1_ANGL_In1_117 = Sum6_Out1_111;
|
72
|
Switch1_ANGL_In2_118 = Relational_Operator_ANGL_Out1_101;
|
73
|
tel
|
74
|
|
75
|
node DynamicSaturation ( up_Out1_293 : real ; u_Out1_303 : real ; lo_Out1_313 : real )
|
76
|
returns ( y1_In1_355 : real ) ;
|
77
|
var LowerRelop1_Out1_319 : bool ;
|
78
|
UpperRelop_Out1_325 : bool ;
|
79
|
switch2_1_Out1_334 : real ;
|
80
|
switch2_2_Out1_343 : real ;
|
81
|
LowerRelop1_In1_317 : real ;
|
82
|
LowerRelop1_In2_318 : real ;
|
83
|
UpperRelop_In1_323 : real ;
|
84
|
UpperRelop_In2_324 : real ;
|
85
|
switch2_1_In1_331 : real ;
|
86
|
switch2_1_In2_332 : bool ;
|
87
|
switch2_1_In3_333 : real ;
|
88
|
switch2_2_In1_340 : real ;
|
89
|
switch2_2_In2_341 : bool ;
|
90
|
switch2_2_In3_342 : real ;
|
91
|
|
92
|
let
|
93
|
LowerRelop1_Out1_319 = LowerRelop1_In1_317 > LowerRelop1_In2_318;
|
94
|
UpperRelop_Out1_325 = UpperRelop_In1_323 < UpperRelop_In2_324;
|
95
|
switch2_1_Out1_334 = if switch2_1_In2_332 then switch2_1_In1_331 else switch2_1_In3_333;
|
96
|
switch2_2_Out1_343 = if switch2_2_In2_341 then switch2_2_In1_340 else switch2_2_In3_342;
|
97
|
switch2_2_In1_340 = up_Out1_293;
|
98
|
LowerRelop1_In2_318 = up_Out1_293;
|
99
|
switch2_1_In3_333 = u_Out1_303;
|
100
|
LowerRelop1_In1_317 = u_Out1_303;
|
101
|
UpperRelop_In1_323 = u_Out1_303;
|
102
|
switch2_1_In1_331 = lo_Out1_313;
|
103
|
UpperRelop_In2_324 = lo_Out1_313;
|
104
|
switch2_2_In3_342 = switch2_1_Out1_334;
|
105
|
y1_In1_355 = switch2_2_Out1_343;
|
106
|
switch2_2_In2_341 = LowerRelop1_Out1_319;
|
107
|
switch2_1_In2_332 = UpperRelop_Out1_325;
|
108
|
tel
|
109
|
|
110
|
node Saturation ( signal : real ) returns ( saturated_signal : real ) ;
|
111
|
var low_lim : real ;
|
112
|
let
|
113
|
saturated_signal = if ( low_lim <= 1000.000000 ) then low_lim else 1000.000000;
|
114
|
low_lim = if ( 0.000100 >= signal ) then 0.000100 else signal;
|
115
|
tel
|
116
|
|
117
|
node HgdCmdProcessor ( Vt_Out1_19 : real ; HdgCmdMCP_Out1_29 : real ; Heading_Out1_39 : real )
|
118
|
returns ( HdgCmd_In1_947 : real ; HdgRteCmd_In1_959 : real ) ;
|
119
|
-- var var2zetaOmega_Out1_59 : real ;
|
120
|
-- ANGL_Out1_66 : real ;
|
121
|
-- Divide_Out1_262 : real ;
|
122
|
-- Divide1_Out1_274 : real ;
|
123
|
-- DynamicSaturation_Out1_283 : real ;
|
124
|
-- Integrator1_Out1_380 : real ;
|
125
|
-- Logical_Operator_Out1_385 : bool ;
|
126
|
-- Product_Out1_397 : real ;
|
127
|
-- Product1_Out1_409 : real ;
|
128
|
-- Saturation1_Out1_416 : real ;
|
129
|
-- Sum2_Out1_489 : real ;
|
130
|
-- Sum3_Out1_499 : real ;
|
131
|
-- Trigonometric_Function_Out1_504 : real ;
|
132
|
-- k_Out1_723 : real ;
|
133
|
-- k1_Out1_732 : real ;
|
134
|
-- k2_Out1_741 : real ;
|
135
|
-- k4_Out1_750 : real ;
|
136
|
-- k6_Out1_759 : real ;
|
137
|
-- omegaSq_Out1_771 : real ;
|
138
|
-- omegaSq2_Out1_783 : real ;
|
139
|
-- omega_calc_Out1_790 : real ;
|
140
|
-- rateLim_calc_Out1_875 : real ;
|
141
|
-- times514dot8_Out1_890 : real ;
|
142
|
-- Integrator1_1_Out1_598 : real ;
|
143
|
-- Logical_Operator1_1_Out1_604 : bool ;
|
144
|
-- Logical_Operator1_2_Out1_610 : bool ;
|
145
|
-- Logical_Operator1_3_Out1_616 : bool ;
|
146
|
-- Relational_Operator_Out1_622 : bool ;
|
147
|
-- Relational_Operator1_Out1_628 : bool ;
|
148
|
-- Relational_Operator2_Out1_634 : bool ;
|
149
|
-- Relational_Operator3_Out1_640 : bool ;
|
150
|
-- Switch1_Out1_649 : real ;
|
151
|
-- Switch2_Out1_658 : real ;
|
152
|
-- Switch3_Out1_667 : real ;
|
153
|
-- var2zetaOmega_In1_57 : real ;
|
154
|
-- var2zetaOmega_In2_58 : real ;
|
155
|
-- ANGL_In1_65 : real ;
|
156
|
-- DynamicSaturation_In1_280 : real ;
|
157
|
-- DynamicSaturation_In2_281 : real ;
|
158
|
-- DynamicSaturation_In3_282 : real ;
|
159
|
-- Integrator1_In1_377 : real ;
|
160
|
-- Integrator1_In2_378 : bool ;
|
161
|
-- Integrator1_In3_379 : real ;
|
162
|
-- Logical_Operator_In1_384 : bool ;
|
163
|
-- Product_In2_396 : real ;
|
164
|
-- Product1_In1_407 : real ;
|
165
|
-- Product1_In2_408 : real ;
|
166
|
-- Saturation1_In1_415 : real ;
|
167
|
-- Sum2_In1_487 : real ;
|
168
|
-- Sum2_In2_488 : real ;
|
169
|
-- Sum3_In1_497 : real ;
|
170
|
-- Sum3_In2_498 : real ;
|
171
|
-- Trigonometric_Function_In1_503 : real ;
|
172
|
-- k_In1_722 : real ;
|
173
|
-- k1_In1_731 : real ;
|
174
|
-- k2_In1_740 : real ;
|
175
|
-- k4_In1_749 : real ;
|
176
|
-- k6_In1_758 : real ;
|
177
|
-- omegaSq_In1_769 : real ;
|
178
|
-- omegaSq_In2_770 : real ;
|
179
|
-- omegaSq2_In1_781 : real ;
|
180
|
-- omegaSq2_In2_782 : real ;
|
181
|
-- omega_calc_In1_789 : real ;
|
182
|
-- rateLim_calc_In1_873 : real ;
|
183
|
-- rateLim_calc_In2_874 : real ;
|
184
|
-- times514dot8_In1_889 : real ;
|
185
|
-- Integrator1_1_In1_595 : real ;
|
186
|
-- Integrator1_1_In2_596 : bool ;
|
187
|
-- Logical_Operator1_1_In1_602 : bool ;
|
188
|
-- Logical_Operator1_1_In2_603 : bool ;
|
189
|
-- Logical_Operator1_2_In1_608 : bool ;
|
190
|
-- Logical_Operator1_2_In2_609 : bool ;
|
191
|
-- Logical_Operator1_3_In1_614 : bool ;
|
192
|
-- Logical_Operator1_3_In2_615 : bool ;
|
193
|
-- Relational_Operator_In1_620 : real ;
|
194
|
-- Relational_Operator_In2_621 : real ;
|
195
|
-- Relational_Operator1_In1_626 : real ;
|
196
|
-- Relational_Operator1_In2_627 : real ;
|
197
|
-- Relational_Operator2_In1_632 : real ;
|
198
|
-- Relational_Operator3_In1_638 : real ;
|
199
|
-- Switch1_In1_646 : real ;
|
200
|
-- Switch1_In2_647 : bool ;
|
201
|
-- Switch1_In3_648 : real ;
|
202
|
-- Switch2_In1_655 : real ;
|
203
|
-- Switch2_In2_656 : bool ;
|
204
|
-- Switch2_In3_657 : real ;
|
205
|
-- Switch3_In2_665 : bool ;
|
206
|
-- Switch3_In3_666 : real ;
|
207
|
-- integrator_out_1 : real ;
|
208
|
-- integrator_out_2 : real ;
|
209
|
let
|
210
|
HdgCmd_In1_947 = HdgCmdMCP_Out1_29;
|
211
|
HdgRteCmd_In1_959 = 0.0;
|
212
|
tel
|
213
|
|
214
|
node HeadingControl ( HdgCmdMCP_Out1_19 : real ; Heading_Out1_29 : real ; psidot_Out1_39 : real ; Vt_Out1_49 : real ; Roll_Out1_59 : real ; HeadEng_Out1_69 : bool ; rollLimitinput_Out1_79 : real )
|
215
|
returns ( RollCmd_In1_1442 : real ) ;
|
216
|
var HdgCmdProcessor_Out1_88 : real ;
|
217
|
HdgCmdProcessor_Out2_89 : real ;
|
218
|
Khdg_Out1_1105 : real ;
|
219
|
Khdg1_Out1_1114 : real ;
|
220
|
Khdg2_Out1_1123 : real ;
|
221
|
Khdgrte_Out1_1132 : real ;
|
222
|
Logical_Operator_Out1_1137 : bool ;
|
223
|
Max_Out1_1147 : real ;
|
224
|
Min_Out1_1157 : real ;
|
225
|
Mux_Out1_1161_0 : real ;
|
226
|
Mux_Out1_1161_1 : real ;
|
227
|
Sum_Out1_1171 : real ;
|
228
|
Sum1_Out1_1181 : real ;
|
229
|
Sum2_Out1_1191 : real ;
|
230
|
Sum3_Out1_1201 : real ;
|
231
|
Sum4_Out1_1211 : real ;
|
232
|
-- sgn_Out1_1430 : real ;
|
233
|
Integrator1_1_Out1_1305 : real ;
|
234
|
Logical_Operator1_1_Out1_1311 : bool ;
|
235
|
Logical_Operator1_2_Out1_1317 : bool ;
|
236
|
Logical_Operator1_3_Out1_1323 : bool ;
|
237
|
Relational_Operator_Out1_1329 : bool ;
|
238
|
Relational_Operator1_Out1_1335 : bool ;
|
239
|
Relational_Operator2_Out1_1341 : bool ;
|
240
|
Relational_Operator3_Out1_1347 : bool ;
|
241
|
Switch1_Out1_1356 : real ;
|
242
|
Switch2_Out1_1365 : real ;
|
243
|
Switch3_Out1_1374 : real ;
|
244
|
HdgCmdProcessor_In1_85 : real ;
|
245
|
HdgCmdProcessor_In2_86 : real ;
|
246
|
HdgCmdProcessor_In3_87 : real ;
|
247
|
Khdg_In1_1104 : real ;
|
248
|
Khdg1_In1_1113 : real ;
|
249
|
Khdg2_In1_1122 : real ;
|
250
|
Khdgrte_In1_1131 : real ;
|
251
|
Logical_Operator_In1_1136 : bool ;
|
252
|
Max_In1_1145 : real ;
|
253
|
Max_In2_1146 : real ;
|
254
|
Min_In1_1155 : real ;
|
255
|
Min_In2_1156 : real ;
|
256
|
Mux_In1_1159 : real ;
|
257
|
Mux_In2_1160 : real ;
|
258
|
Sum_In1_1169 : real ;
|
259
|
Sum_In2_1170 : real ;
|
260
|
Sum1_In1_1179 : real ;
|
261
|
Sum1_In2_1180 : real ;
|
262
|
Sum2_In1_1189 : real ;
|
263
|
Sum2_In2_1190 : real ;
|
264
|
Sum3_In1_1199 : real ;
|
265
|
Sum3_In2_1200 : real ;
|
266
|
Sum4_In1_1209 : real ;
|
267
|
Sum4_In2_1210 : real ;
|
268
|
-- sgn_In1_1429 : real ;
|
269
|
Integrator1_1_In1_1302 : real ;
|
270
|
Integrator1_1_In2_1303 : bool ;
|
271
|
Integrator1_1_In3_1304 : real ;
|
272
|
Logical_Operator1_1_In1_1309 : bool ;
|
273
|
Logical_Operator1_1_In2_1310 : bool ;
|
274
|
Logical_Operator1_2_In1_1315 : bool ;
|
275
|
Logical_Operator1_2_In2_1316 : bool ;
|
276
|
Logical_Operator1_3_In1_1321 : bool ;
|
277
|
Logical_Operator1_3_In2_1322 : bool ;
|
278
|
-- Relational_Operator_In1_1327 : real ;
|
279
|
Relational_Operator_In2_1328 : real ;
|
280
|
-- Relational_Operator1_In1_1333 : real ;
|
281
|
Relational_Operator1_In2_1334 : real ;
|
282
|
Relational_Operator2_In1_1339 : real ;
|
283
|
-- Relational_Operator2_In2_1340 : real ;
|
284
|
Relational_Operator3_In1_1345 : real ;
|
285
|
-- Relational_Operator3_In2_1346 : real ;
|
286
|
-- Switch1_In1_1353 : real ;
|
287
|
Switch1_In2_1354 : bool ;
|
288
|
Switch1_In3_1355 : real ;
|
289
|
-- Switch2_In1_1362 : real ;
|
290
|
Switch2_In2_1363 : bool ;
|
291
|
Switch2_In3_1364 : real ;
|
292
|
-- Switch3_In1_1371 : real ;
|
293
|
Switch3_In2_1372 : bool ;
|
294
|
Switch3_In3_1373 : real ;
|
295
|
integrator_out : real ;
|
296
|
let
|
297
|
HdgCmdProcessor_Out1_88 , HdgCmdProcessor_Out2_89 = HgdCmdProcessor ( HdgCmdProcessor_In1_85 , HdgCmdProcessor_In2_86 , HdgCmdProcessor_In3_87 );
|
298
|
Khdg_Out1_1105 = 0.050000 * Khdg_In1_1104;
|
299
|
-- Khdg1_Out1_1114 = int_to_real ( 1 ) *Khdg1_In1_1113;
|
300
|
Khdg1_Out1_1114 = Khdg1_In1_1113;
|
301
|
-- Khdg2_Out1_1123 = int_to_real ( 1 ) * Khdg2_In1_1122;
|
302
|
Khdg2_Out1_1123 = Khdg2_In1_1122;
|
303
|
Khdgrte_Out1_1132 = 3.0 * Khdgrte_In1_1131;
|
304
|
Logical_Operator_Out1_1137 = not Logical_Operator_In1_1136;
|
305
|
Max_Out1_1147 = if Max_In1_1145 >= Max_In2_1146 then Max_In1_1145 else Max_In2_1146;
|
306
|
Min_Out1_1157 = if Min_In1_1155 <= Min_In2_1156 then Min_In1_1155 else Min_In2_1156;
|
307
|
-- [ Mux_Out1_1161_0 , Mux_Out1_1161_1 ] = [ Mux_In1_1159 , Mux_In2_1160 ];
|
308
|
Mux_Out1_1161_0 = Mux_In1_1159 ;
|
309
|
Mux_Out1_1161_1 = Mux_In2_1160 ;
|
310
|
Sum_Out1_1171 = Sum_In1_1169 + - Sum_In2_1170;
|
311
|
Sum1_Out1_1181 = Sum1_In1_1179 + Sum1_In2_1180;
|
312
|
Sum2_Out1_1191 = Sum2_In1_1189 + - Sum2_In2_1190;
|
313
|
Sum3_Out1_1201 = - Sum3_In1_1199 + - Sum3_In2_1200;
|
314
|
Sum4_Out1_1211 = Sum4_In1_1209 + - Sum4_In2_1210;
|
315
|
-- sgn_Out1_1430 = int_to_real ( - 1 ) * sgn_In1_1429;
|
316
|
-- sgn_Out1_1430 = - 30.0;
|
317
|
integrator_out = integrator_reset ( Integrator1_1_In1_1302 , Integrator1_1_In2_1303 , Integrator1_1_In3_1304);
|
318
|
Integrator1_1_Out1_1305 = Integrator1_1_In3_1304 -> pre (integrator_out);
|
319
|
Logical_Operator1_1_Out1_1311 = Logical_Operator1_1_In1_1309 and Logical_Operator1_1_In2_1310;
|
320
|
Logical_Operator1_2_Out1_1317 = Logical_Operator1_2_In1_1315 and Logical_Operator1_2_In2_1316;
|
321
|
Logical_Operator1_3_Out1_1323 = Logical_Operator1_3_In1_1321 or Logical_Operator1_3_In2_1322;
|
322
|
-- Relational_Operator_Out1_1329 = Relational_Operator_In1_1327 <= Relational_Operator_In2_1328;
|
323
|
Relational_Operator_Out1_1329 = rollLimitinput_Out1_79 <= Relational_Operator_In2_1328;
|
324
|
-- Relational_Operator1_Out1_1335 = Relational_Operator1_In1_1333 >= Relational_Operator1_In2_1334;
|
325
|
Relational_Operator1_Out1_1335 = -rollLimitinput_Out1_79 >= Relational_Operator1_In2_1334;
|
326
|
-- Relational_Operator2_Out1_1341 = Relational_Operator2_In1_1339 > Relational_Operator2_In2_1340;
|
327
|
Relational_Operator2_Out1_1341 = Relational_Operator2_In1_1339 > 0.0;
|
328
|
-- Relational_Operator3_Out1_1347 = Relational_Operator3_In1_1345 < Relational_Operator3_In2_1346;
|
329
|
Relational_Operator3_Out1_1347 = Relational_Operator3_In1_1345 < 0.0;
|
330
|
-- Switch1_Out1_1356 = if Switch1_In2_1354 then Switch1_In1_1353 else Switch1_In3_1355;
|
331
|
Switch1_Out1_1356 = if Switch1_In2_1354 then rollLimitinput_Out1_79 else Switch1_In3_1355;
|
332
|
-- Switch2_Out1_1365 = if Switch2_In2_1363 then Switch2_In1_1362 else Switch2_In3_1364;
|
333
|
Switch2_Out1_1365 = if Switch2_In2_1363 then -rollLimitinput_Out1_79 else Switch2_In3_1364;
|
334
|
Switch3_Out1_1374 = if Switch3_In2_1372 then 0.0 else Switch3_In3_1373;
|
335
|
HdgCmdProcessor_In3_87 = Heading_Out1_29;
|
336
|
Sum_In2_1170 = Heading_Out1_29;
|
337
|
Sum_In1_1169 = HdgCmdProcessor_Out1_88;
|
338
|
Mux_In2_1160 = HdgCmdProcessor_Out1_88;
|
339
|
Khdg_In1_1104 = Sum_Out1_1171;
|
340
|
Sum1_In1_1179 = Khdg_Out1_1105;
|
341
|
Sum1_In2_1180 = Sum2_Out1_1191;
|
342
|
Sum2_In1_1189 = HdgCmdProcessor_Out2_89;
|
343
|
Sum2_In2_1190 = psidot_Out1_39;
|
344
|
HdgCmdProcessor_In1_85 = Vt_Out1_49;
|
345
|
HdgCmdProcessor_In2_86 = HdgCmdMCP_Out1_19;
|
346
|
Mux_In1_1159 = HdgCmdMCP_Out1_19;
|
347
|
RollCmd_In1_1442 = Switch2_Out1_1365;
|
348
|
Sum3_In1_1199 = Switch2_Out1_1365;
|
349
|
Sum4_In2_1210 = Switch2_Out1_1365;
|
350
|
-- sgn_In1_1429 = 30.0;
|
351
|
Khdgrte_In1_1131 = Sum1_Out1_1181;
|
352
|
Min_In1_1155 = Max_Out1_1147;
|
353
|
Logical_Operator_In1_1136 = HeadEng_Out1_69;
|
354
|
Khdg1_In1_1113 = Sum3_Out1_1201;
|
355
|
Khdg2_In1_1122 = Sum4_Out1_1211;
|
356
|
Max_In2_1146 = Khdg1_Out1_1114;
|
357
|
Min_In2_1156 = Khdg2_Out1_1123;
|
358
|
Max_In1_1145 = Khdgrte_Out1_1132;
|
359
|
Sum3_In2_1200 = rollLimitinput_Out1_79;
|
360
|
Sum4_In1_1209 = rollLimitinput_Out1_79;
|
361
|
-- Relational_Operator1_In1_1333 = sgn_Out1_1430;
|
362
|
-- Switch2_In1_1362 = sgn_Out1_1430;
|
363
|
-- Relational_Operator1_In1_1333 = -30.0;
|
364
|
-- Switch2_In1_1362 = -30.0;
|
365
|
Integrator1_1_In3_1304 = Roll_Out1_59;
|
366
|
Integrator1_1_In2_1303 = Logical_Operator_Out1_1137;
|
367
|
Switch3_In3_1373 = Min_Out1_1157;
|
368
|
Relational_Operator3_In1_1345 = Min_Out1_1157;
|
369
|
Relational_Operator2_In1_1339 = Min_Out1_1157;
|
370
|
-- Relational_Operator_In1_1327 = 30.0;
|
371
|
-- Switch1_In1_1353 = 30.0 ;
|
372
|
Logical_Operator1_2_In2_1316 = Relational_Operator1_Out1_1335;
|
373
|
Switch2_In2_1363 = Relational_Operator1_Out1_1335;
|
374
|
Switch2_In3_1364 = Switch1_Out1_1356;
|
375
|
Relational_Operator1_In2_1334 = Switch1_Out1_1356;
|
376
|
-- Relational_Operator3_In2_1346 = 0.0 ;
|
377
|
Logical_Operator1_2_In1_1315 = Relational_Operator3_Out1_1347;
|
378
|
Logical_Operator1_3_In2_1322 = Logical_Operator1_2_Out1_1317;
|
379
|
Logical_Operator1_3_In1_1321 = Logical_Operator1_1_Out1_1311;
|
380
|
Switch3_In2_1372 = Logical_Operator1_3_Out1_1323;
|
381
|
Logical_Operator1_1_In2_1310 = Relational_Operator2_Out1_1341;
|
382
|
-- Relational_Operator2_In2_1340 = 0.0;
|
383
|
-- Switch3_In1_1371 = 0.0 ;
|
384
|
Integrator1_1_In1_1302 = Switch3_Out1_1374;
|
385
|
Logical_Operator1_1_In1_1309 = Relational_Operator_Out1_1329;
|
386
|
Switch1_In2_1354 = Relational_Operator_Out1_1329;
|
387
|
Relational_Operator_In2_1328 = Integrator1_1_Out1_1305;
|
388
|
Switch1_In3_1355 = Integrator1_1_Out1_1305;
|
389
|
tel
|
390
|
|
391
|
|
392
|
|
393
|
|
394
|
|
395
|
|
396
|
node top ( HdgCmdMCP: real ; Heading : real ; psidot : real ; Vt : real ; Roll : real ; HeadEng : bool ; rollLimitInput : real )
|
397
|
returns (obs : bool);
|
398
|
var RollCmd : real;
|
399
|
right : bool;
|
400
|
preRollCmd : real;
|
401
|
count : int;
|
402
|
let
|
403
|
|
404
|
RollCmd = HeadingControl ( HdgCmdMCP , Heading , psidot , Vt , Roll , HeadEng , rollLimitInput);
|
405
|
|
406
|
-- Put properties here
|
407
|
|
408
|
assert (rollLimitInput > 0) and (rollLimitInput <= 30);
|
409
|
assert (HeadEng = true);
|
410
|
assert rollLimitInput = 30.0;
|
411
|
|
412
|
assert psidot = 0.0;
|
413
|
|
414
|
assert (HdgCmdMCP >= 0.0) and (HdgCmdMCP < 180.0);
|
415
|
assert (Heading >= 0.0) and (Heading < 180.0);
|
416
|
|
417
|
assert true -> HdgCmdMCP > Heading + 10;
|
418
|
assert true -> HdgCmdMCP = pre(HdgCmdMCP);
|
419
|
assert true -> Heading = pre(Heading);
|
420
|
|
421
|
assert Roll = 0.0 -> Roll = pre(RollCmd);
|
422
|
|
423
|
right = ((HdgCmdMCP - Heading > 0.0) and (HdgCmdMCP - Heading < 180.0)) or (Heading - HdgCmdMCP > 180.0);
|
424
|
|
425
|
preRollCmd = 0.0 -> pre(RollCmd);
|
426
|
|
427
|
count = 0 -> pre(count) + 1;
|
428
|
|
429
|
obs = true -> (count < 2) or (HdgCmdMCP = Heading) or
|
430
|
((right = true) and ((RollCmd > pre(preRollCmd)) or (RollCmd = rollLimitInput))) or
|
431
|
((right = false) and ((RollCmd < pre(preRollCmd)) or (RollCmd = - rollLimitInput)));
|
432
|
|
433
|
--!MAIN : true;
|
434
|
--!PROPERTY: obs = true;
|
435
|
tel
|