1 | /* |
2 | * Academic License - for use in teaching, academic research, and meeting |
3 | * course requirements at degree granting institutions only. Not for |
4 | * government, commercial, or other organizational use. |
5 | * |
6 | * File: AHRSControl.c |
7 | * |
8 | * Code generated for Simulink model 'AHRSControl'. |
9 | * |
10 | * Model version : 1.115 |
11 | * Simulink Coder version : 9.3 (R2020a) 18-Nov-2019 |
12 | * C/C++ source code generated on : Fri Jul 10 14:44:58 2020 |
13 | * |
14 | * Target selection: ert.tlc |
15 | * Embedded hardware selection: Intel->x86-32 (Windows32) |
16 | * Code generation objectives: |
17 | * 1. Execution efficiency |
18 | * 2. RAM efficiency |
19 | * Validation result: Not run |
20 | */ |
21 | |
22 | #include "AHRSControl.h" |
23 | |
24 | /* Block states (default storage) */ |
25 | DW_AHRSControl_f_T AHRSControl_DW; |
26 | extern void AHRSControl_Filter(real_T rtu_Signal, real_T rtu_prev_FeedbackSum, |
27 | real_T *rty_filtered_Signal, real_T *rty_FeedbackSum); |
28 | extern void AHRSControl_pitch_feedback(real_T rtu_theta_cmd, const real_T |
29 | rtu_voted_fb[5], real_T *rty_ds); |
30 | extern void AHRSControl_roll_feedback(real_T rtu_phi_cmd, const real_T |
31 | rtu_voted_fb[5], real_T *rty_dc); |
32 | extern void AHRSControl_yaw_feedback(real_T rtu_r_cmd, const real_T |
33 | rtu_voted_fb[5], real_T *rty_dT); |
34 | extern void AHRSControl_Heli_inner_loop(real_T rtu_theta_cmd, real_T rtu_phi_cmd, |
35 | real_T rtu_r_cmd, const real_T rtu_voted_fb[5], real_T rty_actuator_commands[3]); |
36 | extern void AHRSControl_PitchLoop_Disable(void); |
37 | extern void AHRSControl_PitchLoop(real_T rtu_U, real_T *rty_Y); |
38 | extern void AHRSControl_RollLoop_Disable(void); |
39 | extern void AHRSControl_RollLoop(real_T rtu_U, real_T *rty_Y); |
40 | extern void AHRSControl_YawLoop_Disable(void); |
41 | extern void AHRSControl_YawLoop(real_T rtu_U, real_T *rty_Y); |
42 | extern void AHRSCon_Heli_outer_loop_Disable(void); |
43 | extern void AHRSControl_Heli_outer_loop(int16_T rtu_Pilot_theta_cmd, int16_T |
44 | rtu_Pilot_phi_cmd, int16_T rtu_Pilot_r_cmd, const real_T rtu_voted_fb[5], |
45 | real_T *rty_theta_cmd, real_T *rty_phi_cmd, real_T *rty_r_cmd); |
46 | extern void AHRSContro_AHRSLoopCtrl_Disable(void); |
47 | extern void AHRSControl_AHRSLoopCtrl(int16_T rtu_Pilot_theta_cmd, int16_T |
48 | rtu_Pilot_phi_cmd, int16_T rtu_Pilot_r_cmd, const real_T rtu_voted_fb[5], |
49 | real_T rty_actuator_commands[3]); |
50 | extern void AHRSControl_Avg_Value(const real_T rtu_AHRS1[5], const real_T |
51 | rtu_AHRS2[5], const real_T rtu_AHRS3[5], boolean_T rtu_AHRS1_Valid, boolean_T |
52 | rtu_AHRS2_Valid, boolean_T rtu_AHRS3_Valid, real_T rty_Avg_Value[5]); |
53 | extern void AHRSControl_Mid_Value(const real_T rtu_AHRS1[5], const real_T |
54 | rtu_AHRS2[5], const real_T rtu_AHRS3[5], real_T rty_Mid_Value[5]); |
55 | extern void AHRSControl_Single_Value(const real_T rtu_AHRS1[5], const real_T |
56 | rtu_AHRS2[5], const real_T rtu_AHRS3[5], boolean_T rtu_AHRS1_Valid, boolean_T |
57 | rtu_AHRS2_Valid, boolean_T rtu_AHRS3_Valid, real_T rty_Single_Value[5]); |
58 | extern void AHRSControl_AHRS_voter(const AHRS_Bus *rtu_AHRS1, const AHRS_Bus |
59 | *rtu_AHRS2, const AHRS_Bus *rtu_AHRS3, real_T rty_voted_fb[5]); |
60 | |
61 | /* Output and update for Simulink Function: '<S4>/Filter' */ |
62 | void AHRSControl_Filter(real_T rtu_Signal, real_T rtu_prev_FeedbackSum, real_T |
63 | *rty_filtered_Signal, real_T *rty_FeedbackSum) |
64 | { |
65 | real_T rtb_SumB21; |
66 | |
67 | /* Sum: '<S6>/SumA21' incorporates: |
68 | * Gain: '<S6>/a_2_1' |
69 | * Gain: '<S6>/s_1' |
70 | * SignalConversion generated from: '<S6>/Signal' |
71 | * SignalConversion generated from: '<S6>/prev_FeedbackSum' |
72 | */ |
73 | rtb_SumB21 = (0.16840424129937498 * rtu_Signal) - (-0.66319151740124993 * |
74 | rtu_prev_FeedbackSum); |
75 | |
76 | /* SignalConversion generated from: '<S6>/FeedbackSum' */ |
77 | *rty_FeedbackSum = rtb_SumB21; |
78 | |
79 | /* SignalConversion generated from: '<S6>/filtered_Signal' incorporates: |
80 | * SignalConversion generated from: '<S6>/prev_FeedbackSum' |
81 | * Sum: '<S6>/SumB21' |
82 | */ |
83 | *rty_filtered_Signal = rtb_SumB21 + rtu_prev_FeedbackSum; |
84 | } |
85 | |
86 | /* Output and update for Simulink Function: '<S4>/pitch_feedback' */ |
87 | void AHRSControl_pitch_feedback(real_T rtu_theta_cmd, const real_T rtu_voted_fb |
88 | [5], real_T *rty_ds) |
89 | { |
90 | real_T rtb_ds; |
91 | real_T rtb_Filter_caller_o2; |
92 | int32_T i; |
93 | |
94 | /* Gain: '<S8>/Gain' incorporates: |
95 | * SignalConversion generated from: '<S8>/voted_fb' |
96 | */ |
97 | rtb_ds = 0.0; |
98 | for (i = 0; i < 5; i++) { |
99 | rtb_ds += AHRSControl_ConstP.Gain_Gain[i] * rtu_voted_fb[i]; |
100 | } |
101 | |
102 | /* Sum: '<S8>/Sum5' incorporates: |
103 | * Gain: '<S8>/Gain' |
104 | * SignalConversion generated from: '<S8>/theta_cmd' |
105 | */ |
106 | rtb_ds = rtu_theta_cmd - rtb_ds; |
107 | |
108 | /* FunctionCaller: '<S8>/Filter_caller' incorporates: |
109 | * UnitDelay: '<S8>/Unit Delay' |
110 | */ |
111 | AHRSControl_Filter(rtb_ds, AHRSControl_DW.UnitDelay_DSTATE_j, &rtb_ds, |
112 | &rtb_Filter_caller_o2); |
113 | |
114 | /* SignalConversion generated from: '<S8>/ds' */ |
115 | *rty_ds = rtb_ds; |
116 | |
117 | /* Update for UnitDelay: '<S8>/Unit Delay' */ |
118 | AHRSControl_DW.UnitDelay_DSTATE_j = rtb_Filter_caller_o2; |
119 | } |
120 | |
121 | /* Output and update for Simulink Function: '<S4>/roll_feedback' */ |
122 | void AHRSControl_roll_feedback(real_T rtu_phi_cmd, const real_T rtu_voted_fb[5], |
123 | real_T *rty_dc) |
124 | { |
125 | real_T rtb_dc; |
126 | real_T rtb_Filter_caller_o2; |
127 | int32_T i; |
128 | |
129 | /* Gain: '<S9>/Gain' incorporates: |
130 | * SignalConversion generated from: '<S9>/voted_fb' |
131 | */ |
132 | rtb_dc = 0.0; |
133 | for (i = 0; i < 5; i++) { |
134 | rtb_dc += AHRSControl_ConstP.Gain_Gain_d[i] * rtu_voted_fb[i]; |
135 | } |
136 | |
137 | /* Sum: '<S9>/Sum5' incorporates: |
138 | * Gain: '<S9>/Gain' |
139 | * SignalConversion generated from: '<S9>/phi_cmd' |
140 | */ |
141 | rtb_dc = rtu_phi_cmd - rtb_dc; |
142 | |
143 | /* FunctionCaller: '<S9>/Filter_caller' incorporates: |
144 | * UnitDelay: '<S9>/Unit Delay' |
145 | */ |
146 | AHRSControl_Filter(rtb_dc, AHRSControl_DW.UnitDelay_DSTATE_f, &rtb_dc, |
147 | &rtb_Filter_caller_o2); |
148 | |
149 | /* SignalConversion generated from: '<S9>/dc' */ |
150 | *rty_dc = rtb_dc; |
151 | |
152 | /* Update for UnitDelay: '<S9>/Unit Delay' */ |
153 | AHRSControl_DW.UnitDelay_DSTATE_f = rtb_Filter_caller_o2; |
154 | } |
155 | |
156 | /* Output and update for Simulink Function: '<S4>/yaw_feedback' */ |
157 | void AHRSControl_yaw_feedback(real_T rtu_r_cmd, const real_T rtu_voted_fb[5], |
158 | real_T *rty_dT) |
159 | { |
160 | real_T rtb_dT; |
161 | real_T rtb_Filter_caller_o2; |
162 | int32_T i; |
163 | |
164 | /* Gain: '<S10>/Gain' incorporates: |
165 | * SignalConversion generated from: '<S10>/voted_fb' |
166 | */ |
167 | rtb_dT = 0.0; |
168 | for (i = 0; i < 5; i++) { |
169 | rtb_dT += AHRSControl_ConstP.Gain_Gain_b[i] * rtu_voted_fb[i]; |
170 | } |
171 | |
172 | /* Sum: '<S10>/Sum5' incorporates: |
173 | * Gain: '<S10>/Gain' |
174 | * SignalConversion generated from: '<S10>/r_cmd' |
175 | */ |
176 | rtb_dT = rtu_r_cmd - rtb_dT; |
177 | |
178 | /* FunctionCaller: '<S10>/Filter_caller' incorporates: |
179 | * UnitDelay: '<S10>/Unit Delay' |
180 | */ |
181 | AHRSControl_Filter(rtb_dT, AHRSControl_DW.UnitDelay_DSTATE, &rtb_dT, |
182 | &rtb_Filter_caller_o2); |
183 | |
184 | /* SignalConversion generated from: '<S10>/dT' */ |
185 | *rty_dT = rtb_dT; |
186 | |
187 | /* Update for UnitDelay: '<S10>/Unit Delay' */ |
188 | AHRSControl_DW.UnitDelay_DSTATE = rtb_Filter_caller_o2; |
189 | } |
190 | |
191 | /* Output and update for Simulink Function: '<S2>/Heli_inner_loop' */ |
192 | void AHRSControl_Heli_inner_loop(real_T rtu_theta_cmd, real_T rtu_phi_cmd, |
193 | real_T rtu_r_cmd, const real_T rtu_voted_fb[5], real_T rty_actuator_commands[3]) |
194 | { |
195 | real_T rtb_pitch_feedback_caller; |
196 | real_T rtb_TmpSignalConversionAtvote_o[5]; |
197 | real_T rtb_roll_feedback_caller; |
198 | real_T rtb_yaw_feedback_caller; |
199 | int32_T i; |
200 | |
201 | /* SignalConversion generated from: '<S4>/theta_cmd' */ |
202 | rtb_pitch_feedback_caller = rtu_theta_cmd; |
203 | |
204 | /* SignalConversion generated from: '<S4>/voted_fb' */ |
205 | for (i = 0; i < 5; i++) { |
206 | rtb_TmpSignalConversionAtvote_o[i] = rtu_voted_fb[i]; |
207 | } |
208 | |
209 | /* End of SignalConversion generated from: '<S4>/voted_fb' */ |
210 | |
211 | /* FunctionCaller: '<S4>/pitch_feedback_caller' incorporates: |
212 | * SignalConversion generated from: '<S4>/theta_cmd' |
213 | */ |
214 | AHRSControl_pitch_feedback(rtu_theta_cmd, rtb_TmpSignalConversionAtvote_o, |
215 | &rtb_pitch_feedback_caller); |
216 | |
217 | /* SignalConversion generated from: '<S4>/phi_cmd' */ |
218 | rtb_roll_feedback_caller = rtu_phi_cmd; |
219 | |
220 | /* FunctionCaller: '<S4>/roll_feedback_caller' incorporates: |
221 | * SignalConversion generated from: '<S4>/phi_cmd' |
222 | */ |
223 | AHRSControl_roll_feedback(rtu_phi_cmd, rtb_TmpSignalConversionAtvote_o, |
224 | &rtb_roll_feedback_caller); |
225 | |
226 | /* SignalConversion generated from: '<S4>/r_cmd' */ |
227 | rtb_yaw_feedback_caller = rtu_r_cmd; |
228 | |
229 | /* FunctionCaller: '<S4>/yaw_feedback_caller' incorporates: |
230 | * SignalConversion generated from: '<S4>/r_cmd' |
231 | */ |
232 | AHRSControl_yaw_feedback(rtu_r_cmd, rtb_TmpSignalConversionAtvote_o, |
233 | &rtb_yaw_feedback_caller); |
234 | |
235 | /* Gain: '<S4>/Gain' */ |
236 | rtb_pitch_feedback_caller *= 0.0033333333333333335; |
237 | |
238 | /* Saturate: '<S4>/Saturation' */ |
239 | if (rtb_pitch_feedback_caller > 0.1) { |
240 | /* SignalConversion generated from: '<S4>/actuator_commands' */ |
241 | rty_actuator_commands[0] = 0.1; |
242 | } else if (rtb_pitch_feedback_caller < -0.1) { |
243 | /* SignalConversion generated from: '<S4>/actuator_commands' */ |
244 | rty_actuator_commands[0] = -0.1; |
245 | } else { |
246 | /* SignalConversion generated from: '<S4>/actuator_commands' */ |
247 | rty_actuator_commands[0] = rtb_pitch_feedback_caller; |
248 | } |
249 | |
250 | /* Gain: '<S4>/Gain' */ |
251 | rtb_pitch_feedback_caller = 0.0033333333333333335 * rtb_roll_feedback_caller; |
252 | |
253 | /* Saturate: '<S4>/Saturation' */ |
254 | if (rtb_pitch_feedback_caller > 0.1) { |
255 | /* SignalConversion generated from: '<S4>/actuator_commands' */ |
256 | rty_actuator_commands[1] = 0.1; |
257 | } else if (rtb_pitch_feedback_caller < -0.1) { |
258 | /* SignalConversion generated from: '<S4>/actuator_commands' */ |
259 | rty_actuator_commands[1] = -0.1; |
260 | } else { |
261 | /* SignalConversion generated from: '<S4>/actuator_commands' */ |
262 | rty_actuator_commands[1] = rtb_pitch_feedback_caller; |
263 | } |
264 | |
265 | /* Gain: '<S4>/Gain' */ |
266 | rtb_pitch_feedback_caller = 0.0033333333333333335 * rtb_yaw_feedback_caller; |
267 | |
268 | /* Saturate: '<S4>/Saturation' */ |
269 | if (rtb_pitch_feedback_caller > 0.1) { |
270 | /* SignalConversion generated from: '<S4>/actuator_commands' */ |
271 | rty_actuator_commands[2] = 0.1; |
272 | } else if (rtb_pitch_feedback_caller < -0.1) { |
273 | /* SignalConversion generated from: '<S4>/actuator_commands' */ |
274 | rty_actuator_commands[2] = -0.1; |
275 | } else { |
276 | /* SignalConversion generated from: '<S4>/actuator_commands' */ |
277 | rty_actuator_commands[2] = rtb_pitch_feedback_caller; |
278 | } |
279 | } |
280 | |
281 | /* Disable for Simulink Function: '<S5>/PitchLoop' */ |
282 | void AHRSControl_PitchLoop_Disable(void) |
283 | { |
284 | /* Disable for DiscreteIntegrator: '<S12>/Integrator' */ |
285 | AHRSControl_DW.Integrator_DSTATE = AHRSControl_DW.Integrator; |
286 | } |
287 | |
288 | /* Output and update for Simulink Function: '<S5>/PitchLoop' */ |
289 | void AHRSControl_PitchLoop(real_T rtu_U, real_T *rty_Y) |
290 | { |
291 | /* DiscreteIntegrator: '<S12>/Integrator' */ |
292 | AHRSControl_DW.Integrator = AHRSControl_DW.Integrator_DSTATE; |
293 | |
294 | /* SignalConversion generated from: '<S12>/Y' incorporates: |
295 | * Gain: '<S12>/Gain' |
296 | * Gain: '<S12>/Gain1' |
297 | * SignalConversion generated from: '<S12>/U' |
298 | * Sum: '<S12>/Sum3' |
299 | */ |
300 | *rty_Y = (0.01 * AHRSControl_DW.Integrator) + (1.13 * rtu_U); |
301 | |
302 | /* Update for DiscreteIntegrator: '<S12>/Integrator' incorporates: |
303 | * Gain: '<S12>/Gain2' |
304 | * SignalConversion generated from: '<S12>/U' |
305 | */ |
306 | AHRSControl_DW.Integrator_DSTATE += 2.25 * rtu_U; |
307 | if (AHRSControl_DW.Integrator_DSTATE >= 10.0) { |
308 | AHRSControl_DW.Integrator_DSTATE = 10.0; |
309 | } else { |
310 | if (AHRSControl_DW.Integrator_DSTATE <= -10.0) { |
311 | AHRSControl_DW.Integrator_DSTATE = -10.0; |
312 | } |
313 | } |
314 | |
315 | /* End of Update for DiscreteIntegrator: '<S12>/Integrator' */ |
316 | } |
317 | |
318 | /* Disable for Simulink Function: '<S5>/RollLoop' */ |
319 | void AHRSControl_RollLoop_Disable(void) |
320 | { |
321 | /* Disable for DiscreteIntegrator: '<S13>/Integrator1' */ |
322 | AHRSControl_DW.Integrator1_DSTATE = AHRSControl_DW.Integrator1; |
323 | } |
324 | |
325 | /* Output and update for Simulink Function: '<S5>/RollLoop' */ |
326 | void AHRSControl_RollLoop(real_T rtu_U, real_T *rty_Y) |
327 | { |
328 | /* DiscreteIntegrator: '<S13>/Integrator1' */ |
329 | AHRSControl_DW.Integrator1 = AHRSControl_DW.Integrator1_DSTATE; |
330 | |
331 | /* SignalConversion generated from: '<S13>/Y' incorporates: |
332 | * Gain: '<S13>/Gain1' |
333 | * Gain: '<S13>/Gain2' |
334 | * SignalConversion generated from: '<S13>/U' |
335 | * Sum: '<S13>/Sum4' |
336 | */ |
337 | *rty_Y = (0.01 * AHRSControl_DW.Integrator1) + (-0.086 * rtu_U); |
338 | |
339 | /* Update for DiscreteIntegrator: '<S13>/Integrator1' incorporates: |
340 | * Gain: '<S13>/Gain3' |
341 | * SignalConversion generated from: '<S13>/U' |
342 | */ |
343 | AHRSControl_DW.Integrator1_DSTATE += -1.19 * rtu_U; |
344 | if (AHRSControl_DW.Integrator1_DSTATE >= 10.0) { |
345 | AHRSControl_DW.Integrator1_DSTATE = 10.0; |
346 | } else { |
347 | if (AHRSControl_DW.Integrator1_DSTATE <= -10.0) { |
348 | AHRSControl_DW.Integrator1_DSTATE = -10.0; |
349 | } |
350 | } |
351 | |
352 | /* End of Update for DiscreteIntegrator: '<S13>/Integrator1' */ |
353 | } |
354 | |
355 | /* Disable for Simulink Function: '<S5>/YawLoop' */ |
356 | void AHRSControl_YawLoop_Disable(void) |
357 | { |
358 | /* Disable for DiscreteIntegrator: '<S14>/Integrator2' */ |
359 | AHRSControl_DW.Integrator2_DSTATE = AHRSControl_DW.Integrator2; |
360 | } |
361 | |
362 | /* Output and update for Simulink Function: '<S5>/YawLoop' */ |
363 | void AHRSControl_YawLoop(real_T rtu_U, real_T *rty_Y) |
364 | { |
365 | /* DiscreteIntegrator: '<S14>/Integrator2' */ |
366 | AHRSControl_DW.Integrator2 = AHRSControl_DW.Integrator2_DSTATE; |
367 | |
368 | /* SignalConversion generated from: '<S14>/Y' incorporates: |
369 | * Gain: '<S14>/Gain1' |
370 | * Gain: '<S14>/Gain4' |
371 | * SignalConversion generated from: '<S14>/U' |
372 | * Sum: '<S14>/Sum5' |
373 | */ |
374 | *rty_Y = (0.01 * AHRSControl_DW.Integrator2) + (1.33 * rtu_U); |
375 | |
376 | /* Update for DiscreteIntegrator: '<S14>/Integrator2' incorporates: |
377 | * Gain: '<S14>/Gain5' |
378 | * SignalConversion generated from: '<S14>/U' |
379 | */ |
380 | AHRSControl_DW.Integrator2_DSTATE += -2.33 * rtu_U; |
381 | if (AHRSControl_DW.Integrator2_DSTATE >= 10.0) { |
382 | AHRSControl_DW.Integrator2_DSTATE = 10.0; |
383 | } else { |
384 | if (AHRSControl_DW.Integrator2_DSTATE <= -10.0) { |
385 | AHRSControl_DW.Integrator2_DSTATE = -10.0; |
386 | } |
387 | } |
388 | |
389 | /* End of Update for DiscreteIntegrator: '<S14>/Integrator2' */ |
390 | } |
391 | |
392 | /* Disable for Simulink Function: '<S2>/Heli_outer_loop' */ |
393 | void AHRSCon_Heli_outer_loop_Disable(void) |
394 | { |
395 | /* Disable for S-Function (sfun_private_function_caller) generated from: '<S5>/RollLoop' incorporates: |
396 | * SubSystem: '<S5>/RollLoop' |
397 | */ |
398 | AHRSControl_RollLoop_Disable(); |
399 | |
400 | /* End of Disable for S-Function (sfun_private_function_caller) generated from: '<S5>/RollLoop' */ |
401 | |
402 | /* Disable for S-Function (sfun_private_function_caller) generated from: '<S5>/YawLoop' incorporates: |
403 | * SubSystem: '<S5>/YawLoop' |
404 | */ |
405 | AHRSControl_YawLoop_Disable(); |
406 | |
407 | /* End of Disable for S-Function (sfun_private_function_caller) generated from: '<S5>/YawLoop' */ |
408 | |
409 | /* Disable for S-Function (sfun_private_function_caller) generated from: '<S5>/PitchLoop' incorporates: |
410 | * SubSystem: '<S5>/PitchLoop' |
411 | */ |
412 | AHRSControl_PitchLoop_Disable(); |
413 | |
414 | /* End of Disable for S-Function (sfun_private_function_caller) generated from: '<S5>/PitchLoop' */ |
415 | } |
416 | |
417 | /* Output and update for Simulink Function: '<S2>/Heli_outer_loop' */ |
418 | void AHRSControl_Heli_outer_loop(int16_T rtu_Pilot_theta_cmd, int16_T |
419 | rtu_Pilot_phi_cmd, int16_T rtu_Pilot_r_cmd, const real_T rtu_voted_fb[5], |
420 | real_T *rty_theta_cmd, real_T *rty_phi_cmd, real_T *rty_r_cmd) |
421 | { |
422 | real_T rtb_Saturation_f; |
423 | |
424 | /* Sum: '<S5>/Sum1' incorporates: |
425 | * DataTypeConversion: '<S5>/TypeConversion1' |
426 | * Gain: '<S5>/Scaling1' |
427 | * SignalConversion generated from: '<S5>/Pilot_phi_cmd' |
428 | * SignalConversion generated from: '<S5>/voted_fb' |
429 | */ |
430 | rtb_Saturation_f = (0.00091555528427991577 * ((real_T)rtu_Pilot_phi_cmd)) - |
431 | rtu_voted_fb[1]; |
432 | |
433 | /* FunctionCaller: '<S5>/RollLoop_caller' */ |
434 | AHRSControl_RollLoop(rtb_Saturation_f, &rtb_Saturation_f); |
435 | |
436 | /* Saturate: '<S5>/Saturation1' */ |
437 | if (rtb_Saturation_f > 30.0) { |
438 | /* SignalConversion generated from: '<S5>/phi_cmd' */ |
439 | *rty_phi_cmd = 30.0; |
440 | } else if (rtb_Saturation_f < -30.0) { |
441 | /* SignalConversion generated from: '<S5>/phi_cmd' */ |
442 | *rty_phi_cmd = -30.0; |
443 | } else { |
444 | /* SignalConversion generated from: '<S5>/phi_cmd' */ |
445 | *rty_phi_cmd = rtb_Saturation_f; |
446 | } |
447 | |
448 | /* End of Saturate: '<S5>/Saturation1' */ |
449 | |
450 | /* Sum: '<S5>/Sum2' incorporates: |
451 | * DataTypeConversion: '<S5>/TypeConversion2' |
452 | * Gain: '<S5>/Scaling2' |
453 | * SignalConversion generated from: '<S5>/Pilot_r_cmd' |
454 | * SignalConversion generated from: '<S5>/voted_fb' |
455 | */ |
456 | rtb_Saturation_f = (0.00045777764213995788 * ((real_T)rtu_Pilot_r_cmd)) - |
457 | rtu_voted_fb[2]; |
458 | |
459 | /* FunctionCaller: '<S5>/YawLoop_caller' */ |
460 | AHRSControl_YawLoop(rtb_Saturation_f, &rtb_Saturation_f); |
461 | |
462 | /* Saturate: '<S5>/Saturation2' */ |
463 | if (rtb_Saturation_f > 30.0) { |
464 | /* SignalConversion generated from: '<S5>/r_cmd' */ |
465 | *rty_r_cmd = 30.0; |
466 | } else if (rtb_Saturation_f < -30.0) { |
467 | /* SignalConversion generated from: '<S5>/r_cmd' */ |
468 | *rty_r_cmd = -30.0; |
469 | } else { |
470 | /* SignalConversion generated from: '<S5>/r_cmd' */ |
471 | *rty_r_cmd = rtb_Saturation_f; |
472 | } |
473 | |
474 | /* End of Saturate: '<S5>/Saturation2' */ |
475 | |
476 | /* Sum: '<S5>/Sum' incorporates: |
477 | * DataTypeConversion: '<S5>/TypeConversion' |
478 | * Gain: '<S5>/Scaling' |
479 | * SignalConversion generated from: '<S5>/Pilot_theta_cmd' |
480 | * SignalConversion generated from: '<S5>/voted_fb' |
481 | */ |
482 | rtb_Saturation_f = (0.00091555528427991577 * ((real_T)rtu_Pilot_theta_cmd)) - |
483 | rtu_voted_fb[0]; |
484 | |
485 | /* FunctionCaller: '<S5>/PitchLoop_caller' */ |
486 | AHRSControl_PitchLoop(rtb_Saturation_f, &rtb_Saturation_f); |
487 | |
488 | /* Saturate: '<S5>/Saturation' */ |
489 | if (rtb_Saturation_f > 30.0) { |
490 | /* SignalConversion generated from: '<S5>/theta_cmd' */ |
491 | *rty_theta_cmd = 30.0; |
492 | } else if (rtb_Saturation_f < -30.0) { |
493 | /* SignalConversion generated from: '<S5>/theta_cmd' */ |
494 | *rty_theta_cmd = -30.0; |
495 | } else { |
496 | /* SignalConversion generated from: '<S5>/theta_cmd' */ |
497 | *rty_theta_cmd = rtb_Saturation_f; |
498 | } |
499 | |
500 | /* End of Saturate: '<S5>/Saturation' */ |
501 | } |
502 | |
503 | /* Disable for Simulink Function: '<S1>/AHRSLoopCtrl' */ |
504 | void AHRSContro_AHRSLoopCtrl_Disable(void) |
505 | { |
506 | /* Disable for S-Function (sfun_private_function_caller) generated from: '<S2>/Heli_outer_loop' incorporates: |
507 | * SubSystem: '<S2>/Heli_outer_loop' |
508 | */ |
509 | AHRSCon_Heli_outer_loop_Disable(); |
510 | |
511 | /* End of Disable for S-Function (sfun_private_function_caller) generated from: '<S2>/Heli_outer_loop' */ |
512 | } |
513 | |
514 | /* Output and update for Simulink Function: '<S1>/AHRSLoopCtrl' */ |
515 | void AHRSControl_AHRSLoopCtrl(int16_T rtu_Pilot_theta_cmd, int16_T |
516 | rtu_Pilot_phi_cmd, int16_T rtu_Pilot_r_cmd, const real_T rtu_voted_fb[5], |
517 | real_T rty_actuator_commands[3]) |
518 | { |
519 | real_T rtb_TmpSignalConversionAtvote_g[5]; |
520 | real_T rtb_Heli_outer_loop_caller_o1; |
521 | real_T rtb_Heli_outer_loop_caller_o2; |
522 | real_T rtb_Heli_outer_loop_caller_o3; |
523 | int32_T i; |
524 | |
525 | /* SignalConversion generated from: '<S2>/voted_fb' */ |
526 | for (i = 0; i < 5; i++) { |
527 | rtb_TmpSignalConversionAtvote_g[i] = rtu_voted_fb[i]; |
528 | } |
529 | |
530 | /* End of SignalConversion generated from: '<S2>/voted_fb' */ |
531 | |
532 | /* FunctionCaller: '<S2>/Heli_outer_loop_caller' incorporates: |
533 | * SignalConversion generated from: '<S2>/Pilot_phi_cmd' |
534 | * SignalConversion generated from: '<S2>/Pilot_r_cmd' |
535 | * SignalConversion generated from: '<S2>/Pilot_theta_cmd' |
536 | */ |
537 | AHRSControl_Heli_outer_loop(rtu_Pilot_theta_cmd, rtu_Pilot_phi_cmd, |
538 | rtu_Pilot_r_cmd, rtb_TmpSignalConversionAtvote_g, |
539 | &rtb_Heli_outer_loop_caller_o1, &rtb_Heli_outer_loop_caller_o2, |
540 | &rtb_Heli_outer_loop_caller_o3); |
541 | |
542 | /* SignalConversion generated from: '<S2>/actuator_commands' incorporates: |
543 | * FunctionCaller: '<S2>/Heli_inner_loop_caller' |
544 | */ |
545 | AHRSControl_Heli_inner_loop(rtb_Heli_outer_loop_caller_o1, |
546 | rtb_Heli_outer_loop_caller_o2, rtb_Heli_outer_loop_caller_o3, |
547 | rtb_TmpSignalConversionAtvote_g, rty_actuator_commands); |
548 | } |
549 | |
550 | /* Output and update for Simulink Function: '<S3>/Avg_Value' */ |
551 | void AHRSControl_Avg_Value(const real_T rtu_AHRS1[5], const real_T rtu_AHRS2[5], |
552 | const real_T rtu_AHRS3[5], boolean_T rtu_AHRS1_Valid, boolean_T |
553 | rtu_AHRS2_Valid, boolean_T rtu_AHRS3_Valid, real_T rty_Avg_Value[5]) |
554 | { |
555 | int32_T i; |
556 | real_T rtu_AHRS2_Valid_0; |
557 | real_T rtu_AHRS3_Valid_0; |
558 | for (i = 0; i < 5; i++) { |
559 | /* Switch: '<S15>/Switch' incorporates: |
560 | * SignalConversion generated from: '<S15>/AHRS1' |
561 | * SignalConversion generated from: '<S15>/AHRS1_Valid' |
562 | */ |
563 | if (rtu_AHRS1_Valid) { |
564 | rty_Avg_Value[i] = rtu_AHRS1[i]; |
565 | } else { |
566 | rty_Avg_Value[i] = 0.0; |
567 | } |
568 | |
569 | /* End of Switch: '<S15>/Switch' */ |
570 | |
571 | /* Switch: '<S15>/Switch1' incorporates: |
572 | * SignalConversion generated from: '<S15>/AHRS2' |
573 | * SignalConversion generated from: '<S15>/AHRS2_Valid' |
574 | */ |
575 | if (rtu_AHRS2_Valid) { |
576 | rtu_AHRS2_Valid_0 = rtu_AHRS2[i]; |
577 | } else { |
578 | rtu_AHRS2_Valid_0 = 0.0; |
579 | } |
580 | |
581 | /* End of Switch: '<S15>/Switch1' */ |
582 | |
583 | /* Switch: '<S15>/Switch2' incorporates: |
584 | * SignalConversion generated from: '<S15>/AHRS3' |
585 | * SignalConversion generated from: '<S15>/AHRS3_Valid' |
586 | */ |
587 | if (rtu_AHRS3_Valid) { |
588 | rtu_AHRS3_Valid_0 = rtu_AHRS3[i]; |
589 | } else { |
590 | rtu_AHRS3_Valid_0 = 0.0; |
591 | } |
592 | |
593 | /* End of Switch: '<S15>/Switch2' */ |
594 | |
595 | /* SignalConversion generated from: '<S15>/Avg_Value' incorporates: |
596 | * Gain: '<S15>/Gain' |
597 | * Sum: '<S15>/Sum' |
598 | */ |
599 | rty_Avg_Value[i] = ((rty_Avg_Value[i] + rtu_AHRS2_Valid_0) + |
600 | rtu_AHRS3_Valid_0) * 0.5; |
601 | } |
602 | } |
603 | |
604 | /* Output and update for Simulink Function: '<S3>/Mid_Value' */ |
605 | void AHRSControl_Mid_Value(const real_T rtu_AHRS1[5], const real_T rtu_AHRS2[5], |
606 | const real_T rtu_AHRS3[5], real_T rty_Mid_Value[5]) |
607 | { |
608 | int32_T i; |
609 | |
610 | /* SignalConversion generated from: '<S16>/Mid_Value' incorporates: |
611 | * MinMax: '<S16>/MinMax' |
612 | * MinMax: '<S16>/MinMax1' |
613 | * MinMax: '<S16>/MinMax2' |
614 | * MinMax: '<S16>/MinMax3' |
615 | * SignalConversion generated from: '<S16>/AHRS1' |
616 | * SignalConversion generated from: '<S16>/AHRS2' |
617 | * SignalConversion generated from: '<S16>/AHRS3' |
618 | */ |
619 | for (i = 0; i < 5; i++) { |
620 | rty_Mid_Value[i] = fmax(fmax(fmin(rtu_AHRS1[i], rtu_AHRS2[i]), fmin |
621 | (rtu_AHRS2[i], rtu_AHRS3[i])), fmin(rtu_AHRS3[i], rtu_AHRS1[i])); |
622 | } |
623 | |
624 | /* End of SignalConversion generated from: '<S16>/Mid_Value' */ |
625 | } |
626 | |
627 | /* Output and update for Simulink Function: '<S3>/Single_Value' */ |
628 | void AHRSControl_Single_Value(const real_T rtu_AHRS1[5], const real_T rtu_AHRS2 |
629 | [5], const real_T rtu_AHRS3[5], boolean_T rtu_AHRS1_Valid, boolean_T |
630 | rtu_AHRS2_Valid, boolean_T rtu_AHRS3_Valid, real_T rty_Single_Value[5]) |
631 | { |
632 | int32_T i; |
633 | real_T rtu_AHRS2_Valid_0; |
634 | real_T rtu_AHRS3_Valid_0; |
635 | for (i = 0; i < 5; i++) { |
636 | /* Switch: '<S18>/Switch' incorporates: |
637 | * SignalConversion generated from: '<S18>/AHRS1' |
638 | * SignalConversion generated from: '<S18>/AHRS1_Valid' |
639 | */ |
640 | if (rtu_AHRS1_Valid) { |
641 | rty_Single_Value[i] = rtu_AHRS1[i]; |
642 | } else { |
643 | rty_Single_Value[i] = 0.0; |
644 | } |
645 | |
646 | /* End of Switch: '<S18>/Switch' */ |
647 | |
648 | /* Switch: '<S18>/Switch1' incorporates: |
649 | * SignalConversion generated from: '<S18>/AHRS2' |
650 | * SignalConversion generated from: '<S18>/AHRS2_Valid' |
651 | */ |
652 | if (rtu_AHRS2_Valid) { |
653 | rtu_AHRS2_Valid_0 = rtu_AHRS2[i]; |
654 | } else { |
655 | rtu_AHRS2_Valid_0 = 0.0; |
656 | } |
657 | |
658 | /* End of Switch: '<S18>/Switch1' */ |
659 | |
660 | /* Switch: '<S18>/Switch2' incorporates: |
661 | * SignalConversion generated from: '<S18>/AHRS3' |
662 | * SignalConversion generated from: '<S18>/AHRS3_Valid' |
663 | */ |
664 | if (rtu_AHRS3_Valid) { |
665 | rtu_AHRS3_Valid_0 = rtu_AHRS3[i]; |
666 | } else { |
667 | rtu_AHRS3_Valid_0 = 0.0; |
668 | } |
669 | |
670 | /* End of Switch: '<S18>/Switch2' */ |
671 | |
672 | /* SignalConversion generated from: '<S18>/Single_Value' incorporates: |
673 | * Sum: '<S18>/Sum' |
674 | */ |
675 | rty_Single_Value[i] = (rty_Single_Value[i] + rtu_AHRS2_Valid_0) + |
676 | rtu_AHRS3_Valid_0; |
677 | } |
678 | } |
679 | |
680 | /* Output and update for Simulink Function: '<S1>/AHRS_voter' */ |
681 | void AHRSControl_AHRS_voter(const AHRS_Bus *rtu_AHRS1, const AHRS_Bus *rtu_AHRS2, |
682 | const AHRS_Bus *rtu_AHRS3, real_T rty_voted_fb[5]) |
683 | { |
684 | real_T rtb_TmpSignalConversionAtAvg__o[5]; |
685 | real_T rtb_TmpSignalConversionAtAvg__d[5]; |
686 | real_T rtb_MultiportSwitch[5]; |
687 | real_T rtb_Single_Value_caller[5]; |
688 | real_T rtb_Avg_Value_caller[5]; |
689 | int32_T i; |
690 | |
691 | /* SignalConversion generated from: '<S3>/Avg_Value_caller' incorporates: |
692 | * SignalConversion generated from: '<S3>/AHRS1' |
693 | */ |
694 | rtb_TmpSignalConversionAtAvg__o[0] = rtu_AHRS1->theta; |
695 | rtb_TmpSignalConversionAtAvg__o[1] = rtu_AHRS1->phi; |
696 | rtb_TmpSignalConversionAtAvg__o[2] = rtu_AHRS1->r; |
697 | rtb_TmpSignalConversionAtAvg__o[3] = rtu_AHRS1->q; |
698 | rtb_TmpSignalConversionAtAvg__o[4] = rtu_AHRS1->p; |
699 | |
700 | /* SignalConversion generated from: '<S3>/Avg_Value_caller' incorporates: |
701 | * SignalConversion generated from: '<S3>/AHRS2' |
702 | */ |
703 | rtb_TmpSignalConversionAtAvg__d[0] = rtu_AHRS2->theta; |
704 | rtb_TmpSignalConversionAtAvg__d[1] = rtu_AHRS2->phi; |
705 | rtb_TmpSignalConversionAtAvg__d[2] = rtu_AHRS2->r; |
706 | rtb_TmpSignalConversionAtAvg__d[3] = rtu_AHRS2->q; |
707 | rtb_TmpSignalConversionAtAvg__d[4] = rtu_AHRS2->p; |
708 | |
709 | /* SignalConversion generated from: '<S3>/Avg_Value_caller' incorporates: |
710 | * SignalConversion generated from: '<S3>/AHRS3' |
711 | */ |
712 | rtb_MultiportSwitch[0] = rtu_AHRS3->theta; |
713 | rtb_MultiportSwitch[1] = rtu_AHRS3->phi; |
714 | rtb_MultiportSwitch[2] = rtu_AHRS3->r; |
715 | rtb_MultiportSwitch[3] = rtu_AHRS3->q; |
716 | rtb_MultiportSwitch[4] = rtu_AHRS3->p; |
717 | |
718 | /* FunctionCaller: '<S3>/Single_Value_caller' incorporates: |
719 | * SignalConversion generated from: '<S3>/AHRS1' |
720 | * SignalConversion generated from: '<S3>/AHRS2' |
721 | * SignalConversion generated from: '<S3>/AHRS3' |
722 | */ |
723 | AHRSControl_Single_Value(rtb_TmpSignalConversionAtAvg__o, |
724 | rtb_TmpSignalConversionAtAvg__d, rtb_MultiportSwitch, rtu_AHRS1->valid, |
725 | rtu_AHRS2->valid, rtu_AHRS3->valid, rtb_Single_Value_caller); |
726 | |
727 | /* FunctionCaller: '<S3>/Avg_Value_caller' incorporates: |
728 | * SignalConversion generated from: '<S3>/AHRS1' |
729 | * SignalConversion generated from: '<S3>/AHRS2' |
730 | * SignalConversion generated from: '<S3>/AHRS3' |
731 | */ |
732 | AHRSControl_Avg_Value(rtb_TmpSignalConversionAtAvg__o, |
733 | rtb_TmpSignalConversionAtAvg__d, rtb_MultiportSwitch, |
734 | rtu_AHRS1->valid, rtu_AHRS2->valid, rtu_AHRS3->valid, |
735 | rtb_Avg_Value_caller); |
736 | |
737 | /* FunctionCaller: '<S3>/Mid_Value_caller' */ |
738 | AHRSControl_Mid_Value(rtb_TmpSignalConversionAtAvg__o, |
739 | rtb_TmpSignalConversionAtAvg__d, rtb_MultiportSwitch, |
740 | rty_voted_fb); |
741 | |
742 | /* MultiPortSwitch: '<S3>/MultiportSwitch' incorporates: |
743 | * SignalConversion generated from: '<S3>/AHRS1' |
744 | * SignalConversion generated from: '<S3>/AHRS2' |
745 | * SignalConversion generated from: '<S3>/AHRS3' |
746 | * Sum: '<S3>/Sum' |
747 | */ |
748 | switch ((((uint32_T)(rtu_AHRS1->valid ? 1U : 0U)) + (rtu_AHRS2->valid ? 1U : |
749 | 0U)) + (rtu_AHRS3->valid ? 1U : 0U)) { |
750 | case 0: |
751 | /* SignalConversion generated from: '<S3>/voted_fb' */ |
752 | for (i = 0; i < 5; i++) { |
753 | rty_voted_fb[i] = 0.0; |
754 | } |
755 | break; |
756 | |
757 | case 1: |
758 | /* SignalConversion generated from: '<S3>/voted_fb' */ |
759 | for (i = 0; i < 5; i++) { |
760 | rty_voted_fb[i] = rtb_Single_Value_caller[i]; |
761 | } |
762 | break; |
763 | |
764 | case 2: |
765 | /* SignalConversion generated from: '<S3>/voted_fb' */ |
766 | for (i = 0; i < 5; i++) { |
767 | rty_voted_fb[i] = rtb_Avg_Value_caller[i]; |
768 | } |
769 | break; |
770 | |
771 | default: |
772 | /* no actions */ |
773 | break; |
774 | } |
775 | |
776 | /* End of MultiPortSwitch: '<S3>/MultiportSwitch' */ |
777 | } |
778 | |
779 | /* Disable for referenced model: 'AHRSControl' */ |
780 | void AHRSControl_Disable(void) |
781 | { |
782 | /* Disable for S-Function (sfun_private_function_caller) generated from: '<Root>/AHRS_Control' incorporates: |
783 | * SubSystem: '<Root>/AHRS_Control' |
784 | */ |
785 | /* Disable for S-Function (sfun_private_function_caller) generated from: '<S1>/AHRSLoopCtrl' incorporates: |
786 | * SubSystem: '<S1>/AHRSLoopCtrl' |
787 | */ |
788 | AHRSContro_AHRSLoopCtrl_Disable(); |
789 | |
790 | /* End of Disable for S-Function (sfun_private_function_caller) generated from: '<S1>/AHRSLoopCtrl' */ |
791 | /* End of Disable for S-Function (sfun_private_function_caller) generated from: '<Root>/AHRS_Control' */ |
792 | } |
793 | |
794 | /* Output and update for referenced model: 'AHRSControl' */ |
795 | void AHRSControl_AHRS_Control(const int16_T rtu_Pilot_theta_cmd, const int16_T |
796 | rtu_Pilot_phi_cmd, const int16_T rtu_Pilot_r_cmd, const AHRS_Bus *rtu_AHRS1, |
797 | const AHRS_Bus *rtu_AHRS2, const AHRS_Bus *rtu_AHRS3, real_T |
798 | rty_actuator_commands[3]) |
799 | { |
800 | AHRS_Bus rtb_TmpSignalConversionAtAHRS_c; |
801 | AHRS_Bus rtb_TmpSignalConversionAtAHRS_i; |
802 | AHRS_Bus rtb_TmpSignalConversionAtAHRS_e; |
803 | real_T rtb_AHRS_voter_caller[5]; |
804 | |
805 | /* Outputs for Function Call SubSystem: '<Root>/AHRS_Control' */ |
806 | /* SignalConversion generated from: '<S1>/AHRS1' */ |
807 | rtb_TmpSignalConversionAtAHRS_c = *rtu_AHRS1; |
808 | |
809 | /* SignalConversion generated from: '<S1>/AHRS2' */ |
810 | rtb_TmpSignalConversionAtAHRS_i = *rtu_AHRS2; |
811 | |
812 | /* SignalConversion generated from: '<S1>/AHRS3' */ |
813 | rtb_TmpSignalConversionAtAHRS_e = *rtu_AHRS3; |
814 | |
815 | /* FunctionCaller: '<S1>/AHRS_voter_caller' */ |
816 | AHRSControl_AHRS_voter(&rtb_TmpSignalConversionAtAHRS_c, |
817 | &rtb_TmpSignalConversionAtAHRS_i, &rtb_TmpSignalConversionAtAHRS_e, |
818 | rtb_AHRS_voter_caller); |
819 | |
820 | /* SignalConversion generated from: '<S1>/actuator_commands' incorporates: |
821 | * FunctionCaller: '<S1>/AHRSLoopCtrl_caller' |
822 | * SignalConversion generated from: '<S1>/Pilot_phi_cmd' |
823 | * SignalConversion generated from: '<S1>/Pilot_r_cmd' |
824 | * SignalConversion generated from: '<S1>/Pilot_theta_cmd' |
825 | */ |
826 | AHRSControl_AHRSLoopCtrl(rtu_Pilot_theta_cmd, rtu_Pilot_phi_cmd, |
827 | rtu_Pilot_r_cmd, rtb_AHRS_voter_caller, rty_actuator_commands); |
828 | |
829 | /* End of Outputs for SubSystem: '<Root>/AHRS_Control' */ |
830 | } |
831 | |
832 | /* |
833 | * File trailer for generated code. |
834 | * |
835 | * [EOF] |
836 | */ |
837 | |