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: integ_accel.c
7 *
8 * Code generated for Simulink model 'integ_accel'.
9 *
10 * Model version : 1.6
11 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
12 * C/C++ source code generated on : Tue Dec 05 15:11:10 2017
13 *
14 * Target selection: ert.tlc
15 * Embedded hardware selection: ARM Compatible->ARM Cortex
16 * Code generation objectives: Unspecified
17 * Validation result: Not run
18 */
19
20#include "integ_accel.h"
21#include "integ_accel_private.h"
22#include "integ_accel_dt.h"
23
24/* Block signals (auto storage) */
25B_integ_accel_T integ_accel_B;
26
27/* Block states (auto storage) */
28DW_integ_accel_T integ_accel_DW;
29
30/* Real-time model */
31RT_MODEL_integ_accel_T integ_accel_M_;
32RT_MODEL_integ_accel_T *const integ_accel_M = &integ_accel_M_;
33
34/* Forward declaration for local functions */
35static void integ_accel_LDLf_int32_Treal_T(real_T out[], real_T vArray[],
36 int32_T nRows, const real_T in[]);
37static void rate_scheduler(void);
38
39/*
40 * This function updates active task flag for each subrate.
41 * The function is called at model base rate, hence the
42 * generated code self-manages all its subrates.
43 */
44static void rate_scheduler(void)
45{
46 /* Compute which subrates run during the next base time step. Subrates
47 * are an integer multiple of the base rate counter. Therefore, the subtask
48 * counter is reset when it reaches its limit (zero means run).
49 */
50 (integ_accel_M->Timing.TaskCounters.TID[2])++;
51 if ((integ_accel_M->Timing.TaskCounters.TID[2]) > 999) {/* Sample time: [1.0s, 0.0s] */
52 integ_accel_M->Timing.TaskCounters.TID[2] = 0;
53 }
54}
55
56static void integ_accel_LDLf_int32_Treal_T(real_T out[], real_T vArray[],
57 int32_T nRows, const real_T in[])
58{
59 int32_T c;
60 int32_T r;
61 int32_T idx1;
62 int32_T idx2;
63 boolean_T done;
64 int32_T k;
65 real_T prod;
66
67 /* S-Function (sdspldl2): '<S12>/LDL Factorization' */
68 done = false;
69
70 /* use done to control the for loop return early */
71 /* when non-positive definite eigenvalue is detected */
72 c = 0;
73 while ((c < nRows) && (!done)) {
74 idx2 = c * nRows;
75 for (r = 0; r < c; r++) {
76 idx1 = r * nRows;
77 integ_accel_B.mYTmp = out[idx1 + c];
78 integ_accel_B.mYTmpR = out[idx1 + r];
79 vArray[r] = integ_accel_B.mYTmp * integ_accel_B.mYTmpR;
80 }
81
82 integ_accel_B.mYTmp = in[idx2 + c];
83 idx1 = c;
84 for (r = 0; r < c; r++) {
85 prod = out[idx1] * vArray[r];
86 integ_accel_B.mYTmp -= prod;
87 idx1 += nRows;
88 }
89
90 integ_accel_B.mYTmpR = integ_accel_B.mYTmp;
91 if (integ_accel_B.mYTmp <= 0.0) {
92 done = true;
93 } else {
94 out[idx2 + c] = integ_accel_B.mYTmp;
95 for (r = c + 1; r < nRows; r++) {
96 integ_accel_B.mYTmp = in[idx2 + r];
97 idx1 = r;
98 for (k = 0; k < c; k++) {
99 prod = out[idx1] * vArray[k];
100 integ_accel_B.mYTmp -= prod;
101 idx1 += nRows;
102 }
103
104 out[idx2 + r] = integ_accel_B.mYTmp / integ_accel_B.mYTmpR;
105 }
106 }
107
108 c++;
109 }
110
111 /* transpose and copy lower sub-triang to upper */
112 c = 0;
113 while ((c < nRows) && (!done)) {
114 for (r = c + 1; r < nRows; r++) {
115 integ_accel_B.mYTmp = out[c * nRows + r];
116 out[r * nRows + c] = integ_accel_B.mYTmp;
117 }
118
119 c++;
120 }
121
122 /* End of S-Function (sdspldl2): '<S12>/LDL Factorization' */
123}
124
125/* Model step function */
126void integ_accel_step(void)
127{
128 boolean_T p;
129 boolean_T p_0;
130 int16_T output[3];
131 uint8_T status;
132 uint8_T output_raw[6];
133 uint8_T y[2];
134 uint8_T x[2];
135 int32_T idxV;
136 int32_T i;
137 int32_T s5_iter;
138 int16_T output_0;
139
140 /* Reset subsysRan breadcrumbs */
141 srClearBC(integ_accel_DW.Subsystem1_SubsysRanBC);
142 if (integ_accel_M->Timing.TaskCounters.TID[2] == 0) {
143 /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates:
144 * MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor'
145 */
146 p = false;
147 p_0 = true;
148 if (!(integ_accel_DW.obj.SampleTime ==
149 integ_accel_P.FXOS87006AxesSensor_SampleTime)) {
150 p_0 = false;
151 }
152
153 if (p_0) {
154 p = true;
155 }
156
157 if (!p) {
158 integ_accel_DW.obj.SampleTime =
159 integ_accel_P.FXOS87006AxesSensor_SampleTime;
160 }
161
162 output[0] = 0;
163 output[1] = 0;
164 output[2] = 0;
165 status = 1U;
166 integ_accel_B.i2cname = 0;
167 status = MW_I2C_MasterWrite(integ_accel_B.i2cname, 29U, &status, 1U, true,
168 false);
169 if (0 == status) {
170 integ_accel_B.i2cname = 0;
171 MW_I2C_MasterRead(integ_accel_B.i2cname, 29U, output_raw, 6U, false, true);
172 memcpy((void *)&output[0], (void *)&output_raw[0], (size_t)3 * sizeof
173 (int16_T));
174 output_0 = output[0];
175 memcpy((void *)&y[0], (void *)&output_0, (size_t)2 * sizeof(uint8_T));
176 x[0] = y[1];
177 x[1] = y[0];
178 memcpy((void *)&output_0, (void *)&x[0], (size_t)1 * sizeof(int16_T));
179 output[0] = output_0;
180 output_0 = output[1];
181 memcpy((void *)&y[0], (void *)&output_0, (size_t)2 * sizeof(uint8_T));
182 x[0] = y[1];
183 x[1] = y[0];
184 memcpy((void *)&output_0, (void *)&x[0], (size_t)1 * sizeof(int16_T));
185 output[1] = output_0;
186 output_0 = output[2];
187 memcpy((void *)&y[0], (void *)&output_0, (size_t)2 * sizeof(uint8_T));
188 x[0] = y[1];
189 x[1] = y[0];
190 memcpy((void *)&output_0, (void *)&x[0], (size_t)1 * sizeof(int16_T));
191 output[2] = output_0;
192 }
193
194 /* MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates:
195 * Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor'
196 */
197 integ_accel_B.FXOS87006AxesSensor[0] = (real_T)(int16_T)(output[0] >> 2) *
198 0.244 / 1000.0;
199 integ_accel_B.FXOS87006AxesSensor[1] = (real_T)(int16_T)(output[1] >> 2) *
200 0.244 / 1000.0;
201 integ_accel_B.FXOS87006AxesSensor[2] = (real_T)(int16_T)(output[2] >> 2) *
202 0.244 / 1000.0;
203
204 /* Outputs for Iterator SubSystem: '<S3>/Control' incorporates:
205 * ForIterator: '<S5>/Iterator'
206 */
207 /* UnitDelay: '<S3>/Unit Delay3' */
208 memcpy(&integ_accel_B.Assignment[0], &integ_accel_DW.UnitDelay3_DSTATE[0],
209 sizeof(real_T) << 4U);
210
211 /* UnitDelay: '<S3>/Unit Delay2' */
212 integ_accel_B.Assignment_c[0] = integ_accel_DW.UnitDelay2_DSTATE[0];
213 integ_accel_B.Assignment_c[1] = integ_accel_DW.UnitDelay2_DSTATE[1];
214 integ_accel_B.Assignment_c[2] = integ_accel_DW.UnitDelay2_DSTATE[2];
215 integ_accel_B.Assignment_c[3] = integ_accel_DW.UnitDelay2_DSTATE[3];
216
217 /* Selector: '<S6>/Selector' incorporates:
218 * Math: '<S12>/Math Function'
219 * S-Function (sdspdiag2): '<S12>/Extract Diagonal'
220 *
221 * About '<S12>/Math Function':
222 * Operator: reciprocal
223 */
224 for (s5_iter = 0; s5_iter + 1 <= integ_accel_P.Iterator_IterationLimit;
225 s5_iter++) {
226 for (idxV = 0; idxV < 2; idxV++) {
227 /* Product: '<S7>/H*P_prdt' incorporates:
228 * Constant: '<S7>/Hcst'
229 * Math: '<S7>/P_prd Trans'
230 * UnitDelay: '<S3>/Unit Delay3'
231 */
232 for (i = 0; i < 4; i++) {
233 integ_accel_B.y[idxV + (i << 1)] = 0.0;
234 integ_accel_B.y[idxV + (i << 1)] += integ_accel_P.Hcst_Value[idxV] *
235 integ_accel_DW.UnitDelay3_DSTATE[i];
236 integ_accel_B.y[idxV + (i << 1)] += integ_accel_P.Hcst_Value[idxV + 2]
237 * integ_accel_DW.UnitDelay3_DSTATE[i + 4];
238 integ_accel_B.y[idxV + (i << 1)] += integ_accel_P.Hcst_Value[idxV + 4]
239 * integ_accel_DW.UnitDelay3_DSTATE[i + 8];
240 integ_accel_B.y[idxV + (i << 1)] += integ_accel_P.Hcst_Value[idxV + 6]
241 * integ_accel_DW.UnitDelay3_DSTATE[i + 12];
242 }
243
244 /* End of Product: '<S7>/H*P_prdt' */
245
246 /* Sum: '<S7>/H*P_prdt*Ht+R' incorporates:
247 * Constant: '<S7>/Constant2'
248 * Constant: '<S7>/Hcst_t'
249 * Product: '<S7>/H*P_prdt*Ht'
250 */
251 for (i = 0; i < 2; i++) {
252 integ_accel_B.LDLFactorization[idxV + (i << 1)] =
253 (((integ_accel_P.Hcst_t_Value[(i << 2) + 1] * integ_accel_B.y[idxV +
254 2] + integ_accel_P.Hcst_t_Value[i << 2] * integ_accel_B.y[idxV])
255 + integ_accel_P.Hcst_t_Value[(i << 2) + 2] * integ_accel_B.y[idxV
256 + 4]) + integ_accel_P.Hcst_t_Value[(i << 2) + 3] *
257 integ_accel_B.y[idxV + 6]) + integ_accel_P.Constant2_Value[(i << 1)
258 + idxV];
259 }
260
261 /* End of Sum: '<S7>/H*P_prdt*Ht+R' */
262 }
263
264 /* S-Function (sdspldl2): '<S12>/LDL Factorization' */
265 integ_accel_LDLf_int32_Treal_T(&integ_accel_B.LDLFactorization[0U],
266 &integ_accel_DW.LDLFactorization_VMX[0U], 2,
267 &integ_accel_B.LDLFactorization[0U]);
268
269 /* S-Function (sdspfbsub2): '<S12>/Forward Substitution' */
270 idxV = 1;
271 integ_accel_B.currentTime = integ_accel_B.y[1];
272 i = 0;
273 while (i < 1) {
274 integ_accel_B.currentTime -= integ_accel_B.LDLFactorization[idxV] *
275 integ_accel_B.y[0];
276 idxV += 2;
277 i = 1;
278 }
279
280 integ_accel_B.y[1] = integ_accel_B.currentTime;
281 idxV = 1;
282 integ_accel_B.currentTime = integ_accel_B.y[3];
283 i = 0;
284 while (i < 1) {
285 integ_accel_B.currentTime -= integ_accel_B.LDLFactorization[idxV] *
286 integ_accel_B.y[2];
287 idxV += 2;
288 i = 1;
289 }
290
291 integ_accel_B.y[3] = integ_accel_B.currentTime;
292 idxV = 1;
293 integ_accel_B.currentTime = integ_accel_B.y[5];
294 i = 0;
295 while (i < 1) {
296 integ_accel_B.currentTime -= integ_accel_B.LDLFactorization[idxV] *
297 integ_accel_B.y[4];
298 idxV += 2;
299 i = 1;
300 }
301
302 integ_accel_B.y[5] = integ_accel_B.currentTime;
303 idxV = 1;
304 integ_accel_B.currentTime = integ_accel_B.y[7];
305 i = 0;
306 while (i < 1) {
307 integ_accel_B.currentTime -= integ_accel_B.LDLFactorization[idxV] *
308 integ_accel_B.y[6];
309 idxV += 2;
310 i = 1;
311 }
312
313 integ_accel_B.y[7] = integ_accel_B.currentTime;
314
315 /* End of S-Function (sdspfbsub2): '<S12>/Forward Substitution' */
316 integ_accel_B.ZHX_prd[0] = 1.0 / integ_accel_B.LDLFactorization[0];
317
318 /* Math: '<S12>/Math Function' incorporates:
319 * S-Function (sdspdiag2): '<S12>/Extract Diagonal'
320 *
321 * About '<S12>/Math Function':
322 * Operator: reciprocal
323 */
324 integ_accel_B.currentTime = 1.0 / integ_accel_B.LDLFactorization[3];
325
326 /* S-Function (sdspdmult2): '<S12>/Matrix Scaling' */
327 idxV = 0;
328 for (i = 0; i < 4; i++) {
329 integ_accel_B.BackwardSubstitution[idxV] = integ_accel_B.y[idxV] *
330 integ_accel_B.ZHX_prd[0];
331 idxV++;
332 integ_accel_B.BackwardSubstitution[idxV] = integ_accel_B.y[idxV] *
333 integ_accel_B.currentTime;
334 idxV++;
335 }
336
337 /* End of S-Function (sdspdmult2): '<S12>/Matrix Scaling' */
338
339 /* S-Function (sdspfbsub2): '<S12>/Backward Substitution' */
340 idxV = 0;
341 integ_accel_B.currentTime = integ_accel_B.BackwardSubstitution[0];
342 i = 1;
343 while (i > 0) {
344 integ_accel_B.currentTime -= integ_accel_B.LDLFactorization[idxV + 2] *
345 integ_accel_B.BackwardSubstitution[1];
346 idxV -= 2;
347 i = 0;
348 }
349
350 integ_accel_B.BackwardSubstitution[0] = integ_accel_B.currentTime;
351 idxV = 0;
352 integ_accel_B.currentTime = integ_accel_B.BackwardSubstitution[2];
353 i = 1;
354 while (i > 0) {
355 integ_accel_B.currentTime -= integ_accel_B.LDLFactorization[idxV + 2] *
356 integ_accel_B.BackwardSubstitution[3];
357 idxV -= 2;
358 i = 0;
359 }
360
361 integ_accel_B.BackwardSubstitution[2] = integ_accel_B.currentTime;
362 idxV = 0;
363 integ_accel_B.currentTime = integ_accel_B.BackwardSubstitution[4];
364 i = 1;
365 while (i > 0) {
366 integ_accel_B.currentTime -= integ_accel_B.LDLFactorization[idxV + 2] *
367 integ_accel_B.BackwardSubstitution[5];
368 idxV -= 2;
369 i = 0;
370 }
371
372 integ_accel_B.BackwardSubstitution[4] = integ_accel_B.currentTime;
373 idxV = 0;
374 integ_accel_B.currentTime = integ_accel_B.BackwardSubstitution[6];
375 i = 1;
376 while (i > 0) {
377 integ_accel_B.currentTime -= integ_accel_B.LDLFactorization[idxV + 2] *
378 integ_accel_B.BackwardSubstitution[7];
379 idxV -= 2;
380 i = 0;
381 }
382
383 integ_accel_B.BackwardSubstitution[6] = integ_accel_B.currentTime;
384
385 /* End of S-Function (sdspfbsub2): '<S12>/Backward Substitution' */
386 for (idxV = 0; idxV < 2; idxV++) {
387 for (i = 0; i < 4; i++) {
388 /* Math: '<S7>/K Trans' */
389 integ_accel_B.y[i + (idxV << 2)] = integ_accel_B.BackwardSubstitution
390 [(i << 1) + idxV];
391
392 /* Product: '<S7>/K*H*P_prd' incorporates:
393 * Constant: '<S7>/Hcst'
394 */
395 integ_accel_B.dv2[idxV + (i << 1)] = 0.0;
396 integ_accel_B.dv2[idxV + (i << 1)] +=
397 integ_accel_DW.UnitDelay3_DSTATE[i << 2] *
398 integ_accel_P.Hcst_Value[idxV];
399 integ_accel_B.dv2[idxV + (i << 1)] +=
400 integ_accel_DW.UnitDelay3_DSTATE[(i << 2) + 1] *
401 integ_accel_P.Hcst_Value[idxV + 2];
402 integ_accel_B.dv2[idxV + (i << 1)] +=
403 integ_accel_DW.UnitDelay3_DSTATE[(i << 2) + 2] *
404 integ_accel_P.Hcst_Value[idxV + 4];
405 integ_accel_B.dv2[idxV + (i << 1)] +=
406 integ_accel_DW.UnitDelay3_DSTATE[(i << 2) + 3] *
407 integ_accel_P.Hcst_Value[idxV + 6];
408 }
409 }
410
411 for (idxV = 0; idxV < 4; idxV++) {
412 /* Sum: '<S7>/P_prd-K*H*P_prd' incorporates:
413 * Product: '<S7>/A*P*At'
414 * Product: '<S7>/K*H*P_prd'
415 * UnitDelay: '<S3>/Unit Delay3'
416 */
417 for (i = 0; i < 4; i++) {
418 integ_accel_B.dv0[idxV + (i << 2)] = integ_accel_DW.UnitDelay3_DSTATE
419 [(i << 2) + idxV] - (integ_accel_B.dv2[(i << 1) + 1] *
420 integ_accel_B.y[idxV + 4] + integ_accel_B.dv2[i << 1] *
421 integ_accel_B.y[idxV]);
422 }
423
424 /* End of Sum: '<S7>/P_prd-K*H*P_prd' */
425
426 /* Product: '<S7>/A*P*At' incorporates:
427 * Constant: '<S7>/Constant4'
428 */
429 for (i = 0; i < 4; i++) {
430 integ_accel_B.dv1[idxV + (i << 2)] = 0.0;
431 integ_accel_B.dv1[idxV + (i << 2)] += integ_accel_P.Constant4_Value[i <<
432 2] * integ_accel_B.dv0[idxV];
433 integ_accel_B.dv1[idxV + (i << 2)] += integ_accel_P.Constant4_Value[(i
434 << 2) + 1] * integ_accel_B.dv0[idxV + 4];
435 integ_accel_B.dv1[idxV + (i << 2)] += integ_accel_P.Constant4_Value[(i
436 << 2) + 2] * integ_accel_B.dv0[idxV + 8];
437 integ_accel_B.dv1[idxV + (i << 2)] += integ_accel_P.Constant4_Value[(i
438 << 2) + 3] * integ_accel_B.dv0[idxV + 12];
439 }
440 }
441
442 /* Reshape: '<S7>/P_prd_Reshape' incorporates:
443 * Constant: '<S7>/Constant3'
444 * Constant: '<S7>/Constant5'
445 * Constant: '<S7>/Hcst'
446 * Product: '<S7>/A*P*At'
447 * Product: '<S7>/K*H*P_prd'
448 * Sum: '<S7>/A*P*At+Q'
449 * UnitDelay: '<S3>/Unit Delay3'
450 */
451 for (idxV = 0; idxV < 4; idxV++) {
452 for (i = 0; i < 4; i++) {
453 integ_accel_B.P_prd_Reshape[idxV + (i << 2)] = (((integ_accel_B.dv1[(i
454 << 2) + 1] * integ_accel_P.Constant3_Value[idxV + 4] +
455 integ_accel_B.dv1[i << 2] * integ_accel_P.Constant3_Value[idxV]) +
456 integ_accel_B.dv1[(i << 2) + 2] * integ_accel_P.Constant3_Value[idxV
457 + 8]) + integ_accel_B.dv1[(i << 2) + 3] *
458 integ_accel_P.Constant3_Value[idxV + 12]) +
459 integ_accel_P.Constant5_Value[(i << 2) + idxV];
460 }
461 }
462
463 /* End of Reshape: '<S7>/P_prd_Reshape' */
464
465 /* Sum: '<S7>/Z-H*X_prd' incorporates:
466 * Constant: '<S7>/Hcst'
467 * Product: '<S7>/H*X_prd'
468 * UnitDelay: '<S3>/Unit Delay2'
469 */
470 for (idxV = 0; idxV < 2; idxV++) {
471 integ_accel_B.ZHX_prd[idxV] = integ_accel_B.FXOS87006AxesSensor[idxV] -
472 (((integ_accel_P.Hcst_Value[idxV + 2] *
473 integ_accel_DW.UnitDelay2_DSTATE[1] + integ_accel_P.Hcst_Value[idxV]
474 * integ_accel_DW.UnitDelay2_DSTATE[0]) +
475 integ_accel_P.Hcst_Value[idxV + 4] *
476 integ_accel_DW.UnitDelay2_DSTATE[2]) + integ_accel_P.Hcst_Value[idxV
477 + 6] * integ_accel_DW.UnitDelay2_DSTATE[3]);
478 }
479
480 /* End of Sum: '<S7>/Z-H*X_prd' */
481
482 /* Sum: '<S7>/X_prd+K*(Z-H*X_prd)' incorporates:
483 * Product: '<S7>/K*(Z-H*X_prd)'
484 * UnitDelay: '<S3>/Unit Delay2'
485 */
486 for (idxV = 0; idxV < 4; idxV++) {
487 integ_accel_B.LDLFactorization[idxV] = (integ_accel_B.y[idxV + 4] *
488 integ_accel_B.ZHX_prd[1] + integ_accel_B.y[idxV] *
489 integ_accel_B.ZHX_prd[0]) + integ_accel_DW.UnitDelay2_DSTATE[idxV];
490 }
491
492 /* End of Sum: '<S7>/X_prd+K*(Z-H*X_prd)' */
493
494 /* Assignment: '<S8>/Assignment' */
495 memcpy(&integ_accel_B.Assignment[0], &integ_accel_B.P_prd_Reshape[0],
496 sizeof(real_T) << 4U);
497 for (i = 0; i < 4; i++) {
498 /* Assignment: '<S9>/Assignment' */
499 integ_accel_B.Assignment_o[i] = integ_accel_B.LDLFactorization[i];
500
501 /* Product: '<S7>/A*X' incorporates:
502 * Constant: '<S7>/Constant3'
503 */
504 integ_accel_B.currentTime = integ_accel_P.Constant3_Value[i + 12] *
505 integ_accel_B.LDLFactorization[3] + (integ_accel_P.Constant3_Value[i +
506 8] * integ_accel_B.LDLFactorization[2] +
507 (integ_accel_P.Constant3_Value[i + 4] *
508 integ_accel_B.LDLFactorization[1] + integ_accel_P.Constant3_Value[i] *
509 integ_accel_B.LDLFactorization[0]));
510
511 /* Assignment: '<S10>/Assignment' incorporates:
512 * Product: '<S7>/A*X'
513 */
514 integ_accel_B.Assignment_c[i] = integ_accel_B.currentTime;
515 }
516 }
517
518 /* End of Selector: '<S6>/Selector' */
519 /* End of Outputs for SubSystem: '<S3>/Control' */
520 }
521
522 /* Step: '<Root>/Step' */
523 integ_accel_B.currentTime = integ_accel_M->Timing.t[0];
524 if (integ_accel_B.currentTime < integ_accel_P.Step_Time) {
525 integ_accel_B.currentTime = integ_accel_P.Step_Y0;
526 } else {
527 integ_accel_B.currentTime = integ_accel_P.Step_YFinal;
528 }
529
530 /* End of Step: '<Root>/Step' */
531
532 /* Outputs for Enabled SubSystem: '<Root>/Subsystem1' incorporates:
533 * EnablePort: '<S2>/Enable'
534 */
535 if (integ_accel_B.currentTime > 0.0) {
536 if (!integ_accel_DW.Subsystem1_MODE) {
537 integ_accel_DW.Subsystem1_MODE = true;
538 }
539
540 if (integ_accel_M->Timing.TaskCounters.TID[2] == 0) {
541 /* DiscreteIntegrator: '<S2>/Discrete-Time Integrator4' */
542 integ_accel_B.DiscreteTimeIntegrator4 =
543 integ_accel_DW.DiscreteTimeIntegrator4_DSTATE;
544
545 /* DiscreteIntegrator: '<S2>/Discrete-Time Integrator6' */
546 integ_accel_B.DiscreteTimeIntegrator6 =
547 integ_accel_DW.DiscreteTimeIntegrator6_DSTATE;
548
549 /* DiscreteIntegrator: '<S2>/Discrete-Time Integrator5' */
550 integ_accel_B.DiscreteTimeIntegrator5 =
551 integ_accel_DW.DiscreteTimeIntegrator5_DSTATE;
552
553 /* DiscreteIntegrator: '<S2>/Discrete-Time Integrator7' */
554 integ_accel_B.DiscreteTimeIntegrator7 =
555 integ_accel_DW.DiscreteTimeIntegrator7_DSTATE;
556
557 /* DiscreteIntegrator: '<S2>/Discrete-Time Integrator' */
558 integ_accel_B.DiscreteTimeIntegrator =
559 integ_accel_DW.DiscreteTimeIntegrator_DSTATE;
560
561 /* DiscreteIntegrator: '<S2>/Discrete-Time Integrator2' */
562 integ_accel_B.DiscreteTimeIntegrator2 =
563 integ_accel_DW.DiscreteTimeIntegrator2_DSTATE;
564
565 /* DiscreteIntegrator: '<S2>/Discrete-Time Integrator1' */
566 integ_accel_B.DiscreteTimeIntegrator1 =
567 integ_accel_DW.DiscreteTimeIntegrator1_DSTATE;
568
569 /* DiscreteIntegrator: '<S2>/Discrete-Time Integrator3' */
570 integ_accel_B.DiscreteTimeIntegrator3 =
571 integ_accel_DW.DiscreteTimeIntegrator3_DSTATE;
572 }
573
574 srUpdateBC(integ_accel_DW.Subsystem1_SubsysRanBC);
575 } else {
576 if (integ_accel_DW.Subsystem1_MODE) {
577 integ_accel_DW.Subsystem1_MODE = false;
578 }
579 }
580
581 /* End of Outputs for SubSystem: '<Root>/Subsystem1' */
582 if (integ_accel_M->Timing.TaskCounters.TID[2] == 0) {
583 }
584
585 if (integ_accel_M->Timing.TaskCounters.TID[2] == 0) {
586 /* Update for UnitDelay: '<S3>/Unit Delay2' */
587 integ_accel_DW.UnitDelay2_DSTATE[0] = integ_accel_B.Assignment_c[0];
588 integ_accel_DW.UnitDelay2_DSTATE[1] = integ_accel_B.Assignment_c[1];
589 integ_accel_DW.UnitDelay2_DSTATE[2] = integ_accel_B.Assignment_c[2];
590 integ_accel_DW.UnitDelay2_DSTATE[3] = integ_accel_B.Assignment_c[3];
591
592 /* Update for UnitDelay: '<S3>/Unit Delay3' */
593 memcpy(&integ_accel_DW.UnitDelay3_DSTATE[0], &integ_accel_B.Assignment[0],
594 sizeof(real_T) << 4U);
595 }
596
597 /* Update for Enabled SubSystem: '<Root>/Subsystem1' incorporates:
598 * Update for EnablePort: '<S2>/Enable'
599 */
600 if (integ_accel_DW.Subsystem1_MODE && (integ_accel_M->Timing.TaskCounters.TID
601 [2] == 0)) {
602 /* Update for DiscreteIntegrator: '<S2>/Discrete-Time Integrator4' */
603 integ_accel_DW.DiscreteTimeIntegrator4_DSTATE +=
604 integ_accel_P.DiscreteTimeIntegrator4_gainval *
605 integ_accel_B.Assignment_o[0];
606
607 /* Update for DiscreteIntegrator: '<S2>/Discrete-Time Integrator6' */
608 integ_accel_DW.DiscreteTimeIntegrator6_DSTATE +=
609 integ_accel_P.DiscreteTimeIntegrator6_gainval *
610 integ_accel_B.DiscreteTimeIntegrator4;
611
612 /* Update for DiscreteIntegrator: '<S2>/Discrete-Time Integrator5' */
613 integ_accel_DW.DiscreteTimeIntegrator5_DSTATE +=
614 integ_accel_P.DiscreteTimeIntegrator5_gainval *
615 integ_accel_B.Assignment_o[2];
616
617 /* Update for DiscreteIntegrator: '<S2>/Discrete-Time Integrator7' */
618 integ_accel_DW.DiscreteTimeIntegrator7_DSTATE +=
619 integ_accel_P.DiscreteTimeIntegrator7_gainval *
620 integ_accel_B.DiscreteTimeIntegrator5;
621
622 /* Update for DiscreteIntegrator: '<S2>/Discrete-Time Integrator' */
623 integ_accel_DW.DiscreteTimeIntegrator_DSTATE +=
624 integ_accel_P.DiscreteTimeIntegrator_gainval *
625 integ_accel_B.FXOS87006AxesSensor[0];
626
627 /* Update for DiscreteIntegrator: '<S2>/Discrete-Time Integrator2' */
628 integ_accel_DW.DiscreteTimeIntegrator2_DSTATE +=
629 integ_accel_P.DiscreteTimeIntegrator2_gainval *
630 integ_accel_B.DiscreteTimeIntegrator;
631
632 /* Update for DiscreteIntegrator: '<S2>/Discrete-Time Integrator1' */
633 integ_accel_DW.DiscreteTimeIntegrator1_DSTATE +=
634 integ_accel_P.DiscreteTimeIntegrator1_gainval *
635 integ_accel_B.FXOS87006AxesSensor[1];
636
637 /* Update for DiscreteIntegrator: '<S2>/Discrete-Time Integrator3' */
638 integ_accel_DW.DiscreteTimeIntegrator3_DSTATE +=
639 integ_accel_P.DiscreteTimeIntegrator3_gainval *
640 integ_accel_B.DiscreteTimeIntegrator1;
641 }
642
643 /* End of Update for SubSystem: '<Root>/Subsystem1' */
644
645 /* External mode */
646 rtExtModeUploadCheckTrigger(3);
647
648 { /* Sample time: [0.0s, 0.0s] */
649 rtExtModeUpload(0, integ_accel_M->Timing.t[0]);
650 }
651
652 { /* Sample time: [0.001s, 0.0s] */
653 rtExtModeUpload(1, ((integ_accel_M->Timing.clockTick1) * 0.001));
654 }
655
656 if (integ_accel_M->Timing.TaskCounters.TID[2] == 0) {/* Sample time: [1.0s, 0.0s] */
657 rtExtModeUpload(2, ((integ_accel_M->Timing.clockTick2) * 1.0));
658 }
659
660 /* signal main to stop simulation */
661 { /* Sample time: [0.0s, 0.0s] */
662 if ((rtmGetTFinal(integ_accel_M)!=-1) &&
663 !((rtmGetTFinal(integ_accel_M)-integ_accel_M->Timing.t[0]) >
664 integ_accel_M->Timing.t[0] * (DBL_EPSILON))) {
665 rtmSetErrorStatus(integ_accel_M, "Simulation finished");
666 }
667
668 if (rtmGetStopRequested(integ_accel_M)) {
669 rtmSetErrorStatus(integ_accel_M, "Simulation finished");
670 }
671 }
672
673 /* Update absolute time for base rate */
674 /* The "clockTick0" counts the number of times the code of this task has
675 * been executed. The absolute time is the multiplication of "clockTick0"
676 * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
677 * overflow during the application lifespan selected.
678 */
679 integ_accel_M->Timing.t[0] =
680 (++integ_accel_M->Timing.clockTick0) * integ_accel_M->Timing.stepSize0;
681
682 {
683 /* Update absolute timer for sample time: [0.001s, 0.0s] */
684 /* The "clockTick1" counts the number of times the code of this task has
685 * been executed. The resolution of this integer timer is 0.001, which is the step size
686 * of the task. Size of "clockTick1" ensures timer will not overflow during the
687 * application lifespan selected.
688 */
689 integ_accel_M->Timing.clockTick1++;
690 }
691
692 if (integ_accel_M->Timing.TaskCounters.TID[2] == 0) {
693 /* Update absolute timer for sample time: [1.0s, 0.0s] */
694 /* The "clockTick2" counts the number of times the code of this task has
695 * been executed. The resolution of this integer timer is 1.0, which is the step size
696 * of the task. Size of "clockTick2" ensures timer will not overflow during the
697 * application lifespan selected.
698 */
699 integ_accel_M->Timing.clockTick2++;
700 }
701
702 rate_scheduler();
703}
704
705/* Model initialize function */
706void integ_accel_initialize(void)
707{
708 /* Registration code */
709
710 /* initialize real-time model */
711 (void) memset((void *)integ_accel_M, 0,
712 sizeof(RT_MODEL_integ_accel_T));
713
714 {
715 /* Setup solver object */
716 rtsiSetSimTimeStepPtr(&integ_accel_M->solverInfo,
717 &integ_accel_M->Timing.simTimeStep);
718 rtsiSetTPtr(&integ_accel_M->solverInfo, &rtmGetTPtr(integ_accel_M));
719 rtsiSetStepSizePtr(&integ_accel_M->solverInfo,
720 &integ_accel_M->Timing.stepSize0);
721 rtsiSetErrorStatusPtr(&integ_accel_M->solverInfo, (&rtmGetErrorStatus
722 (integ_accel_M)));
723 rtsiSetRTModelPtr(&integ_accel_M->solverInfo, integ_accel_M);
724 }
725
726 rtsiSetSimTimeStep(&integ_accel_M->solverInfo, MAJOR_TIME_STEP);
727 rtsiSetSolverName(&integ_accel_M->solverInfo,"FixedStepDiscrete");
728 rtmSetTPtr(integ_accel_M, &integ_accel_M->Timing.tArray[0]);
729 rtmSetTFinal(integ_accel_M, -1);
730 integ_accel_M->Timing.stepSize0 = 0.001;
731
732 /* External mode info */
733 integ_accel_M->Sizes.checksums[0] = (2691778551U);
734 integ_accel_M->Sizes.checksums[1] = (2170082580U);
735 integ_accel_M->Sizes.checksums[2] = (426877032U);
736 integ_accel_M->Sizes.checksums[3] = (801698773U);
737
738 {
739 static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE;
740 static RTWExtModeInfo rt_ExtModeInfo;
741 static const sysRanDType *systemRan[4];
742 integ_accel_M->extModeInfo = (&rt_ExtModeInfo);
743 rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan);
744 systemRan[0] = &rtAlwaysEnabled;
745 systemRan[1] = &rtAlwaysEnabled;
746 systemRan[2] = &rtAlwaysEnabled;
747 systemRan[3] = (sysRanDType *)&integ_accel_DW.Subsystem1_SubsysRanBC;
748 rteiSetModelMappingInfoPtr(integ_accel_M->extModeInfo,
749 &integ_accel_M->SpecialInfo.mappingInfo);
750 rteiSetChecksumsPtr(integ_accel_M->extModeInfo,
751 integ_accel_M->Sizes.checksums);
752 rteiSetTPtr(integ_accel_M->extModeInfo, rtmGetTPtr(integ_accel_M));
753 }
754
755 /* block I/O */
756 (void) memset(((void *) &integ_accel_B), 0,
757 sizeof(B_integ_accel_T));
758
759 /* states (dwork) */
760 (void) memset((void *)&integ_accel_DW, 0,
761 sizeof(DW_integ_accel_T));
762
763 /* data type transition information */
764 {
765 static DataTypeTransInfo dtInfo;
766 (void) memset((char_T *) &dtInfo, 0,
767 sizeof(dtInfo));
768 integ_accel_M->SpecialInfo.mappingInfo = (&dtInfo);
769 dtInfo.numDataTypes = 15;
770 dtInfo.dataTypeSizes = &rtDataTypeSizes[0];
771 dtInfo.dataTypeNames = &rtDataTypeNames[0];
772
773 /* Block I/O transition table */
774 dtInfo.BTransTable = &rtBTransTable;
775
776 /* Parameters transition table */
777 dtInfo.PTransTable = &rtPTransTable;
778 }
779
780 {
781 freedomk64f_fxos8700_integ_ac_T *obj;
782 uint32_T i2cname;
783 uint8_T output;
784 uint8_T status;
785 uint8_T SwappedDataBytes[2];
786 int32_T i;
787
788 /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */
789 obj = &integ_accel_DW.obj;
790 integ_accel_DW.obj.isInitialized = 0;
791 obj->i2cobj.isInitialized = 0;
792
793 /* [EOF] */
794 /* */
795 integ_accel_DW.obj.SampleTime = integ_accel_P.FXOS87006AxesSensor_SampleTime;
796 obj = &integ_accel_DW.obj;
797 integ_accel_DW.obj.isInitialized = 1;
798 i2cname = 0;
799 MW_I2C_Open(i2cname, 0U);
800
801 /* KHz */
802 obj->i2cobj.BusSpeed = 100000U;
803 i2cname = 0;
804 MW_I2C_SetBusSpeed(i2cname, obj->i2cobj.BusSpeed);
805 SwappedDataBytes[0] = 43U;
806 SwappedDataBytes[1] = 64U;
807 i2cname = 0;
808 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
809 OSA_TimeDelay(500U);
810 output = 0U;
811 status = 42U;
812 i2cname = 0;
813 status = MW_I2C_MasterWrite(i2cname, 29U, &status, 1U, true, false);
814 if (0 == status) {
815 i2cname = 0;
816 MW_I2C_MasterRead(i2cname, 29U, &status, 1U, false, true);
817 memcpy((void *)&output, (void *)&status, (size_t)1 * sizeof(uint8_T));
818 }
819
820 SwappedDataBytes[0] = 42U;
821 SwappedDataBytes[1] = (uint8_T)(output & 254);
822 i2cname = 0;
823 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
824 SwappedDataBytes[0] = 14U;
825 SwappedDataBytes[1] = 0U;
826 i2cname = 0;
827 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
828 SwappedDataBytes[0] = 91U;
829 SwappedDataBytes[1] = 0U;
830 i2cname = 0;
831 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
832 SwappedDataBytes[0] = 42U;
833 SwappedDataBytes[1] = 1U;
834 i2cname = 0;
835 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
836
837 /* End of Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */
838
839 /* InitializeConditions for UnitDelay: '<S3>/Unit Delay2' */
840 integ_accel_DW.UnitDelay2_DSTATE[0] =
841 integ_accel_P.UnitDelay2_InitialCondition[0];
842 integ_accel_DW.UnitDelay2_DSTATE[1] =
843 integ_accel_P.UnitDelay2_InitialCondition[1];
844 integ_accel_DW.UnitDelay2_DSTATE[2] =
845 integ_accel_P.UnitDelay2_InitialCondition[2];
846 integ_accel_DW.UnitDelay2_DSTATE[3] =
847 integ_accel_P.UnitDelay2_InitialCondition[3];
848
849 /* SystemInitialize for Iterator SubSystem: '<S3>/Control' */
850 /* SystemInitialize for Outport: '<S5>/X_est' */
851 integ_accel_B.Assignment_o[0] = integ_accel_P.X_est_Y0;
852
853 /* SystemInitialize for Outport: '<S5>/X_prd' */
854 integ_accel_B.Assignment_c[0] = integ_accel_P.X_prd_Y0;
855
856 /* SystemInitialize for Outport: '<S5>/X_est' */
857 integ_accel_B.Assignment_o[1] = integ_accel_P.X_est_Y0;
858
859 /* SystemInitialize for Outport: '<S5>/X_prd' */
860 integ_accel_B.Assignment_c[1] = integ_accel_P.X_prd_Y0;
861
862 /* SystemInitialize for Outport: '<S5>/X_est' */
863 integ_accel_B.Assignment_o[2] = integ_accel_P.X_est_Y0;
864
865 /* SystemInitialize for Outport: '<S5>/X_prd' */
866 integ_accel_B.Assignment_c[2] = integ_accel_P.X_prd_Y0;
867
868 /* SystemInitialize for Outport: '<S5>/X_est' */
869 integ_accel_B.Assignment_o[3] = integ_accel_P.X_est_Y0;
870
871 /* SystemInitialize for Outport: '<S5>/X_prd' */
872 integ_accel_B.Assignment_c[3] = integ_accel_P.X_prd_Y0;
873 for (i = 0; i < 16; i++) {
874 /* InitializeConditions for UnitDelay: '<S3>/Unit Delay3' */
875 integ_accel_DW.UnitDelay3_DSTATE[i] =
876 integ_accel_P.UnitDelay3_InitialCondition[i];
877
878 /* SystemInitialize for Outport: '<S5>/P_prd' */
879 integ_accel_B.Assignment[i] = integ_accel_P.P_prd_Y0;
880 }
881
882 /* End of SystemInitialize for SubSystem: '<S3>/Control' */
883
884 /* SystemInitialize for Enabled SubSystem: '<Root>/Subsystem1' */
885 /* InitializeConditions for DiscreteIntegrator: '<S2>/Discrete-Time Integrator4' */
886 integ_accel_DW.DiscreteTimeIntegrator4_DSTATE =
887 integ_accel_P.DiscreteTimeIntegrator4_IC;
888
889 /* InitializeConditions for DiscreteIntegrator: '<S2>/Discrete-Time Integrator6' */
890 integ_accel_DW.DiscreteTimeIntegrator6_DSTATE =
891 integ_accel_P.DiscreteTimeIntegrator6_IC;
892
893 /* InitializeConditions for DiscreteIntegrator: '<S2>/Discrete-Time Integrator5' */
894 integ_accel_DW.DiscreteTimeIntegrator5_DSTATE =
895 integ_accel_P.DiscreteTimeIntegrator5_IC;
896
897 /* InitializeConditions for DiscreteIntegrator: '<S2>/Discrete-Time Integrator7' */
898 integ_accel_DW.DiscreteTimeIntegrator7_DSTATE =
899 integ_accel_P.DiscreteTimeIntegrator7_IC;
900
901 /* InitializeConditions for DiscreteIntegrator: '<S2>/Discrete-Time Integrator' */
902 integ_accel_DW.DiscreteTimeIntegrator_DSTATE =
903 integ_accel_P.DiscreteTimeIntegrator_IC;
904
905 /* InitializeConditions for DiscreteIntegrator: '<S2>/Discrete-Time Integrator2' */
906 integ_accel_DW.DiscreteTimeIntegrator2_DSTATE =
907 integ_accel_P.DiscreteTimeIntegrator2_IC;
908
909 /* InitializeConditions for DiscreteIntegrator: '<S2>/Discrete-Time Integrator1' */
910 integ_accel_DW.DiscreteTimeIntegrator1_DSTATE =
911 integ_accel_P.DiscreteTimeIntegrator1_IC;
912
913 /* InitializeConditions for DiscreteIntegrator: '<S2>/Discrete-Time Integrator3' */
914 integ_accel_DW.DiscreteTimeIntegrator3_DSTATE =
915 integ_accel_P.DiscreteTimeIntegrator3_IC;
916
917 /* SystemInitialize for Outport: '<S2>/x_filter' */
918 integ_accel_B.DiscreteTimeIntegrator6 = integ_accel_P.x_filter_Y0;
919
920 /* SystemInitialize for Outport: '<S2>/y_filter' */
921 integ_accel_B.DiscreteTimeIntegrator7 = integ_accel_P.y_filter_Y0;
922
923 /* SystemInitialize for Outport: '<S2>/x_meas' */
924 integ_accel_B.DiscreteTimeIntegrator2 = integ_accel_P.x_meas_Y0;
925
926 /* SystemInitialize for Outport: '<S2>/y_meas' */
927 integ_accel_B.DiscreteTimeIntegrator3 = integ_accel_P.y_meas_Y0;
928
929 /* End of SystemInitialize for SubSystem: '<Root>/Subsystem1' */
930 }
931}
932
933/* Model terminate function */
934void integ_accel_terminate(void)
935{
936 uint32_T i2cname;
937
938 /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates:
939 * Terminate for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor'
940 */
941 if (integ_accel_DW.obj.isInitialized == 1) {
942 integ_accel_DW.obj.isInitialized = 2;
943 i2cname = 0;
944 MW_I2C_Close(i2cname);
945 }
946
947 /* End of Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */
948}
949
950/*
951 * File trailer for generated code.
952 *
953 * [EOF]
954 */
955