1/*
2 * FoilControlRaspberrySentinel.c
3 *
4 * Classroom License -- for classroom instructional use only. Not for
5 * government, commercial, academic research, or other organizational use.
6 *
7 * Code generation for model "FoilControlRaspberrySentinel".
8 *
9 * Model version : 1.24
10 * Simulink Coder version : 8.11 (R2016b) 25-Aug-2016
11 * C source code generated on : Wed Nov 23 14:06:32 2016
12 *
13 * Target selection: ert.tlc
14 * Note: GRT includes extra infrastructure and instrumentation for prototyping
15 * Embedded hardware selection: ARM Compatible->ARM Cortex
16 * Code generation objectives: Unspecified
17 * Validation result: Not run
18 */
19
20#include "FoilControlRaspberrySentinel.h"
21#include "FoilControlRaspberrySentinel_private.h"
22#include "FoilControlRaspberrySentinel_dt.h"
23#define FoilControlRaspberryS_PinNumber (17.0)
24
25/* Block signals (auto storage) */
26B_FoilControlRaspberrySentine_T FoilControlRaspberrySentinel_B;
27
28/* Block states (auto storage) */
29DW_FoilControlRaspberrySentin_T FoilControlRaspberrySentinel_DW;
30
31/* Real-time model */
32RT_MODEL_FoilControlRaspberry_T FoilControlRaspberrySentinel_M_;
33RT_MODEL_FoilControlRaspberry_T *const FoilControlRaspberrySentinel_M =
34 &FoilControlRaspberrySentinel_M_;
35real_T rt_powd_snf(real_T u0, real_T u1)
36{
37 real_T y;
38 real_T tmp;
39 real_T tmp_0;
40 if (rtIsNaN(u0) || rtIsNaN(u1)) {
41 y = (rtNaN);
42 } else {
43 tmp = fabs(u0);
44 tmp_0 = fabs(u1);
45 if (rtIsInf(u1)) {
46 if (tmp == 1.0) {
47 y = (rtNaN);
48 } else if (tmp > 1.0) {
49 if (u1 > 0.0) {
50 y = (rtInf);
51 } else {
52 y = 0.0;
53 }
54 } else if (u1 > 0.0) {
55 y = 0.0;
56 } else {
57 y = (rtInf);
58 }
59 } else if (tmp_0 == 0.0) {
60 y = 1.0;
61 } else if (tmp_0 == 1.0) {
62 if (u1 > 0.0) {
63 y = u0;
64 } else {
65 y = 1.0 / u0;
66 }
67 } else if (u1 == 2.0) {
68 y = u0 * u0;
69 } else if ((u1 == 0.5) && (u0 >= 0.0)) {
70 y = sqrt(u0);
71 } else if ((u0 < 0.0) && (u1 > floor(u1))) {
72 y = (rtNaN);
73 } else {
74 y = pow(u0, u1);
75 }
76 }
77
78 return y;
79}
80
81/* Model step function */
82void FoilControlRaspberrySentinel_step(void)
83{
84 int32_T i;
85 int32_T xoffset;
86 uint8_T rtb_FixPtSum1;
87 real_T rtb_FilterCoefficient_idx_0;
88 real_T rtb_FilterCoefficient_idx_1;
89 real_T rtb_Sum_idx_0;
90 real_T rtb_Sum_idx_1;
91
92 /* S-Function (sfcn_getFoilCheck): '<Root>/MagMan Foil with error' */
93 sfcn_getFoilCheck_Outputs_wrapper
94 ( &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o1,
95 &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2,
96 &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o3,
97 &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o4,
98 &FoilControlRaspberrySentinel_DW.MagManFoilwitherror_DSTATE);
99
100 /* MATLAB Function: '<Root>/ht1' incorporates:
101 * Constant: '<Root>/Sentinel Calibration matrix'
102 */
103 /* MATLAB Function 'ht1': '<S6>:1' */
104 /* out=km2*[x;y;x^2;y^2;x*y;1]; */
105 /* '<S6>:1:4' xa = [x,y]; */
106 /* '<S6>:1:5' H = km4(:); */
107 /* '<S6>:1:6' xa = [xa,ones(size(xa,1),1)]; */
108 /* homogeneous coordinates */
109 /* '<S6>:1:7' out = [ (xa*H(1:3))./(xa*H(7:9)), (xa*H(4:6))./(xa*H(7:9)) ]; */
110 /* '<S6>:1:9' X=out(1); */
111 FoilControlRaspberrySentinel_B.X =
112 ((FoilControlRaspberrySentinel_B.MagManFoilwitherror_o1 *
113 FoilControlRaspberrySentinel_P.km4pi[0] +
114 FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2 *
115 FoilControlRaspberrySentinel_P.km4pi[1]) +
116 FoilControlRaspberrySentinel_P.km4pi[2]) /
117 ((FoilControlRaspberrySentinel_B.MagManFoilwitherror_o1 *
118 FoilControlRaspberrySentinel_P.km4pi[6] +
119 FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2 *
120 FoilControlRaspberrySentinel_P.km4pi[7]) +
121 FoilControlRaspberrySentinel_P.km4pi[8]);
122
123 /* '<S6>:1:10' Y=out(2); */
124 FoilControlRaspberrySentinel_B.Y =
125 ((FoilControlRaspberrySentinel_B.MagManFoilwitherror_o1 *
126 FoilControlRaspberrySentinel_P.km4pi[3] +
127 FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2 *
128 FoilControlRaspberrySentinel_P.km4pi[4]) +
129 FoilControlRaspberrySentinel_P.km4pi[5]) /
130 ((FoilControlRaspberrySentinel_B.MagManFoilwitherror_o1 *
131 FoilControlRaspberrySentinel_P.km4pi[6] +
132 FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2 *
133 FoilControlRaspberrySentinel_P.km4pi[7]) +
134 FoilControlRaspberrySentinel_P.km4pi[8]);
135
136 /* MATLAB Function: '<S7>/Embedded MATLAB Function' */
137 /* MATLAB Function 'linearization/Embedded MATLAB Function': '<S12>:1' */
138 /* '<S12>:1:2' Ao=zeros(2,16); */
139 memset(&FoilControlRaspberrySentinel_B.Ao[0], 0, sizeof(real_T) << 5U);
140
141 /* '<S12>:1:4' for iy=1:4 */
142 for (xoffset = 0; xoffset < 4; xoffset++) {
143 /* '<S12>:1:5' for ix=1:4 */
144 for (i = 0; i < 4; i++) {
145 /* '<S12>:1:6' [cFx, cFy] = fForce(x, y, ix, iy); */
146 /* '<S12>:1:14' d=dist(x,y, ix, iy); */
147 /* '<S12>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */
148 FoilControlRaspberrySentinel_B.lim = FoilControlRaspberrySentinel_B.X -
149 (1.0 + (real_T)i);
150 rtb_Sum_idx_1 = FoilControlRaspberrySentinel_B.Y - (1.0 + (real_T)xoffset);
151 FoilControlRaspberrySentinel_B.lim = sqrt
152 (FoilControlRaspberrySentinel_B.lim * FoilControlRaspberrySentinel_B.lim
153 + rtb_Sum_idx_1 * rtb_Sum_idx_1);
154
155 /* '<S12>:1:15' if d~=0 */
156 if (FoilControlRaspberrySentinel_B.lim != 0.0) {
157 /* '<S12>:1:16' F=gForce(d); */
158 /* '<S12>:1:43' a = 622.06; */
159 /* '<S12>:1:44' b = 9.5032; */
160 /* '<S12>:1:45' c = 3.1884; */
161 /* '<S12>:1:46' F = x.*a./(b.*x.^2+ c).^3; */
162 FoilControlRaspberrySentinel_B.lim = FoilControlRaspberrySentinel_B.lim *
163 622.06 / rt_powd_snf(FoilControlRaspberrySentinel_B.lim *
164 FoilControlRaspberrySentinel_B.lim * 9.5032 + 3.1884, 3.0);
165
166 /* '<S12>:1:17' Fv=F*vector(x,y,ix,iy); */
167 /* '<S12>:1:32' vx=ix-x; */
168 rtb_Sum_idx_1 = (1.0 + (real_T)i) - FoilControlRaspberrySentinel_B.X;
169
170 /* '<S12>:1:33' vy=iy-y; */
171 rtb_FilterCoefficient_idx_0 = (1.0 + (real_T)xoffset) -
172 FoilControlRaspberrySentinel_B.Y;
173
174 /* '<S12>:1:35' if(vx==0 && vy ==0) */
175 if ((rtb_Sum_idx_1 == 0.0) && (rtb_FilterCoefficient_idx_0 == 0.0)) {
176 /* '<S12>:1:36' v=[0 0]; */
177 rtb_Sum_idx_0 = 0.0;
178 rtb_Sum_idx_1 = 0.0;
179 } else {
180 /* '<S12>:1:37' else */
181 /* '<S12>:1:38' v = [vx vy]/dist(vx, vy, 0 ,0); */
182 /* '<S12>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */
183 rtb_FilterCoefficient_idx_1 = sqrt(rtb_Sum_idx_1 * rtb_Sum_idx_1 +
184 rtb_FilterCoefficient_idx_0 * rtb_FilterCoefficient_idx_0);
185 rtb_Sum_idx_0 = rtb_Sum_idx_1 / rtb_FilterCoefficient_idx_1;
186 rtb_Sum_idx_1 = rtb_FilterCoefficient_idx_0 /
187 rtb_FilterCoefficient_idx_1;
188 }
189
190 rtb_Sum_idx_0 *= FoilControlRaspberrySentinel_B.lim;
191 rtb_Sum_idx_1 *= FoilControlRaspberrySentinel_B.lim;
192
193 /* '<S12>:1:18' Fx=Fv(1); */
194 FoilControlRaspberrySentinel_B.lim = rtb_Sum_idx_0;
195
196 /* '<S12>:1:19' Fy=Fv(2); */
197 } else {
198 /* '<S12>:1:20' else */
199 /* '<S12>:1:21' Fx=0; */
200 FoilControlRaspberrySentinel_B.lim = 0.0;
201
202 /* '<S12>:1:22' Fy=0; */
203 rtb_Sum_idx_1 = 0.0;
204 }
205
206 /* '<S12>:1:7' Ao(1,(iy-1)*4+ix) = cFx; */
207 FoilControlRaspberrySentinel_B.Ao[((xoffset << 2) + i) << 1] =
208 FoilControlRaspberrySentinel_B.lim;
209
210 /* '<S12>:1:8' Ao(2,(iy-1)*4+ix) = cFy; */
211 FoilControlRaspberrySentinel_B.Ao[1 + (((xoffset << 2) + i) << 1)] =
212 rtb_Sum_idx_1;
213 }
214 }
215
216 /* End of MATLAB Function: '<S7>/Embedded MATLAB Function' */
217
218 /* Gain: '<S5>/Filter Coefficient' incorporates:
219 * Constant: '<Root>/Constant'
220 * Constant: '<Root>/Constant1'
221 * DiscreteIntegrator: '<S5>/Filter'
222 * Gain: '<S5>/Derivative Gain'
223 * Gain: '<S5>/Setpoint Weighting (Derivative)'
224 * Sum: '<S5>/Sum3'
225 * Sum: '<S5>/SumD'
226 */
227 rtb_FilterCoefficient_idx_0 =
228 ((FoilControlRaspberrySentinel_P.PIDController2DOF1_c *
229 FoilControlRaspberrySentinel_P.Constant_Value -
230 FoilControlRaspberrySentinel_B.X) *
231 FoilControlRaspberrySentinel_P.PIDController2DOF1_D -
232 FoilControlRaspberrySentinel_DW.Filter_DSTATE[0]) *
233 FoilControlRaspberrySentinel_P.PIDController2DOF1_N;
234 rtb_FilterCoefficient_idx_1 =
235 ((FoilControlRaspberrySentinel_P.PIDController2DOF1_c *
236 FoilControlRaspberrySentinel_P.Constant1_Value -
237 FoilControlRaspberrySentinel_B.Y) *
238 FoilControlRaspberrySentinel_P.PIDController2DOF1_D -
239 FoilControlRaspberrySentinel_DW.Filter_DSTATE[1]) *
240 FoilControlRaspberrySentinel_P.PIDController2DOF1_N;
241
242 /* Sum: '<S5>/Sum' incorporates:
243 * Constant: '<Root>/Constant'
244 * Constant: '<Root>/Constant1'
245 * DiscreteIntegrator: '<S5>/Integrator'
246 * Gain: '<S5>/Proportional Gain'
247 * Gain: '<S5>/Setpoint Weighting (Proportional)'
248 * Sum: '<S5>/Sum1'
249 */
250 rtb_Sum_idx_0 = ((FoilControlRaspberrySentinel_P.PIDController2DOF1_b *
251 FoilControlRaspberrySentinel_P.Constant_Value -
252 FoilControlRaspberrySentinel_B.X) *
253 FoilControlRaspberrySentinel_P.PIDController2DOF1_P +
254 FoilControlRaspberrySentinel_DW.Integrator_DSTATE[0]) +
255 rtb_FilterCoefficient_idx_0;
256 rtb_Sum_idx_1 = ((FoilControlRaspberrySentinel_P.PIDController2DOF1_b *
257 FoilControlRaspberrySentinel_P.Constant1_Value -
258 FoilControlRaspberrySentinel_B.Y) *
259 FoilControlRaspberrySentinel_P.PIDController2DOF1_P +
260 FoilControlRaspberrySentinel_DW.Integrator_DSTATE[1]) +
261 rtb_FilterCoefficient_idx_1;
262
263 /* Saturate: '<S5>/Saturate' */
264 if (rtb_Sum_idx_0 >
265 FoilControlRaspberrySentinel_P.PIDController2DOF1_UpperSaturat) {
266 FoilControlRaspberrySentinel_B.Saturate[0] =
267 FoilControlRaspberrySentinel_P.PIDController2DOF1_UpperSaturat;
268 } else if (rtb_Sum_idx_0 <
269 FoilControlRaspberrySentinel_P.PIDController2DOF1_LowerSaturat) {
270 FoilControlRaspberrySentinel_B.Saturate[0] =
271 FoilControlRaspberrySentinel_P.PIDController2DOF1_LowerSaturat;
272 } else {
273 FoilControlRaspberrySentinel_B.Saturate[0] = rtb_Sum_idx_0;
274 }
275
276 if (rtb_Sum_idx_1 >
277 FoilControlRaspberrySentinel_P.PIDController2DOF1_UpperSaturat) {
278 FoilControlRaspberrySentinel_B.Saturate[1] =
279 FoilControlRaspberrySentinel_P.PIDController2DOF1_UpperSaturat;
280 } else if (rtb_Sum_idx_1 <
281 FoilControlRaspberrySentinel_P.PIDController2DOF1_LowerSaturat) {
282 FoilControlRaspberrySentinel_B.Saturate[1] =
283 FoilControlRaspberrySentinel_P.PIDController2DOF1_LowerSaturat;
284 } else {
285 FoilControlRaspberrySentinel_B.Saturate[1] = rtb_Sum_idx_1;
286 }
287
288 /* End of Saturate: '<S5>/Saturate' */
289
290 /* S-Function (cvxfasd): '<S7>/MagMan linearization' */
291 cvxfasd_Outputs_wrapper(&FoilControlRaspberrySentinel_B.Ao[0],
292 &FoilControlRaspberrySentinel_B.Saturate[0],
293 &FoilControlRaspberrySentinel_B.Saturate[1],
294 &FoilControlRaspberrySentinel_B.MagManlinearization_o1[0],
295 &FoilControlRaspberrySentinel_B.MagManlinearization_o2 );
296
297 /* MATLAB Function: '<Root>/pwm' */
298 /* MATLAB Function 'pwm': '<S9>:1' */
299 /* conversion from scaling factor to duty cycle of PWM */
300 /* '<S9>:1:3' x = max(0, min(1,x) ); */
301 /* limits */
302 /* '<S9>:1:4' i = 0.18515*x + 0.069587*x.^3 + 0.19285*x.^(1/3); */
303 /* ratio to current conversion */
304 /* '<S9>:1:5' pw = i/0.455/2; */
305 /* '<S9>:1:6' pw(pw>0.05) = pw(pw>0.05) + 0.5; */
306 /* '<S9>:1:7' pw(pw<0.5) = 0; */
307 for (xoffset = 0; xoffset < 16; xoffset++) {
308 if ((1.0 <= FoilControlRaspberrySentinel_B.MagManlinearization_o1[xoffset]) ||
309 rtIsNaN(FoilControlRaspberrySentinel_B.MagManlinearization_o1[xoffset]))
310 {
311 FoilControlRaspberrySentinel_B.lim = 1.0;
312 } else {
313 FoilControlRaspberrySentinel_B.lim =
314 FoilControlRaspberrySentinel_B.MagManlinearization_o1[xoffset];
315 }
316
317 if ((0.0 >= FoilControlRaspberrySentinel_B.lim) || rtIsNaN
318 (FoilControlRaspberrySentinel_B.lim)) {
319 FoilControlRaspberrySentinel_B.pw[xoffset] = 0.0;
320 } else {
321 FoilControlRaspberrySentinel_B.pw[xoffset] =
322 FoilControlRaspberrySentinel_B.lim;
323 }
324
325 FoilControlRaspberrySentinel_B.pw[xoffset] = ((0.18515 *
326 FoilControlRaspberrySentinel_B.pw[xoffset] + 0.069587 * rt_powd_snf
327 (FoilControlRaspberrySentinel_B.pw[xoffset], 3.0)) + 0.19285 * rt_powd_snf
328 (FoilControlRaspberrySentinel_B.pw[xoffset], 0.33333333333333331)) / 0.455
329 / 2.0;
330 if (FoilControlRaspberrySentinel_B.pw[xoffset] > 0.05) {
331 FoilControlRaspberrySentinel_B.pw[xoffset] += 0.5;
332 }
333
334 if (FoilControlRaspberrySentinel_B.pw[xoffset] < 0.5) {
335 FoilControlRaspberrySentinel_B.pw[xoffset] = 0.0;
336 }
337 }
338
339 /* '<S9>:1:8' pw=pw'; */
340 for (i = 0; i < 4; i++) {
341 FoilControlRaspberrySentinel_B.y[i << 2] =
342 FoilControlRaspberrySentinel_B.pw[i];
343 FoilControlRaspberrySentinel_B.y[1 + (i << 2)] =
344 FoilControlRaspberrySentinel_B.pw[i + 4];
345 FoilControlRaspberrySentinel_B.y[2 + (i << 2)] =
346 FoilControlRaspberrySentinel_B.pw[i + 8];
347 FoilControlRaspberrySentinel_B.y[3 + (i << 2)] =
348 FoilControlRaspberrySentinel_B.pw[i + 12];
349 }
350
351 for (i = 0; i < 4; i++) {
352 FoilControlRaspberrySentinel_B.pw[i << 2] =
353 FoilControlRaspberrySentinel_B.y[i << 2];
354 FoilControlRaspberrySentinel_B.pw[1 + (i << 2)] =
355 FoilControlRaspberrySentinel_B.y[(i << 2) + 1];
356 FoilControlRaspberrySentinel_B.pw[2 + (i << 2)] =
357 FoilControlRaspberrySentinel_B.y[(i << 2) + 2];
358 FoilControlRaspberrySentinel_B.pw[3 + (i << 2)] =
359 FoilControlRaspberrySentinel_B.y[(i << 2) + 3];
360 }
361
362 /* End of MATLAB Function: '<Root>/pwm' */
363
364 /* Start for MATLABSystem: '<S4>/Digital Write' incorporates:
365 * Constant: '<S1>/Constant'
366 * Constant: '<S2>/Constant'
367 * Logic: '<Root>/Logical Operator'
368 * MATLABSystem: '<S4>/Digital Write'
369 * RelationalOperator: '<S1>/Compare'
370 * RelationalOperator: '<S2>/Compare'
371 * UnitDelay: '<S3>/Output'
372 */
373 MW_gpioWrite((uint32_T)FoilControlRaspberryS_PinNumber, (uint8_T)
374 ((FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2 >
375 FoilControlRaspberrySentinel_P.Constant_Value_a) &&
376 (FoilControlRaspberrySentinel_DW.Output_DSTATE >
377 FoilControlRaspberrySentinel_P.Constant_Value_p)));
378
379 /* MATLAB Function: '<Root>/proud' */
380 /* MATLAB Function 'proud': '<S8>:1' */
381 /* '<S8>:1:2' i = (max(0, pw-0.5).*(0.455*2))'; */
382 for (xoffset = 0; xoffset < 16; xoffset++) {
383 if ((0.0 >= FoilControlRaspberrySentinel_B.pw[xoffset] - 0.5) || rtIsNaN
384 (FoilControlRaspberrySentinel_B.pw[xoffset] - 0.5)) {
385 FoilControlRaspberrySentinel_B.y[xoffset] = 0.0;
386 } else {
387 FoilControlRaspberrySentinel_B.y[xoffset] =
388 FoilControlRaspberrySentinel_B.pw[xoffset] - 0.5;
389 }
390 }
391
392 for (i = 0; i < 4; i++) {
393 FoilControlRaspberrySentinel_B.i[i << 2] =
394 FoilControlRaspberrySentinel_B.y[i] * 0.91;
395 FoilControlRaspberrySentinel_B.i[1 + (i << 2)] =
396 FoilControlRaspberrySentinel_B.y[i + 4] * 0.91;
397 FoilControlRaspberrySentinel_B.i[2 + (i << 2)] =
398 FoilControlRaspberrySentinel_B.y[i + 8] * 0.91;
399 FoilControlRaspberrySentinel_B.i[3 + (i << 2)] =
400 FoilControlRaspberrySentinel_B.y[i + 12] * 0.91;
401 }
402
403 /* '<S8>:1:3' lim = 0; */
404 FoilControlRaspberrySentinel_B.lim = 0.0;
405
406 /* '<S8>:1:4' if sum(sum(abs(i))) < 4 */
407 memcpy(&FoilControlRaspberrySentinel_B.y[0],
408 &FoilControlRaspberrySentinel_B.i[0], sizeof(real_T) << 4U);
409 for (i = 0; i < 4; i++) {
410 xoffset = i << 2;
411 FoilControlRaspberrySentinel_B.y_m[i] =
412 ((FoilControlRaspberrySentinel_B.y[xoffset + 1] +
413 FoilControlRaspberrySentinel_B.y[xoffset]) +
414 FoilControlRaspberrySentinel_B.y[xoffset + 2]) +
415 FoilControlRaspberrySentinel_B.y[xoffset + 3];
416 }
417
418 if (((FoilControlRaspberrySentinel_B.y_m[0] +
419 FoilControlRaspberrySentinel_B.y_m[1]) +
420 FoilControlRaspberrySentinel_B.y_m[2]) +
421 FoilControlRaspberrySentinel_B.y_m[3] < 4.0) {
422 /* '<S8>:1:5' lim = 1; */
423 FoilControlRaspberrySentinel_B.lim = 1.0;
424 }
425
426 /* End of MATLAB Function: '<Root>/proud' */
427
428 /* Switch: '<Root>/Switch1' incorporates:
429 * Constant: '<Root>/Constant2'
430 */
431 if (FoilControlRaspberrySentinel_B.lim != 0.0) {
432 memcpy(&FoilControlRaspberrySentinel_B.Switch1[0],
433 &FoilControlRaspberrySentinel_B.pw[0], sizeof(real_T) << 4U);
434 } else {
435 memcpy(&FoilControlRaspberrySentinel_B.Switch1[0],
436 &FoilControlRaspberrySentinel_P.Constant2_Value[0], sizeof(real_T) <<
437 4U);
438 }
439
440 /* End of Switch: '<Root>/Switch1' */
441
442 /* S-Function (sfcn_setPlatform): '<Root>/MagMan Coils' */
443 sfcn_setPlatform_Outputs_wrapper(&FoilControlRaspberrySentinel_B.Switch1[0],
444 &FoilControlRaspberrySentinel_DW.MagManCoils_DSTATE);
445
446 /* Sum: '<S10>/FixPt Sum1' incorporates:
447 * Constant: '<S10>/FixPt Constant'
448 * UnitDelay: '<S3>/Output'
449 */
450 rtb_FixPtSum1 = (uint8_T)((uint32_T)
451 FoilControlRaspberrySentinel_DW.Output_DSTATE +
452 FoilControlRaspberrySentinel_P.FixPtConstant_Value);
453
454 /* S-Function "sfcn_getFoilCheck_wrapper" Block: <Root>/MagMan Foil with error */
455 sfcn_getFoilCheck_Update_wrapper
456 ( &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o1,
457 &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2,
458 &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o3,
459 &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o4,
460 &FoilControlRaspberrySentinel_DW.MagManFoilwitherror_DSTATE);
461
462 /* Update for DiscreteIntegrator: '<S5>/Integrator' incorporates:
463 * Constant: '<Root>/Constant'
464 * Gain: '<S5>/Integral Gain'
465 * Gain: '<S5>/Kb'
466 * Sum: '<S5>/Sum2'
467 * Sum: '<S5>/SumI1'
468 * Sum: '<S5>/SumI2'
469 */
470 FoilControlRaspberrySentinel_DW.Integrator_DSTATE[0] +=
471 ((FoilControlRaspberrySentinel_P.Constant_Value -
472 FoilControlRaspberrySentinel_B.X) *
473 FoilControlRaspberrySentinel_P.PIDController2DOF1_I +
474 (FoilControlRaspberrySentinel_B.Saturate[0] - rtb_Sum_idx_0) *
475 FoilControlRaspberrySentinel_P.PIDController2DOF1_Kb) *
476 FoilControlRaspberrySentinel_P.Integrator_gainval;
477
478 /* Update for DiscreteIntegrator: '<S5>/Filter' */
479 FoilControlRaspberrySentinel_DW.Filter_DSTATE[0] +=
480 FoilControlRaspberrySentinel_P.Filter_gainval * rtb_FilterCoefficient_idx_0;
481
482 /* Update for DiscreteIntegrator: '<S5>/Integrator' incorporates:
483 * Constant: '<Root>/Constant1'
484 * Gain: '<S5>/Integral Gain'
485 * Gain: '<S5>/Kb'
486 * Sum: '<S5>/Sum2'
487 * Sum: '<S5>/SumI1'
488 * Sum: '<S5>/SumI2'
489 */
490 FoilControlRaspberrySentinel_DW.Integrator_DSTATE[1] +=
491 ((FoilControlRaspberrySentinel_P.Constant1_Value -
492 FoilControlRaspberrySentinel_B.Y) *
493 FoilControlRaspberrySentinel_P.PIDController2DOF1_I +
494 (FoilControlRaspberrySentinel_B.Saturate[1] - rtb_Sum_idx_1) *
495 FoilControlRaspberrySentinel_P.PIDController2DOF1_Kb) *
496 FoilControlRaspberrySentinel_P.Integrator_gainval;
497
498 /* Update for DiscreteIntegrator: '<S5>/Filter' */
499 FoilControlRaspberrySentinel_DW.Filter_DSTATE[1] +=
500 FoilControlRaspberrySentinel_P.Filter_gainval * rtb_FilterCoefficient_idx_1;
501
502 /* Switch: '<S11>/FixPt Switch' */
503 if (rtb_FixPtSum1 > FoilControlRaspberrySentinel_P.CounterLimited_uplimit) {
504 /* Update for UnitDelay: '<S3>/Output' incorporates:
505 * Constant: '<S11>/Constant'
506 */
507 FoilControlRaspberrySentinel_DW.Output_DSTATE =
508 FoilControlRaspberrySentinel_P.Constant_Value_h;
509 } else {
510 /* Update for UnitDelay: '<S3>/Output' */
511 FoilControlRaspberrySentinel_DW.Output_DSTATE = rtb_FixPtSum1;
512 }
513
514 /* End of Switch: '<S11>/FixPt Switch' */
515
516 /* S-Function "sfcn_setPlatform_wrapper" Block: <Root>/MagMan Coils */
517 sfcn_setPlatform_Update_wrapper(&FoilControlRaspberrySentinel_B.Switch1[0],
518 &FoilControlRaspberrySentinel_DW.MagManCoils_DSTATE);
519
520 /* External mode */
521 rtExtModeUploadCheckTrigger(1);
522
523 { /* Sample time: [0.02s, 0.0s] */
524 rtExtModeUpload(0, FoilControlRaspberrySentinel_M->Timing.taskTime0);
525 }
526
527 /* signal main to stop simulation */
528 { /* Sample time: [0.02s, 0.0s] */
529 if ((rtmGetTFinal(FoilControlRaspberrySentinel_M)!=-1) &&
530 !((rtmGetTFinal(FoilControlRaspberrySentinel_M)-
531 FoilControlRaspberrySentinel_M->Timing.taskTime0) >
532 FoilControlRaspberrySentinel_M->Timing.taskTime0 * (DBL_EPSILON))) {
533 rtmSetErrorStatus(FoilControlRaspberrySentinel_M, "Simulation finished");
534 }
535
536 if (rtmGetStopRequested(FoilControlRaspberrySentinel_M)) {
537 rtmSetErrorStatus(FoilControlRaspberrySentinel_M, "Simulation finished");
538 }
539 }
540
541 /* Update absolute time for base rate */
542 /* The "clockTick0" counts the number of times the code of this task has
543 * been executed. The absolute time is the multiplication of "clockTick0"
544 * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
545 * overflow during the application lifespan selected.
546 * Timer of this task consists of two 32 bit unsigned integers.
547 * The two integers represent the low bits Timing.clockTick0 and the high bits
548 * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
549 */
550 if (!(++FoilControlRaspberrySentinel_M->Timing.clockTick0)) {
551 ++FoilControlRaspberrySentinel_M->Timing.clockTickH0;
552 }
553
554 FoilControlRaspberrySentinel_M->Timing.taskTime0 =
555 FoilControlRaspberrySentinel_M->Timing.clockTick0 *
556 FoilControlRaspberrySentinel_M->Timing.stepSize0 +
557 FoilControlRaspberrySentinel_M->Timing.clockTickH0 *
558 FoilControlRaspberrySentinel_M->Timing.stepSize0 * 4294967296.0;
559}
560
561/* Model initialize function */
562void FoilControlRaspberrySentinel_initialize(void)
563{
564 /* Registration code */
565
566 /* initialize non-finites */
567 rt_InitInfAndNaN(sizeof(real_T));
568
569 /* initialize real-time model */
570 (void) memset((void *)FoilControlRaspberrySentinel_M, 0,
571 sizeof(RT_MODEL_FoilControlRaspberry_T));
572 rtmSetTFinal(FoilControlRaspberrySentinel_M, -1);
573 FoilControlRaspberrySentinel_M->Timing.stepSize0 = 0.02;
574
575 /* External mode info */
576 FoilControlRaspberrySentinel_M->Sizes.checksums[0] = (495282541U);
577 FoilControlRaspberrySentinel_M->Sizes.checksums[1] = (2055743715U);
578 FoilControlRaspberrySentinel_M->Sizes.checksums[2] = (3829557736U);
579 FoilControlRaspberrySentinel_M->Sizes.checksums[3] = (3329260777U);
580
581 {
582 static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE;
583 static RTWExtModeInfo rt_ExtModeInfo;
584 static const sysRanDType *systemRan[8];
585 FoilControlRaspberrySentinel_M->extModeInfo = (&rt_ExtModeInfo);
586 rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan);
587 systemRan[0] = &rtAlwaysEnabled;
588 systemRan[1] = &rtAlwaysEnabled;
589 systemRan[2] = &rtAlwaysEnabled;
590 systemRan[3] = &rtAlwaysEnabled;
591 systemRan[4] = &rtAlwaysEnabled;
592 systemRan[5] = &rtAlwaysEnabled;
593 systemRan[6] = &rtAlwaysEnabled;
594 systemRan[7] = &rtAlwaysEnabled;
595 rteiSetModelMappingInfoPtr(FoilControlRaspberrySentinel_M->extModeInfo,
596 &FoilControlRaspberrySentinel_M->SpecialInfo.mappingInfo);
597 rteiSetChecksumsPtr(FoilControlRaspberrySentinel_M->extModeInfo,
598 FoilControlRaspberrySentinel_M->Sizes.checksums);
599 rteiSetTPtr(FoilControlRaspberrySentinel_M->extModeInfo, rtmGetTPtr
600 (FoilControlRaspberrySentinel_M));
601 }
602
603 /* block I/O */
604 (void) memset(((void *) &FoilControlRaspberrySentinel_B), 0,
605 sizeof(B_FoilControlRaspberrySentine_T));
606
607 /* states (dwork) */
608 (void) memset((void *)&FoilControlRaspberrySentinel_DW, 0,
609 sizeof(DW_FoilControlRaspberrySentin_T));
610
611 /* data type transition information */
612 {
613 static DataTypeTransInfo dtInfo;
614 (void) memset((char_T *) &dtInfo, 0,
615 sizeof(dtInfo));
616 FoilControlRaspberrySentinel_M->SpecialInfo.mappingInfo = (&dtInfo);
617 dtInfo.numDataTypes = 15;
618 dtInfo.dataTypeSizes = &rtDataTypeSizes[0];
619 dtInfo.dataTypeNames = &rtDataTypeNames[0];
620
621 /* Block I/O transition table */
622 dtInfo.BTransTable = &rtBTransTable;
623
624 /* Parameters transition table */
625 dtInfo.PTransTable = &rtPTransTable;
626 }
627
628 /* Start for MATLABSystem: '<S4>/Digital Write' */
629 FoilControlRaspberrySentinel_DW.obj.isInitialized = 0;
630 FoilControlRaspberrySentinel_DW.objisempty = true;
631 FoilControlRaspberrySentinel_DW.obj.isInitialized = 1;
632 MW_gpioInit((uint32_T)FoilControlRaspberryS_PinNumber, true);
633
634 /* S-Function Block: <Root>/MagMan Foil with error */
635 {
636 real_T initVector[1] = { 0 };
637
638 {
639 int_T i1;
640 for (i1=0; i1 < 1; i1++) {
641 FoilControlRaspberrySentinel_DW.MagManFoilwitherror_DSTATE = initVector
642 [0];
643 }
644 }
645 }
646
647 /* InitializeConditions for DiscreteIntegrator: '<S5>/Integrator' */
648 FoilControlRaspberrySentinel_DW.Integrator_DSTATE[0] =
649 FoilControlRaspberrySentinel_P.Integrator_IC;
650
651 /* InitializeConditions for DiscreteIntegrator: '<S5>/Filter' */
652 FoilControlRaspberrySentinel_DW.Filter_DSTATE[0] =
653 FoilControlRaspberrySentinel_P.Filter_IC;
654
655 /* InitializeConditions for DiscreteIntegrator: '<S5>/Integrator' */
656 FoilControlRaspberrySentinel_DW.Integrator_DSTATE[1] =
657 FoilControlRaspberrySentinel_P.Integrator_IC;
658
659 /* InitializeConditions for DiscreteIntegrator: '<S5>/Filter' */
660 FoilControlRaspberrySentinel_DW.Filter_DSTATE[1] =
661 FoilControlRaspberrySentinel_P.Filter_IC;
662
663 /* InitializeConditions for UnitDelay: '<S3>/Output' */
664 FoilControlRaspberrySentinel_DW.Output_DSTATE =
665 FoilControlRaspberrySentinel_P.Output_InitialCondition;
666
667 /* S-Function Block: <Root>/MagMan Coils */
668 {
669 real_T initVector[1] = { 0 };
670
671 {
672 int_T i1;
673 for (i1=0; i1 < 1; i1++) {
674 FoilControlRaspberrySentinel_DW.MagManCoils_DSTATE = initVector[0];
675 }
676 }
677 }
678}
679
680/* Model terminate function */
681void FoilControlRaspberrySentinel_terminate(void)
682{
683 /* Start for MATLABSystem: '<S4>/Digital Write' incorporates:
684 * Terminate for MATLABSystem: '<S4>/Digital Write'
685 */
686 if (FoilControlRaspberrySentinel_DW.obj.isInitialized == 1) {
687 FoilControlRaspberrySentinel_DW.obj.isInitialized = 2;
688 MW_gpioTerminate((uint32_T)FoilControlRaspberryS_PinNumber);
689 }
690
691 /* End of Start for MATLABSystem: '<S4>/Digital Write' */
692}
693