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) */
25DW_AHRSControl_f_T AHRSControl_DW;
26extern void AHRSControl_Filter(real_T rtu_Signal, real_T rtu_prev_FeedbackSum,
27 real_T *rty_filtered_Signal, real_T *rty_FeedbackSum);
28extern void AHRSControl_pitch_feedback(real_T rtu_theta_cmd, const real_T
29 rtu_voted_fb[5], real_T *rty_ds);
30extern void AHRSControl_roll_feedback(real_T rtu_phi_cmd, const real_T
31 rtu_voted_fb[5], real_T *rty_dc);
32extern void AHRSControl_yaw_feedback(real_T rtu_r_cmd, const real_T
33 rtu_voted_fb[5], real_T *rty_dT);
34extern 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]);
36extern void AHRSControl_PitchLoop_Disable(void);
37extern void AHRSControl_PitchLoop(real_T rtu_U, real_T *rty_Y);
38extern void AHRSControl_RollLoop_Disable(void);
39extern void AHRSControl_RollLoop(real_T rtu_U, real_T *rty_Y);
40extern void AHRSControl_YawLoop_Disable(void);
41extern void AHRSControl_YawLoop(real_T rtu_U, real_T *rty_Y);
42extern void AHRSCon_Heli_outer_loop_Disable(void);
43extern 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);
46extern void AHRSContro_AHRSLoopCtrl_Disable(void);
47extern 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]);
50extern 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]);
53extern 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]);
55extern 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]);
58extern 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' */
62void 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' */
87void 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' */
122void 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' */
157void 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' */
192void 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' */
282void 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' */
289void 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' */
319void 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' */
326void 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' */
356void 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' */
363void 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' */
393void 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' */
418void 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' */
504void 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' */
515void 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' */
551void 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' */
605void 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' */
628void 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' */
681void 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' */
780void 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' */
795void 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