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) */ |
25 | B_integ_accel_T integ_accel_B; |
26 | |
27 | /* Block states (auto storage) */ |
28 | DW_integ_accel_T integ_accel_DW; |
29 | |
30 | /* Real-time model */ |
31 | RT_MODEL_integ_accel_T integ_accel_M_; |
32 | RT_MODEL_integ_accel_T *const integ_accel_M = &integ_accel_M_; |
33 | |
34 | /* Forward declaration for local functions */ |
35 | static void integ_accel_LDLf_int32_Treal_T(real_T out[], real_T vArray[], |
36 | int32_T nRows, const real_T in[]); |
37 | static 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 | */ |
44 | static 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 | |
56 | static 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 */ |
126 | void 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 */ |
706 | void 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 */ |
934 | void 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 | |