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_and_Gyro.c
7 *
8 * Code generated for Simulink model 'integ_Accel_and_Gyro'.
9 *
10 * Model version : 1.7
11 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
12 * C/C++ source code generated on : Tue Dec 05 15:47:02 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_and_Gyro.h"
21#include "integ_Accel_and_Gyro_private.h"
22#include "integ_Accel_and_Gyro_dt.h"
23#define integ_Accel_and_G_RepeatedStart (false)
24#define integ_Accel_and_Gy_SlaveAddress (104U)
25#define integ_Accel_and_Gyro_NoAck (false)
26
27/* Named constants for Chart: '<S2>/MPU6050' */
28#define i_IN_Lecture_acceleration3_axes ((uint8_T)1U)
29#define integ_Accel__IN_NO_ACTIVE_CHILD ((uint8_T)0U)
30#define integ_Accel_a_IN_initialisation ((uint8_T)2U)
31#define integ_Accel_and_RegisterAddress ((uint8_T)59U)
32#define integ_Accel_and__SlaveAddress_j (104U)
33
34/* Block signals (auto storage) */
35B_integ_Accel_and_Gyro_T integ_Accel_and_Gyro_B;
36
37/* Block states (auto storage) */
38DW_integ_Accel_and_Gyro_T integ_Accel_and_Gyro_DW;
39
40/* Real-time model */
41RT_MODEL_integ_Accel_and_Gyro_T integ_Accel_and_Gyro_M_;
42RT_MODEL_integ_Accel_and_Gyro_T *const integ_Accel_and_Gyro_M =
43 &integ_Accel_and_Gyro_M_;
44static void rate_scheduler(void);
45
46/*
47 * This function updates active task flag for each subrate.
48 * The function is called at model base rate, hence the
49 * generated code self-manages all its subrates.
50 */
51static void rate_scheduler(void)
52{
53 /* Compute which subrates run during the next base time step. Subrates
54 * are an integer multiple of the base rate counter. Therefore, the subtask
55 * counter is reset when it reaches its limit (zero means run).
56 */
57 (integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[2])++;
58 if ((integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[2]) > 19) {/* Sample time: [1.0s, 0.0s] */
59 integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[2] = 0;
60 }
61}
62
63/* Start for function-call system: '<S17>/i2cWr' */
64void integ_Accel_and_Gyr_i2cWr_Start(DW_i2cWr_integ_Accel_and_Gyro_T *localDW)
65{
66 uint32_T i2cname;
67
68 /* Start for MATLABSystem: '<S19>/I2C Master Write' */
69 localDW->obj.isInitialized = 0;
70
71 /* [EOF] */
72 /* */
73 localDW->obj.isInitialized = 1;
74 i2cname = 0;
75 MW_I2C_Open(i2cname, 0U);
76
77 /* 100KHz. */
78 /* KHz */
79 localDW->obj.BusSpeed = 100000U;
80 i2cname = 0;
81 MW_I2C_SetBusSpeed(i2cname, localDW->obj.BusSpeed);
82}
83
84/* Output and update for function-call system: '<S17>/i2cWr' */
85void integ_Accel_and_Gyro_i2cWr(uint16_T rtu_dataw,
86 DW_i2cWr_integ_Accel_and_Gyro_T *localDW)
87{
88 uint8_T SwappedDataBytes[2];
89 uint32_T i2cname;
90 uint16_T x;
91 uint8_T x_0[2];
92
93 /* Start for MATLABSystem: '<S19>/I2C Master Write' incorporates:
94 * MATLABSystem: '<S19>/I2C Master Write'
95 */
96 memcpy((void *)&SwappedDataBytes[0], (void *)&rtu_dataw, (size_t)2 * sizeof
97 (uint8_T));
98 x_0[0] = SwappedDataBytes[1];
99 x_0[1] = SwappedDataBytes[0];
100 memcpy((void *)&x, (void *)&x_0[0], (size_t)1 * sizeof(uint16_T));
101 memcpy((void *)&SwappedDataBytes[0], (void *)&x, (size_t)2 * sizeof(uint8_T));
102 i2cname = 0;
103 MW_I2C_MasterWrite(i2cname, integ_Accel_and_Gy_SlaveAddress, SwappedDataBytes,
104 2U, integ_Accel_and_G_RepeatedStart,
105 integ_Accel_and_Gyro_NoAck);
106 localDW->i2cWr_SubsysRanBC = 4;
107}
108
109/* Termination for function-call system: '<S17>/i2cWr' */
110void integ_Accel_and_Gyro_i2cWr_Term(DW_i2cWr_integ_Accel_and_Gyro_T *localDW)
111{
112 uint32_T i2cname;
113
114 /* Start for MATLABSystem: '<S19>/I2C Master Write' incorporates:
115 * Terminate for MATLABSystem: '<S19>/I2C Master Write'
116 */
117 if (localDW->obj.isInitialized == 1) {
118 localDW->obj.isInitialized = 2;
119 i2cname = 0;
120 MW_I2C_Close(i2cname);
121 }
122
123 /* End of Start for MATLABSystem: '<S19>/I2C Master Write' */
124}
125
126void LDLf_int32_Treal32_T(real32_T out[], real32_T vArray[], int32_T nRows,
127 const real32_T in[])
128{
129 int32_T c;
130 int32_T r;
131 int32_T idx1;
132 int32_T idx2;
133 real32_T mYTmp;
134 real32_T mYTmpR;
135 boolean_T done;
136 int32_T k;
137 real32_T prod;
138
139 /* S-Function (sdspldl2): '<S13>/LDL Factorization' */
140 done = false;
141
142 /* use done to control the for loop return early */
143 /* when non-positive definite eigenvalue is detected */
144 c = 0;
145 while ((c < nRows) && (!done)) {
146 idx2 = c * nRows;
147 for (r = 0; r < c; r++) {
148 idx1 = r * nRows;
149 mYTmp = out[idx1 + c];
150 mYTmpR = out[idx1 + r];
151 vArray[r] = mYTmp * mYTmpR;
152 }
153
154 mYTmp = in[idx2 + c];
155 idx1 = c;
156 for (r = 0; r < c; r++) {
157 prod = out[idx1] * vArray[r];
158 mYTmp -= prod;
159 idx1 += nRows;
160 }
161
162 mYTmpR = mYTmp;
163 if (mYTmp <= 0.0F) {
164 done = true;
165 } else {
166 out[idx2 + c] = mYTmp;
167 for (r = c + 1; r < nRows; r++) {
168 mYTmp = in[idx2 + r];
169 idx1 = r;
170 for (k = 0; k < c; k++) {
171 prod = out[idx1] * vArray[k];
172 mYTmp -= prod;
173 idx1 += nRows;
174 }
175
176 out[idx2 + r] = mYTmp / mYTmpR;
177 }
178 }
179
180 c++;
181 }
182
183 /* transpose and copy lower sub-triang to upper */
184 c = 0;
185 while ((c < nRows) && (!done)) {
186 for (r = c + 1; r < nRows; r++) {
187 mYTmp = out[c * nRows + r];
188 out[r * nRows + c] = mYTmp;
189 }
190
191 c++;
192 }
193
194 /* End of S-Function (sdspldl2): '<S13>/LDL Factorization' */
195}
196
197/* Model step function */
198void integ_Accel_and_Gyro_step(void)
199{
200 /* local block i/o variables */
201 real32_T rtb_DiscreteTimeIntegrator4;
202 boolean_T p;
203 boolean_T p_0;
204 uint8_T status;
205 uint8_T y[2];
206 uint8_T x[2];
207 int16_T output;
208 uint16_T output_0;
209
210 /* Reset subsysRan breadcrumbs */
211 srClearBC(integ_Accel_and_Gyro_DW.i2cWr.i2cWr_SubsysRanBC);
212
213 /* Reset subsysRan breadcrumbs */
214 srClearBC(integ_Accel_and_Gyro_DW.i2cRd_SubsysRanBC);
215
216 /* Reset subsysRan breadcrumbs */
217 srClearBC(integ_Accel_and_Gyro_DW.Subsystem1_SubsysRanBC);
218
219 /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates:
220 * MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor'
221 */
222 p = false;
223 p_0 = true;
224 if (!(integ_Accel_and_Gyro_DW.obj.SampleTime ==
225 integ_Accel_and_Gyro_P.FXOS87006AxesSensor_SampleTime)) {
226 p_0 = false;
227 }
228
229 if (p_0) {
230 p = true;
231 }
232
233 if (!p) {
234 integ_Accel_and_Gyro_DW.obj.SampleTime =
235 integ_Accel_and_Gyro_P.FXOS87006AxesSensor_SampleTime;
236 }
237
238 integ_Accel_and_Gyro_B.output_c[0] = 0;
239 integ_Accel_and_Gyro_B.output_c[1] = 0;
240 integ_Accel_and_Gyro_B.output_c[2] = 0;
241 status = 1U;
242 integ_Accel_and_Gyro_B.i2cname = 0;
243 status = MW_I2C_MasterWrite(integ_Accel_and_Gyro_B.i2cname, 29U, &status, 1U,
244 true, false);
245 if (0 == status) {
246 integ_Accel_and_Gyro_B.i2cname = 0;
247 MW_I2C_MasterRead(integ_Accel_and_Gyro_B.i2cname, 29U,
248 integ_Accel_and_Gyro_B.output_raw_m, 6U, false, true);
249 memcpy((void *)&integ_Accel_and_Gyro_B.output_c[0], (void *)
250 &integ_Accel_and_Gyro_B.output_raw_m[0], (size_t)3 * sizeof(int16_T));
251 output = integ_Accel_and_Gyro_B.output_c[0];
252 memcpy((void *)&y[0], (void *)&output, (size_t)2 * sizeof(uint8_T));
253 x[0] = y[1];
254 x[1] = y[0];
255 memcpy((void *)&output, (void *)&x[0], (size_t)1 * sizeof(int16_T));
256 integ_Accel_and_Gyro_B.output_c[0] = output;
257 output = integ_Accel_and_Gyro_B.output_c[1];
258 memcpy((void *)&y[0], (void *)&output, (size_t)2 * sizeof(uint8_T));
259 x[0] = y[1];
260 x[1] = y[0];
261 memcpy((void *)&output, (void *)&x[0], (size_t)1 * sizeof(int16_T));
262 integ_Accel_and_Gyro_B.output_c[1] = output;
263 output = integ_Accel_and_Gyro_B.output_c[2];
264 memcpy((void *)&y[0], (void *)&output, (size_t)2 * sizeof(uint8_T));
265 x[0] = y[1];
266 x[1] = y[0];
267 memcpy((void *)&output, (void *)&x[0], (size_t)1 * sizeof(int16_T));
268 integ_Accel_and_Gyro_B.output_c[2] = output;
269 }
270
271 /* MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates:
272 * Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor'
273 */
274 integ_Accel_and_Gyro_B.FXOS87006AxesSensor[0] = (real_T)(int16_T)
275 (integ_Accel_and_Gyro_B.output_c[0] >> 2) * 0.244 / 1000.0;
276 integ_Accel_and_Gyro_B.FXOS87006AxesSensor[1] = (real_T)(int16_T)
277 (integ_Accel_and_Gyro_B.output_c[1] >> 2) * 0.244 / 1000.0;
278 integ_Accel_and_Gyro_B.FXOS87006AxesSensor[2] = (real_T)(int16_T)
279 (integ_Accel_and_Gyro_B.output_c[2] >> 2) * 0.244 / 1000.0;
280 if (integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[2] == 0) {
281 /* Chart: '<S2>/MPU6050' */
282 /* Gateway: MPU6050 sensor/MPU6050 */
283 /* During: MPU6050 sensor/MPU6050 */
284 if (integ_Accel_and_Gyro_DW.is_active_c3_integ_Accel_and_Gy == 0U) {
285 /* Entry: MPU6050 sensor/MPU6050 */
286 integ_Accel_and_Gyro_DW.is_active_c3_integ_Accel_and_Gy = 1U;
287
288 /* Entry Internal: MPU6050 sensor/MPU6050 */
289 /* Transition: '<S17>:35' */
290 if (integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro !=
291 integ_Accel_a_IN_initialisation) {
292 integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro =
293 integ_Accel_a_IN_initialisation;
294
295 /* Entry 'initialisation': '<S17>:34' */
296 /* '<S17>:34:1' i2cWr(uint16(hex2dec( '6B00' ))); */
297 /* Simulink Function 'i2cWr': '<S17>:28' */
298 integ_Accel_and_Gyro_B.dataw = 27392U;
299
300 /* Outputs for Function Call SubSystem: '<S17>/i2cWr' */
301 integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw,
302 &integ_Accel_and_Gyro_DW.i2cWr);
303
304 /* End of Outputs for SubSystem: '<S17>/i2cWr' */
305
306 /* Power on */
307 /* '<S17>:34:3' i2cWr(uint16(hex2dec( '1B00' ))); */
308 /* Simulink Function 'i2cWr': '<S17>:28' */
309 integ_Accel_and_Gyro_B.dataw = 6912U;
310
311 /* Outputs for Function Call SubSystem: '<S17>/i2cWr' */
312 integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw,
313 &integ_Accel_and_Gyro_DW.i2cWr);
314
315 /* End of Outputs for SubSystem: '<S17>/i2cWr' */
316
317 /* FSEL= 0 gyro 0x00=250°/s */
318 /* '<S17>:34:4' i2cWr(uint16(hex2dec( '1C08' ))); */
319 /* Simulink Function 'i2cWr': '<S17>:28' */
320 integ_Accel_and_Gyro_B.dataw = 7176U;
321
322 /* Outputs for Function Call SubSystem: '<S17>/i2cWr' */
323 integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw,
324 &integ_Accel_and_Gyro_DW.i2cWr);
325
326 /* End of Outputs for SubSystem: '<S17>/i2cWr' */
327
328 /* acce 0x08= 4G */
329 /* '<S17>:34:5' i2cWr(uint16(hex2dec( '1A06' ))); */
330 /* Simulink Function 'i2cWr': '<S17>:28' */
331 integ_Accel_and_Gyro_B.dataw = 6662U;
332
333 /* Outputs for Function Call SubSystem: '<S17>/i2cWr' */
334 integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw,
335 &integ_Accel_and_Gyro_DW.i2cWr);
336
337 /* End of Outputs for SubSystem: '<S17>/i2cWr' */
338
339 /* filtrage+actu 0x06= 19ms */
340 /* '<S17>:34:6' i2cWr(uint16(hex2dec( '3710' ))); */
341 /* Simulink Function 'i2cWr': '<S17>:28' */
342 integ_Accel_and_Gyro_B.dataw = 14096U;
343
344 /* Outputs for Function Call SubSystem: '<S17>/i2cWr' */
345 integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw,
346 &integ_Accel_and_Gyro_DW.i2cWr);
347
348 /* End of Outputs for SubSystem: '<S17>/i2cWr' */
349
350 /* INT_PIN_CFG */
351 /* '<S17>:34:7' i2cWr(uint16(hex2dec( '3800' ))); */
352 /* Simulink Function 'i2cWr': '<S17>:28' */
353 integ_Accel_and_Gyro_B.dataw = 14336U;
354
355 /* Outputs for Function Call SubSystem: '<S17>/i2cWr' */
356 integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw,
357 &integ_Accel_and_Gyro_DW.i2cWr);
358
359 /* End of Outputs for SubSystem: '<S17>/i2cWr' */
360
361 /* Interrupt Enable 0 désactivé */
362 }
363 } else {
364 switch (integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro) {
365 case i_IN_Lecture_acceleration3_axes:
366 /* Outputs for Function Call SubSystem: '<S17>/i2cRd' */
367 /* Start for MATLABSystem: '<S18>/I2C Master Read' incorporates:
368 * MATLABSystem: '<S18>/I2C Master Read'
369 */
370 /* During 'Lecture_acceleration3_axes': '<S17>:52' */
371 /* '<S17>:52:1' val=i2cRd; */
372 /* Simulink Function 'i2cRd': '<S17>:56' */
373 p = false;
374 p_0 = true;
375 if (!(integ_Accel_and_Gyro_DW.obj_d.SampleTime ==
376 integ_Accel_and_Gyro_P.I2CMasterRead_SampleTime)) {
377 p_0 = false;
378 }
379
380 if (p_0) {
381 p = true;
382 }
383
384 if (!p) {
385 integ_Accel_and_Gyro_DW.obj_d.SampleTime =
386 integ_Accel_and_Gyro_P.I2CMasterRead_SampleTime;
387 }
388
389 for (integ_Accel_and_Gyro_B.i = 0; integ_Accel_and_Gyro_B.i < 7;
390 integ_Accel_and_Gyro_B.i++) {
391 integ_Accel_and_Gyro_B.output[integ_Accel_and_Gyro_B.i] = 0U;
392 }
393
394 status = integ_Accel_and_RegisterAddress;
395 integ_Accel_and_Gyro_B.i2cname = 0;
396 status = MW_I2C_MasterWrite(integ_Accel_and_Gyro_B.i2cname,
397 integ_Accel_and__SlaveAddress_j, &status, 1U, true, false);
398 if (0 == status) {
399 integ_Accel_and_Gyro_B.i2cname = 0;
400 MW_I2C_MasterRead(integ_Accel_and_Gyro_B.i2cname,
401 integ_Accel_and__SlaveAddress_j,
402 integ_Accel_and_Gyro_B.output_raw, 14U, false, true);
403 memcpy((void *)&integ_Accel_and_Gyro_B.output[0], (void *)
404 &integ_Accel_and_Gyro_B.output_raw[0], (size_t)7 * sizeof
405 (uint16_T));
406 for (integ_Accel_and_Gyro_B.i = 0; integ_Accel_and_Gyro_B.i < 7;
407 integ_Accel_and_Gyro_B.i++) {
408 output_0 = integ_Accel_and_Gyro_B.output[integ_Accel_and_Gyro_B.i];
409 memcpy((void *)&y[0], (void *)&output_0, (size_t)2 * sizeof(uint8_T));
410 x[0] = y[1];
411 x[1] = y[0];
412 memcpy((void *)&output_0, (void *)&x[0], (size_t)1 * sizeof(uint16_T));
413 integ_Accel_and_Gyro_B.output[integ_Accel_and_Gyro_B.i] = output_0;
414 }
415 }
416
417 /* DataTypeConversion: '<S18>/Data Type Conversion' incorporates:
418 * MATLABSystem: '<S18>/I2C Master Read'
419 * Start for MATLABSystem: '<S18>/I2C Master Read'
420 */
421 for (integ_Accel_and_Gyro_B.i = 0; integ_Accel_and_Gyro_B.i < 7;
422 integ_Accel_and_Gyro_B.i++) {
423 integ_Accel_and_Gyro_B.DataTypeConversion[integ_Accel_and_Gyro_B.i] =
424 (int16_T)integ_Accel_and_Gyro_B.output[integ_Accel_and_Gyro_B.i];
425 }
426
427 /* End of DataTypeConversion: '<S18>/Data Type Conversion' */
428 integ_Accel_and_Gyro_DW.i2cRd_SubsysRanBC = 4;
429
430 /* End of Outputs for SubSystem: '<S17>/i2cRd' */
431 /* '<S17>:52:1' acc=val(1:3); */
432 /* '<S17>:52:1' gyro=val(5:7); */
433 integ_Accel_and_Gyro_B.acc[0] =
434 integ_Accel_and_Gyro_B.DataTypeConversion[0];
435 integ_Accel_and_Gyro_B.gyro[0] =
436 integ_Accel_and_Gyro_B.DataTypeConversion[4];
437 integ_Accel_and_Gyro_B.acc[1] =
438 integ_Accel_and_Gyro_B.DataTypeConversion[1];
439 integ_Accel_and_Gyro_B.gyro[1] =
440 integ_Accel_and_Gyro_B.DataTypeConversion[5];
441 integ_Accel_and_Gyro_B.acc[2] =
442 integ_Accel_and_Gyro_B.DataTypeConversion[2];
443 integ_Accel_and_Gyro_B.gyro[2] =
444 integ_Accel_and_Gyro_B.DataTypeConversion[6];
445
446 /* '<S17>:52:3' temp=val(4:4); */
447 integ_Accel_and_Gyro_B.temp = integ_Accel_and_Gyro_B.DataTypeConversion
448 [3];
449 break;
450
451 case integ_Accel_a_IN_initialisation:
452 /* During 'initialisation': '<S17>:34' */
453 /* Transition: '<S17>:60' */
454 integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro =
455 i_IN_Lecture_acceleration3_axes;
456 break;
457 }
458 }
459
460 /* End of Chart: '<S2>/MPU6050' */
461
462 /* Gain: '<S2>/Gain' */
463 integ_Accel_and_Gyro_B.Gain[0] = (real32_T)integ_Accel_and_Gyro_P.Gain_Gain *
464 7.4505806E-9F * (real32_T)integ_Accel_and_Gyro_B.acc[0];
465 integ_Accel_and_Gyro_B.Gain[1] = (real32_T)integ_Accel_and_Gyro_P.Gain_Gain *
466 7.4505806E-9F * (real32_T)integ_Accel_and_Gyro_B.acc[1];
467 integ_Accel_and_Gyro_B.Gain[2] = (real32_T)integ_Accel_and_Gyro_P.Gain_Gain *
468 7.4505806E-9F * (real32_T)integ_Accel_and_Gyro_B.acc[2];
469
470 /* Outputs for Iterator SubSystem: '<S4>/Control' incorporates:
471 * ForIterator: '<S6>/Iterator'
472 */
473 /* UnitDelay: '<S4>/Unit Delay3' */
474 memcpy(&integ_Accel_and_Gyro_B.Assignment[0],
475 &integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[0], sizeof(real32_T) << 4U);
476
477 /* UnitDelay: '<S4>/Unit Delay2' */
478 integ_Accel_and_Gyro_B.Assignment_c[0] =
479 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[0];
480 integ_Accel_and_Gyro_B.Assignment_c[1] =
481 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[1];
482 integ_Accel_and_Gyro_B.Assignment_c[2] =
483 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[2];
484 integ_Accel_and_Gyro_B.Assignment_c[3] =
485 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[3];
486
487 /* Selector: '<S7>/Selector' incorporates:
488 * Math: '<S13>/Math Function'
489 * S-Function (sdspdiag2): '<S13>/Extract Diagonal'
490 *
491 * About '<S13>/Math Function':
492 * Operator: reciprocal
493 */
494 integ_Accel_and_Gyro_B.s6_iter = 0;
495 while (integ_Accel_and_Gyro_B.s6_iter + 1 <=
496 integ_Accel_and_Gyro_P.Iterator_IterationLimit) {
497 for (integ_Accel_and_Gyro_B.i = 0; integ_Accel_and_Gyro_B.i < 2;
498 integ_Accel_and_Gyro_B.i++) {
499 /* Product: '<S8>/H*P_prdt' incorporates:
500 * Constant: '<S8>/Hcst'
501 * Math: '<S8>/P_prd Trans'
502 * UnitDelay: '<S4>/Unit Delay3'
503 */
504 for (integ_Accel_and_Gyro_B.idxV = 0; integ_Accel_and_Gyro_B.idxV < 4;
505 integ_Accel_and_Gyro_B.idxV++) {
506 integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.i +
507 (integ_Accel_and_Gyro_B.idxV << 1)] = 0.0F;
508 integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.i +
509 (integ_Accel_and_Gyro_B.idxV << 1)] +=
510 integ_Accel_and_Gyro_P.Hcst_Value[integ_Accel_and_Gyro_B.i] *
511 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[integ_Accel_and_Gyro_B.idxV];
512 integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.i +
513 (integ_Accel_and_Gyro_B.idxV << 1)] +=
514 integ_Accel_and_Gyro_P.Hcst_Value[integ_Accel_and_Gyro_B.i + 2] *
515 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[integ_Accel_and_Gyro_B.idxV
516 + 4];
517 integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.i +
518 (integ_Accel_and_Gyro_B.idxV << 1)] +=
519 integ_Accel_and_Gyro_P.Hcst_Value[integ_Accel_and_Gyro_B.i + 4] *
520 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[integ_Accel_and_Gyro_B.idxV
521 + 8];
522 integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.i +
523 (integ_Accel_and_Gyro_B.idxV << 1)] +=
524 integ_Accel_and_Gyro_P.Hcst_Value[integ_Accel_and_Gyro_B.i + 6] *
525 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[integ_Accel_and_Gyro_B.idxV
526 + 12];
527 }
528
529 /* End of Product: '<S8>/H*P_prdt' */
530
531 /* Sum: '<S8>/H*P_prdt*Ht+R' incorporates:
532 * Constant: '<S8>/Constant2'
533 * Constant: '<S8>/Hcst_t'
534 * Product: '<S8>/H*P_prdt*Ht'
535 */
536 for (integ_Accel_and_Gyro_B.idxV = 0; integ_Accel_and_Gyro_B.idxV < 2;
537 integ_Accel_and_Gyro_B.idxV++) {
538 integ_Accel_and_Gyro_B.LDLFactorization[integ_Accel_and_Gyro_B.i +
539 (integ_Accel_and_Gyro_B.idxV << 1)] =
540 (((integ_Accel_and_Gyro_P.Hcst_t_Value[(integ_Accel_and_Gyro_B.idxV <<
541 2) + 1] * integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.i + 2]
542 + integ_Accel_and_Gyro_P.Hcst_t_Value[integ_Accel_and_Gyro_B.idxV
543 << 2] * integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.i]) +
544 integ_Accel_and_Gyro_P.Hcst_t_Value[(integ_Accel_and_Gyro_B.idxV <<
545 2) + 2] * integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.i + 4])
546 + integ_Accel_and_Gyro_P.Hcst_t_Value[(integ_Accel_and_Gyro_B.idxV <<
547 2) + 3] * integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.i + 6])
548 + integ_Accel_and_Gyro_P.Constant2_Value
549 [(integ_Accel_and_Gyro_B.idxV << 1) + integ_Accel_and_Gyro_B.i];
550 }
551
552 /* End of Sum: '<S8>/H*P_prdt*Ht+R' */
553 }
554
555 /* S-Function (sdspldl2): '<S13>/LDL Factorization' */
556 LDLf_int32_Treal32_T(&integ_Accel_and_Gyro_B.LDLFactorization[0U],
557 &integ_Accel_and_Gyro_DW.LDLFactorization_VMX[0U], 2,
558 &integ_Accel_and_Gyro_B.LDLFactorization[0U]);
559
560 /* S-Function (sdspfbsub2): '<S13>/Forward Substitution' */
561 integ_Accel_and_Gyro_B.idxV = 1;
562 integ_Accel_and_Gyro_B.accTmp = integ_Accel_and_Gyro_B.y[1];
563 integ_Accel_and_Gyro_B.i = 0;
564 while (integ_Accel_and_Gyro_B.i < 1) {
565 integ_Accel_and_Gyro_B.accTmp -=
566 integ_Accel_and_Gyro_B.LDLFactorization[integ_Accel_and_Gyro_B.idxV] *
567 integ_Accel_and_Gyro_B.y[0];
568 integ_Accel_and_Gyro_B.idxV += 2;
569 integ_Accel_and_Gyro_B.i = 1;
570 }
571
572 integ_Accel_and_Gyro_B.y[1] = integ_Accel_and_Gyro_B.accTmp;
573 integ_Accel_and_Gyro_B.idxV = 1;
574 integ_Accel_and_Gyro_B.accTmp = integ_Accel_and_Gyro_B.y[3];
575 integ_Accel_and_Gyro_B.i = 0;
576 while (integ_Accel_and_Gyro_B.i < 1) {
577 integ_Accel_and_Gyro_B.accTmp -=
578 integ_Accel_and_Gyro_B.LDLFactorization[integ_Accel_and_Gyro_B.idxV] *
579 integ_Accel_and_Gyro_B.y[2];
580 integ_Accel_and_Gyro_B.idxV += 2;
581 integ_Accel_and_Gyro_B.i = 1;
582 }
583
584 integ_Accel_and_Gyro_B.y[3] = integ_Accel_and_Gyro_B.accTmp;
585 integ_Accel_and_Gyro_B.idxV = 1;
586 integ_Accel_and_Gyro_B.accTmp = integ_Accel_and_Gyro_B.y[5];
587 integ_Accel_and_Gyro_B.i = 0;
588 while (integ_Accel_and_Gyro_B.i < 1) {
589 integ_Accel_and_Gyro_B.accTmp -=
590 integ_Accel_and_Gyro_B.LDLFactorization[integ_Accel_and_Gyro_B.idxV] *
591 integ_Accel_and_Gyro_B.y[4];
592 integ_Accel_and_Gyro_B.idxV += 2;
593 integ_Accel_and_Gyro_B.i = 1;
594 }
595
596 integ_Accel_and_Gyro_B.y[5] = integ_Accel_and_Gyro_B.accTmp;
597 integ_Accel_and_Gyro_B.idxV = 1;
598 integ_Accel_and_Gyro_B.accTmp = integ_Accel_and_Gyro_B.y[7];
599 integ_Accel_and_Gyro_B.i = 0;
600 while (integ_Accel_and_Gyro_B.i < 1) {
601 integ_Accel_and_Gyro_B.accTmp -=
602 integ_Accel_and_Gyro_B.LDLFactorization[integ_Accel_and_Gyro_B.idxV] *
603 integ_Accel_and_Gyro_B.y[6];
604 integ_Accel_and_Gyro_B.idxV += 2;
605 integ_Accel_and_Gyro_B.i = 1;
606 }
607
608 integ_Accel_and_Gyro_B.y[7] = integ_Accel_and_Gyro_B.accTmp;
609
610 /* End of S-Function (sdspfbsub2): '<S13>/Forward Substitution' */
611 integ_Accel_and_Gyro_B.ZHX_prd[0] = 1.0F /
612 integ_Accel_and_Gyro_B.LDLFactorization[0];
613
614 /* Math: '<S13>/Math Function' incorporates:
615 * S-Function (sdspdiag2): '<S13>/Extract Diagonal'
616 *
617 * About '<S13>/Math Function':
618 * Operator: reciprocal
619 */
620 integ_Accel_and_Gyro_B.accTmp = 1.0F /
621 integ_Accel_and_Gyro_B.LDLFactorization[3];
622
623 /* S-Function (sdspdmult2): '<S13>/Matrix Scaling' */
624 integ_Accel_and_Gyro_B.idxV = 0;
625 for (integ_Accel_and_Gyro_B.i = 0; integ_Accel_and_Gyro_B.i < 4;
626 integ_Accel_and_Gyro_B.i++) {
627 integ_Accel_and_Gyro_B.BackwardSubstitution[integ_Accel_and_Gyro_B.idxV]
628 = integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.idxV] *
629 integ_Accel_and_Gyro_B.ZHX_prd[0];
630 integ_Accel_and_Gyro_B.idxV++;
631 integ_Accel_and_Gyro_B.BackwardSubstitution[integ_Accel_and_Gyro_B.idxV]
632 = integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.idxV] *
633 integ_Accel_and_Gyro_B.accTmp;
634 integ_Accel_and_Gyro_B.idxV++;
635 }
636
637 /* End of S-Function (sdspdmult2): '<S13>/Matrix Scaling' */
638
639 /* S-Function (sdspfbsub2): '<S13>/Backward Substitution' */
640 integ_Accel_and_Gyro_B.idxV = 0;
641 integ_Accel_and_Gyro_B.accTmp =
642 integ_Accel_and_Gyro_B.BackwardSubstitution[0];
643 integ_Accel_and_Gyro_B.i = 1;
644 while (integ_Accel_and_Gyro_B.i > 0) {
645 integ_Accel_and_Gyro_B.accTmp -=
646 integ_Accel_and_Gyro_B.LDLFactorization[integ_Accel_and_Gyro_B.idxV +
647 2] * integ_Accel_and_Gyro_B.BackwardSubstitution[1];
648 integ_Accel_and_Gyro_B.idxV -= 2;
649 integ_Accel_and_Gyro_B.i = 0;
650 }
651
652 integ_Accel_and_Gyro_B.BackwardSubstitution[0] =
653 integ_Accel_and_Gyro_B.accTmp;
654 integ_Accel_and_Gyro_B.idxV = 0;
655 integ_Accel_and_Gyro_B.accTmp =
656 integ_Accel_and_Gyro_B.BackwardSubstitution[2];
657 integ_Accel_and_Gyro_B.i = 1;
658 while (integ_Accel_and_Gyro_B.i > 0) {
659 integ_Accel_and_Gyro_B.accTmp -=
660 integ_Accel_and_Gyro_B.LDLFactorization[integ_Accel_and_Gyro_B.idxV +
661 2] * integ_Accel_and_Gyro_B.BackwardSubstitution[3];
662 integ_Accel_and_Gyro_B.idxV -= 2;
663 integ_Accel_and_Gyro_B.i = 0;
664 }
665
666 integ_Accel_and_Gyro_B.BackwardSubstitution[2] =
667 integ_Accel_and_Gyro_B.accTmp;
668 integ_Accel_and_Gyro_B.idxV = 0;
669 integ_Accel_and_Gyro_B.accTmp =
670 integ_Accel_and_Gyro_B.BackwardSubstitution[4];
671 integ_Accel_and_Gyro_B.i = 1;
672 while (integ_Accel_and_Gyro_B.i > 0) {
673 integ_Accel_and_Gyro_B.accTmp -=
674 integ_Accel_and_Gyro_B.LDLFactorization[integ_Accel_and_Gyro_B.idxV +
675 2] * integ_Accel_and_Gyro_B.BackwardSubstitution[5];
676 integ_Accel_and_Gyro_B.idxV -= 2;
677 integ_Accel_and_Gyro_B.i = 0;
678 }
679
680 integ_Accel_and_Gyro_B.BackwardSubstitution[4] =
681 integ_Accel_and_Gyro_B.accTmp;
682 integ_Accel_and_Gyro_B.idxV = 0;
683 integ_Accel_and_Gyro_B.accTmp =
684 integ_Accel_and_Gyro_B.BackwardSubstitution[6];
685 integ_Accel_and_Gyro_B.i = 1;
686 while (integ_Accel_and_Gyro_B.i > 0) {
687 integ_Accel_and_Gyro_B.accTmp -=
688 integ_Accel_and_Gyro_B.LDLFactorization[integ_Accel_and_Gyro_B.idxV +
689 2] * integ_Accel_and_Gyro_B.BackwardSubstitution[7];
690 integ_Accel_and_Gyro_B.idxV -= 2;
691 integ_Accel_and_Gyro_B.i = 0;
692 }
693
694 integ_Accel_and_Gyro_B.BackwardSubstitution[6] =
695 integ_Accel_and_Gyro_B.accTmp;
696
697 /* End of S-Function (sdspfbsub2): '<S13>/Backward Substitution' */
698 for (integ_Accel_and_Gyro_B.i = 0; integ_Accel_and_Gyro_B.i < 2;
699 integ_Accel_and_Gyro_B.i++) {
700 for (integ_Accel_and_Gyro_B.idxV = 0; integ_Accel_and_Gyro_B.idxV < 4;
701 integ_Accel_and_Gyro_B.idxV++) {
702 /* Math: '<S8>/K Trans' */
703 integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.idxV +
704 (integ_Accel_and_Gyro_B.i << 2)] =
705 integ_Accel_and_Gyro_B.BackwardSubstitution
706 [(integ_Accel_and_Gyro_B.idxV << 1) + integ_Accel_and_Gyro_B.i];
707
708 /* Product: '<S8>/K*H*P_prd' incorporates:
709 * Constant: '<S8>/Hcst'
710 */
711 integ_Accel_and_Gyro_B.fv2[integ_Accel_and_Gyro_B.i +
712 (integ_Accel_and_Gyro_B.idxV << 1)] = 0.0F;
713 integ_Accel_and_Gyro_B.fv2[integ_Accel_and_Gyro_B.i +
714 (integ_Accel_and_Gyro_B.idxV << 1)] +=
715 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[integ_Accel_and_Gyro_B.idxV
716 << 2] * integ_Accel_and_Gyro_P.Hcst_Value[integ_Accel_and_Gyro_B.i];
717 integ_Accel_and_Gyro_B.fv2[integ_Accel_and_Gyro_B.i +
718 (integ_Accel_and_Gyro_B.idxV << 1)] +=
719 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE
720 [(integ_Accel_and_Gyro_B.idxV << 2) + 1] *
721 integ_Accel_and_Gyro_P.Hcst_Value[integ_Accel_and_Gyro_B.i + 2];
722 integ_Accel_and_Gyro_B.fv2[integ_Accel_and_Gyro_B.i +
723 (integ_Accel_and_Gyro_B.idxV << 1)] +=
724 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE
725 [(integ_Accel_and_Gyro_B.idxV << 2) + 2] *
726 integ_Accel_and_Gyro_P.Hcst_Value[integ_Accel_and_Gyro_B.i + 4];
727 integ_Accel_and_Gyro_B.fv2[integ_Accel_and_Gyro_B.i +
728 (integ_Accel_and_Gyro_B.idxV << 1)] +=
729 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE
730 [(integ_Accel_and_Gyro_B.idxV << 2) + 3] *
731 integ_Accel_and_Gyro_P.Hcst_Value[integ_Accel_and_Gyro_B.i + 6];
732 }
733 }
734
735 for (integ_Accel_and_Gyro_B.i = 0; integ_Accel_and_Gyro_B.i < 4;
736 integ_Accel_and_Gyro_B.i++) {
737 /* Sum: '<S8>/P_prd-K*H*P_prd' incorporates:
738 * Product: '<S8>/A*P*At'
739 * Product: '<S8>/K*H*P_prd'
740 * UnitDelay: '<S4>/Unit Delay3'
741 */
742 for (integ_Accel_and_Gyro_B.idxV = 0; integ_Accel_and_Gyro_B.idxV < 4;
743 integ_Accel_and_Gyro_B.idxV++) {
744 integ_Accel_and_Gyro_B.fv0[integ_Accel_and_Gyro_B.i +
745 (integ_Accel_and_Gyro_B.idxV << 2)] =
746 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE
747 [(integ_Accel_and_Gyro_B.idxV << 2) + integ_Accel_and_Gyro_B.i] -
748 (integ_Accel_and_Gyro_B.fv2[(integ_Accel_and_Gyro_B.idxV << 1) + 1] *
749 integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.i + 4] +
750 integ_Accel_and_Gyro_B.fv2[integ_Accel_and_Gyro_B.idxV << 1] *
751 integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.i]);
752 }
753
754 /* End of Sum: '<S8>/P_prd-K*H*P_prd' */
755
756 /* Product: '<S8>/A*P*At' incorporates:
757 * Constant: '<S8>/Constant4'
758 */
759 for (integ_Accel_and_Gyro_B.idxV = 0; integ_Accel_and_Gyro_B.idxV < 4;
760 integ_Accel_and_Gyro_B.idxV++) {
761 integ_Accel_and_Gyro_B.fv1[integ_Accel_and_Gyro_B.i +
762 (integ_Accel_and_Gyro_B.idxV << 2)] = 0.0F;
763 integ_Accel_and_Gyro_B.fv1[integ_Accel_and_Gyro_B.i +
764 (integ_Accel_and_Gyro_B.idxV << 2)] +=
765 integ_Accel_and_Gyro_P.Constant4_Value[integ_Accel_and_Gyro_B.idxV <<
766 2] * integ_Accel_and_Gyro_B.fv0[integ_Accel_and_Gyro_B.i];
767 integ_Accel_and_Gyro_B.fv1[integ_Accel_and_Gyro_B.i +
768 (integ_Accel_and_Gyro_B.idxV << 2)] +=
769 integ_Accel_and_Gyro_P.Constant4_Value[(integ_Accel_and_Gyro_B.idxV <<
770 2) + 1] * integ_Accel_and_Gyro_B.fv0[integ_Accel_and_Gyro_B.i + 4];
771 integ_Accel_and_Gyro_B.fv1[integ_Accel_and_Gyro_B.i +
772 (integ_Accel_and_Gyro_B.idxV << 2)] +=
773 integ_Accel_and_Gyro_P.Constant4_Value[(integ_Accel_and_Gyro_B.idxV <<
774 2) + 2] * integ_Accel_and_Gyro_B.fv0[integ_Accel_and_Gyro_B.i + 8];
775 integ_Accel_and_Gyro_B.fv1[integ_Accel_and_Gyro_B.i +
776 (integ_Accel_and_Gyro_B.idxV << 2)] +=
777 integ_Accel_and_Gyro_P.Constant4_Value[(integ_Accel_and_Gyro_B.idxV <<
778 2) + 3] * integ_Accel_and_Gyro_B.fv0[integ_Accel_and_Gyro_B.i + 12];
779 }
780 }
781
782 /* Reshape: '<S8>/P_prd_Reshape' incorporates:
783 * Constant: '<S8>/Constant3'
784 * Constant: '<S8>/Constant5'
785 * Constant: '<S8>/Hcst'
786 * Product: '<S8>/A*P*At'
787 * Product: '<S8>/K*H*P_prd'
788 * Sum: '<S8>/A*P*At+Q'
789 * UnitDelay: '<S4>/Unit Delay3'
790 */
791 for (integ_Accel_and_Gyro_B.i = 0; integ_Accel_and_Gyro_B.i < 4;
792 integ_Accel_and_Gyro_B.i++) {
793 for (integ_Accel_and_Gyro_B.idxV = 0; integ_Accel_and_Gyro_B.idxV < 4;
794 integ_Accel_and_Gyro_B.idxV++) {
795 integ_Accel_and_Gyro_B.P_prd_Reshape[integ_Accel_and_Gyro_B.i +
796 (integ_Accel_and_Gyro_B.idxV << 2)] = (((integ_Accel_and_Gyro_B.fv1
797 [(integ_Accel_and_Gyro_B.idxV << 2) + 1] *
798 integ_Accel_and_Gyro_P.Constant3_Value[integ_Accel_and_Gyro_B.i + 4]
799 + integ_Accel_and_Gyro_B.fv1[integ_Accel_and_Gyro_B.idxV << 2] *
800 integ_Accel_and_Gyro_P.Constant3_Value[integ_Accel_and_Gyro_B.i]) +
801 integ_Accel_and_Gyro_B.fv1[(integ_Accel_and_Gyro_B.idxV << 2) + 2] *
802 integ_Accel_and_Gyro_P.Constant3_Value[integ_Accel_and_Gyro_B.i + 8])
803 + integ_Accel_and_Gyro_B.fv1[(integ_Accel_and_Gyro_B.idxV << 2) + 3]
804 * integ_Accel_and_Gyro_P.Constant3_Value[integ_Accel_and_Gyro_B.i +
805 12]) + integ_Accel_and_Gyro_P.Constant5_Value
806 [(integ_Accel_and_Gyro_B.idxV << 2) + integ_Accel_and_Gyro_B.i];
807 }
808 }
809
810 /* End of Reshape: '<S8>/P_prd_Reshape' */
811
812 /* Sum: '<S8>/Z-H*X_prd' incorporates:
813 * Constant: '<S8>/Hcst'
814 * Product: '<S8>/H*X_prd'
815 * UnitDelay: '<S4>/Unit Delay2'
816 */
817 for (integ_Accel_and_Gyro_B.i = 0; integ_Accel_and_Gyro_B.i < 2;
818 integ_Accel_and_Gyro_B.i++) {
819 integ_Accel_and_Gyro_B.ZHX_prd[integ_Accel_and_Gyro_B.i] =
820 integ_Accel_and_Gyro_B.Gain[integ_Accel_and_Gyro_B.i] -
821 (((integ_Accel_and_Gyro_P.Hcst_Value[integ_Accel_and_Gyro_B.i + 2] *
822 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[1] +
823 integ_Accel_and_Gyro_P.Hcst_Value[integ_Accel_and_Gyro_B.i] *
824 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[0]) +
825 integ_Accel_and_Gyro_P.Hcst_Value[integ_Accel_and_Gyro_B.i + 4] *
826 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[2]) +
827 integ_Accel_and_Gyro_P.Hcst_Value[integ_Accel_and_Gyro_B.i + 6] *
828 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[3]);
829 }
830
831 /* End of Sum: '<S8>/Z-H*X_prd' */
832
833 /* Sum: '<S8>/X_prd+K*(Z-H*X_prd)' incorporates:
834 * Product: '<S8>/K*(Z-H*X_prd)'
835 * UnitDelay: '<S4>/Unit Delay2'
836 */
837 for (integ_Accel_and_Gyro_B.i = 0; integ_Accel_and_Gyro_B.i < 4;
838 integ_Accel_and_Gyro_B.i++) {
839 integ_Accel_and_Gyro_B.LDLFactorization[integ_Accel_and_Gyro_B.i] =
840 (integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.i + 4] *
841 integ_Accel_and_Gyro_B.ZHX_prd[1] +
842 integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.i] *
843 integ_Accel_and_Gyro_B.ZHX_prd[0]) +
844 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[integ_Accel_and_Gyro_B.i];
845 }
846
847 /* End of Sum: '<S8>/X_prd+K*(Z-H*X_prd)' */
848
849 /* Assignment: '<S9>/Assignment' */
850 memcpy(&integ_Accel_and_Gyro_B.Assignment[0],
851 &integ_Accel_and_Gyro_B.P_prd_Reshape[0], sizeof(real32_T) << 4U);
852 for (integ_Accel_and_Gyro_B.i = 0; integ_Accel_and_Gyro_B.i < 4;
853 integ_Accel_and_Gyro_B.i++) {
854 /* Assignment: '<S10>/Assignment' */
855 integ_Accel_and_Gyro_B.Assignment_o[integ_Accel_and_Gyro_B.i] =
856 integ_Accel_and_Gyro_B.LDLFactorization[integ_Accel_and_Gyro_B.i];
857
858 /* Product: '<S8>/A*X' incorporates:
859 * Constant: '<S8>/Constant3'
860 */
861 integ_Accel_and_Gyro_B.accTmp =
862 integ_Accel_and_Gyro_P.Constant3_Value[integ_Accel_and_Gyro_B.i + 12] *
863 integ_Accel_and_Gyro_B.LDLFactorization[3] +
864 (integ_Accel_and_Gyro_P.Constant3_Value[integ_Accel_and_Gyro_B.i + 8] *
865 integ_Accel_and_Gyro_B.LDLFactorization[2] +
866 (integ_Accel_and_Gyro_P.Constant3_Value[integ_Accel_and_Gyro_B.i + 4]
867 * integ_Accel_and_Gyro_B.LDLFactorization[1] +
868 integ_Accel_and_Gyro_P.Constant3_Value[integ_Accel_and_Gyro_B.i] *
869 integ_Accel_and_Gyro_B.LDLFactorization[0]));
870
871 /* Assignment: '<S11>/Assignment' incorporates:
872 * Product: '<S8>/A*X'
873 */
874 integ_Accel_and_Gyro_B.Assignment_c[integ_Accel_and_Gyro_B.i] =
875 integ_Accel_and_Gyro_B.accTmp;
876 }
877
878 integ_Accel_and_Gyro_B.s6_iter++;
879 }
880
881 /* End of Selector: '<S7>/Selector' */
882 /* End of Outputs for SubSystem: '<S4>/Control' */
883 /* Gain: '<S2>/Gain2' */
884 integ_Accel_and_Gyro_B.Gain2[0] = (real32_T)
885 integ_Accel_and_Gyro_P.Gain2_Gain * 9.53674316E-7F * (real32_T)
886 integ_Accel_and_Gyro_B.gyro[0];
887 integ_Accel_and_Gyro_B.Gain2[1] = (real32_T)
888 integ_Accel_and_Gyro_P.Gain2_Gain * 9.53674316E-7F * (real32_T)
889 integ_Accel_and_Gyro_B.gyro[1];
890 integ_Accel_and_Gyro_B.Gain2[2] = (real32_T)
891 integ_Accel_and_Gyro_P.Gain2_Gain * 9.53674316E-7F * (real32_T)
892 integ_Accel_and_Gyro_B.gyro[2];
893 }
894
895 /* Step: '<Root>/Step' */
896 integ_Accel_and_Gyro_B.currentTime = integ_Accel_and_Gyro_M->Timing.t[0];
897 if (integ_Accel_and_Gyro_B.currentTime < integ_Accel_and_Gyro_P.Step_Time) {
898 integ_Accel_and_Gyro_B.currentTime = integ_Accel_and_Gyro_P.Step_Y0;
899 } else {
900 integ_Accel_and_Gyro_B.currentTime = integ_Accel_and_Gyro_P.Step_YFinal;
901 }
902
903 /* End of Step: '<Root>/Step' */
904
905 /* Outputs for Enabled SubSystem: '<Root>/Subsystem1' incorporates:
906 * EnablePort: '<S3>/Enable'
907 */
908 if (integ_Accel_and_Gyro_B.currentTime > 0.0) {
909 if (!integ_Accel_and_Gyro_DW.Subsystem1_MODE) {
910 integ_Accel_and_Gyro_DW.Subsystem1_MODE = true;
911 }
912
913 if (integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[2] == 0) {
914 /* DiscreteIntegrator: '<S3>/Discrete-Time Integrator4' */
915 rtb_DiscreteTimeIntegrator4 =
916 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator4_DSTATE;
917
918 /* DiscreteIntegrator: '<S3>/Discrete-Time Integrator6' */
919 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator6 =
920 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator6_DSTATE;
921
922 /* DiscreteIntegrator: '<S3>/Discrete-Time Integrator5' */
923 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator5 =
924 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator5_DSTATE;
925
926 /* DiscreteIntegrator: '<S3>/Discrete-Time Integrator7' */
927 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator7 =
928 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator7_DSTATE;
929
930 /* DiscreteIntegrator: '<S3>/Discrete-Time Integrator' */
931 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator =
932 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator_DSTATE;
933
934 /* DiscreteIntegrator: '<S3>/Discrete-Time Integrator2' */
935 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator2 =
936 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator2_DSTATE;
937
938 /* DiscreteIntegrator: '<S3>/Discrete-Time Integrator1' */
939 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator1 =
940 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator1_DSTATE;
941
942 /* DiscreteIntegrator: '<S3>/Discrete-Time Integrator3' */
943 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator3 =
944 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator3_DSTATE;
945 }
946
947 srUpdateBC(integ_Accel_and_Gyro_DW.Subsystem1_SubsysRanBC);
948 } else {
949 if (integ_Accel_and_Gyro_DW.Subsystem1_MODE) {
950 integ_Accel_and_Gyro_DW.Subsystem1_MODE = false;
951 }
952 }
953
954 /* End of Outputs for SubSystem: '<Root>/Subsystem1' */
955 if (integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[2] == 0) {
956 /* Sum: '<S2>/Sum' incorporates:
957 * Constant: '<S2>/Constant'
958 * Gain: '<S2>/Gain1'
959 */
960 integ_Accel_and_Gyro_B.Sum = (real32_T)integ_Accel_and_Gyro_P.Gain1_Gain *
961 1.1920929E-7F * (real32_T)integ_Accel_and_Gyro_B.temp +
962 integ_Accel_and_Gyro_P.Constant_Value;
963 }
964
965 if (integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[2] == 0) {
966 /* Update for UnitDelay: '<S4>/Unit Delay2' */
967 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[0] =
968 integ_Accel_and_Gyro_B.Assignment_c[0];
969 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[1] =
970 integ_Accel_and_Gyro_B.Assignment_c[1];
971 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[2] =
972 integ_Accel_and_Gyro_B.Assignment_c[2];
973 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[3] =
974 integ_Accel_and_Gyro_B.Assignment_c[3];
975
976 /* Update for UnitDelay: '<S4>/Unit Delay3' */
977 memcpy(&integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[0],
978 &integ_Accel_and_Gyro_B.Assignment[0], sizeof(real32_T) << 4U);
979 }
980
981 /* Update for Enabled SubSystem: '<Root>/Subsystem1' incorporates:
982 * Update for EnablePort: '<S3>/Enable'
983 */
984 if (integ_Accel_and_Gyro_DW.Subsystem1_MODE &&
985 (integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[2] == 0)) {
986 /* Update for DiscreteIntegrator: '<S3>/Discrete-Time Integrator4' */
987 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator4_DSTATE +=
988 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator4_gainval *
989 integ_Accel_and_Gyro_B.Assignment_o[0];
990
991 /* Update for DiscreteIntegrator: '<S3>/Discrete-Time Integrator6' */
992 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator6_DSTATE +=
993 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator6_gainval *
994 rtb_DiscreteTimeIntegrator4;
995
996 /* Update for DiscreteIntegrator: '<S3>/Discrete-Time Integrator5' */
997 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator5_DSTATE +=
998 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator5_gainval *
999 integ_Accel_and_Gyro_B.Assignment_o[2];
1000
1001 /* Update for DiscreteIntegrator: '<S3>/Discrete-Time Integrator7' */
1002 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator7_DSTATE +=
1003 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator7_gainval *
1004 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator5;
1005
1006 /* Update for DiscreteIntegrator: '<S3>/Discrete-Time Integrator' */
1007 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator_DSTATE +=
1008 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator_gainval *
1009 integ_Accel_and_Gyro_B.Gain[0];
1010
1011 /* Update for DiscreteIntegrator: '<S3>/Discrete-Time Integrator2' */
1012 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator2_DSTATE +=
1013 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator2_gainval *
1014 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator;
1015
1016 /* Update for DiscreteIntegrator: '<S3>/Discrete-Time Integrator1' */
1017 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator1_DSTATE +=
1018 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator1_gainval *
1019 integ_Accel_and_Gyro_B.Gain[1];
1020
1021 /* Update for DiscreteIntegrator: '<S3>/Discrete-Time Integrator3' */
1022 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator3_DSTATE +=
1023 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator3_gainval *
1024 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator1;
1025 }
1026
1027 /* End of Update for SubSystem: '<Root>/Subsystem1' */
1028
1029 /* External mode */
1030 rtExtModeUploadCheckTrigger(3);
1031
1032 { /* Sample time: [0.0s, 0.0s] */
1033 rtExtModeUpload(0, integ_Accel_and_Gyro_M->Timing.t[0]);
1034 }
1035
1036 { /* Sample time: [0.05s, 0.0s] */
1037 rtExtModeUpload(1, ((integ_Accel_and_Gyro_M->Timing.clockTick1) * 0.05));
1038 }
1039
1040 if (integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[2] == 0) {/* Sample time: [1.0s, 0.0s] */
1041 rtExtModeUpload(2, ((integ_Accel_and_Gyro_M->Timing.clockTick2) * 1.0));
1042 }
1043
1044 /* signal main to stop simulation */
1045 { /* Sample time: [0.0s, 0.0s] */
1046 if ((rtmGetTFinal(integ_Accel_and_Gyro_M)!=-1) &&
1047 !((rtmGetTFinal(integ_Accel_and_Gyro_M)-integ_Accel_and_Gyro_M->
1048 Timing.t[0]) > integ_Accel_and_Gyro_M->Timing.t[0] * (DBL_EPSILON)))
1049 {
1050 rtmSetErrorStatus(integ_Accel_and_Gyro_M, "Simulation finished");
1051 }
1052
1053 if (rtmGetStopRequested(integ_Accel_and_Gyro_M)) {
1054 rtmSetErrorStatus(integ_Accel_and_Gyro_M, "Simulation finished");
1055 }
1056 }
1057
1058 /* Update absolute time for base rate */
1059 /* The "clockTick0" counts the number of times the code of this task has
1060 * been executed. The absolute time is the multiplication of "clockTick0"
1061 * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
1062 * overflow during the application lifespan selected.
1063 */
1064 integ_Accel_and_Gyro_M->Timing.t[0] =
1065 (++integ_Accel_and_Gyro_M->Timing.clockTick0) *
1066 integ_Accel_and_Gyro_M->Timing.stepSize0;
1067
1068 {
1069 /* Update absolute timer for sample time: [0.05s, 0.0s] */
1070 /* The "clockTick1" counts the number of times the code of this task has
1071 * been executed. The resolution of this integer timer is 0.05, which is the step size
1072 * of the task. Size of "clockTick1" ensures timer will not overflow during the
1073 * application lifespan selected.
1074 */
1075 integ_Accel_and_Gyro_M->Timing.clockTick1++;
1076 }
1077
1078 if (integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[2] == 0) {
1079 /* Update absolute timer for sample time: [1.0s, 0.0s] */
1080 /* The "clockTick2" counts the number of times the code of this task has
1081 * been executed. The resolution of this integer timer is 1.0, which is the step size
1082 * of the task. Size of "clockTick2" ensures timer will not overflow during the
1083 * application lifespan selected.
1084 */
1085 integ_Accel_and_Gyro_M->Timing.clockTick2++;
1086 }
1087
1088 rate_scheduler();
1089}
1090
1091/* Model initialize function */
1092void integ_Accel_and_Gyro_initialize(void)
1093{
1094 /* Registration code */
1095
1096 /* initialize real-time model */
1097 (void) memset((void *)integ_Accel_and_Gyro_M, 0,
1098 sizeof(RT_MODEL_integ_Accel_and_Gyro_T));
1099
1100 {
1101 /* Setup solver object */
1102 rtsiSetSimTimeStepPtr(&integ_Accel_and_Gyro_M->solverInfo,
1103 &integ_Accel_and_Gyro_M->Timing.simTimeStep);
1104 rtsiSetTPtr(&integ_Accel_and_Gyro_M->solverInfo, &rtmGetTPtr
1105 (integ_Accel_and_Gyro_M));
1106 rtsiSetStepSizePtr(&integ_Accel_and_Gyro_M->solverInfo,
1107 &integ_Accel_and_Gyro_M->Timing.stepSize0);
1108 rtsiSetErrorStatusPtr(&integ_Accel_and_Gyro_M->solverInfo,
1109 (&rtmGetErrorStatus(integ_Accel_and_Gyro_M)));
1110 rtsiSetRTModelPtr(&integ_Accel_and_Gyro_M->solverInfo,
1111 integ_Accel_and_Gyro_M);
1112 }
1113
1114 rtsiSetSimTimeStep(&integ_Accel_and_Gyro_M->solverInfo, MAJOR_TIME_STEP);
1115 rtsiSetSolverName(&integ_Accel_and_Gyro_M->solverInfo,"FixedStepDiscrete");
1116 rtmSetTPtr(integ_Accel_and_Gyro_M, &integ_Accel_and_Gyro_M->Timing.tArray[0]);
1117 rtmSetTFinal(integ_Accel_and_Gyro_M, -1);
1118 integ_Accel_and_Gyro_M->Timing.stepSize0 = 0.05;
1119
1120 /* External mode info */
1121 integ_Accel_and_Gyro_M->Sizes.checksums[0] = (2167819526U);
1122 integ_Accel_and_Gyro_M->Sizes.checksums[1] = (3214334288U);
1123 integ_Accel_and_Gyro_M->Sizes.checksums[2] = (1623080433U);
1124 integ_Accel_and_Gyro_M->Sizes.checksums[3] = (2237049995U);
1125
1126 {
1127 static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE;
1128 static RTWExtModeInfo rt_ExtModeInfo;
1129 static const sysRanDType *systemRan[9];
1130 integ_Accel_and_Gyro_M->extModeInfo = (&rt_ExtModeInfo);
1131 rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan);
1132 systemRan[0] = &rtAlwaysEnabled;
1133 systemRan[1] = &rtAlwaysEnabled;
1134 systemRan[2] = &rtAlwaysEnabled;
1135 systemRan[3] = (sysRanDType *)&integ_Accel_and_Gyro_DW.i2cRd_SubsysRanBC;
1136 systemRan[4] = (sysRanDType *)&integ_Accel_and_Gyro_DW.i2cRd_SubsysRanBC;
1137 systemRan[5] = (sysRanDType *)
1138 &integ_Accel_and_Gyro_DW.i2cWr.i2cWr_SubsysRanBC;
1139 systemRan[6] = (sysRanDType *)
1140 &integ_Accel_and_Gyro_DW.i2cWr.i2cWr_SubsysRanBC;
1141 systemRan[7] = &rtAlwaysEnabled;
1142 systemRan[8] = (sysRanDType *)
1143 &integ_Accel_and_Gyro_DW.Subsystem1_SubsysRanBC;
1144 rteiSetModelMappingInfoPtr(integ_Accel_and_Gyro_M->extModeInfo,
1145 &integ_Accel_and_Gyro_M->SpecialInfo.mappingInfo);
1146 rteiSetChecksumsPtr(integ_Accel_and_Gyro_M->extModeInfo,
1147 integ_Accel_and_Gyro_M->Sizes.checksums);
1148 rteiSetTPtr(integ_Accel_and_Gyro_M->extModeInfo, rtmGetTPtr
1149 (integ_Accel_and_Gyro_M));
1150 }
1151
1152 /* block I/O */
1153 (void) memset(((void *) &integ_Accel_and_Gyro_B), 0,
1154 sizeof(B_integ_Accel_and_Gyro_T));
1155
1156 /* states (dwork) */
1157 (void) memset((void *)&integ_Accel_and_Gyro_DW, 0,
1158 sizeof(DW_integ_Accel_and_Gyro_T));
1159
1160 /* data type transition information */
1161 {
1162 static DataTypeTransInfo dtInfo;
1163 (void) memset((char_T *) &dtInfo, 0,
1164 sizeof(dtInfo));
1165 integ_Accel_and_Gyro_M->SpecialInfo.mappingInfo = (&dtInfo);
1166 dtInfo.numDataTypes = 20;
1167 dtInfo.dataTypeSizes = &rtDataTypeSizes[0];
1168 dtInfo.dataTypeNames = &rtDataTypeNames[0];
1169
1170 /* Block I/O transition table */
1171 dtInfo.BTransTable = &rtBTransTable;
1172
1173 /* Parameters transition table */
1174 dtInfo.PTransTable = &rtPTransTable;
1175 }
1176
1177 {
1178 freedomk64f_fxos8700_integ_Ac_T *obj;
1179 uint32_T i2cname;
1180 uint8_T output;
1181 uint8_T status;
1182 uint8_T SwappedDataBytes[2];
1183 int32_T i;
1184
1185 /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */
1186 obj = &integ_Accel_and_Gyro_DW.obj;
1187 integ_Accel_and_Gyro_DW.obj.isInitialized = 0;
1188 obj->i2cobj.isInitialized = 0;
1189
1190 /* [EOF] */
1191 /* */
1192 integ_Accel_and_Gyro_DW.obj.SampleTime =
1193 integ_Accel_and_Gyro_P.FXOS87006AxesSensor_SampleTime;
1194 obj = &integ_Accel_and_Gyro_DW.obj;
1195 integ_Accel_and_Gyro_DW.obj.isInitialized = 1;
1196 i2cname = 0;
1197 MW_I2C_Open(i2cname, 0U);
1198
1199 /* KHz */
1200 obj->i2cobj.BusSpeed = 100000U;
1201 i2cname = 0;
1202 MW_I2C_SetBusSpeed(i2cname, obj->i2cobj.BusSpeed);
1203 SwappedDataBytes[0] = 43U;
1204 SwappedDataBytes[1] = 64U;
1205 i2cname = 0;
1206 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
1207 OSA_TimeDelay(500U);
1208 output = 0U;
1209 status = 42U;
1210 i2cname = 0;
1211 status = MW_I2C_MasterWrite(i2cname, 29U, &status, 1U, true, false);
1212 if (0 == status) {
1213 i2cname = 0;
1214 MW_I2C_MasterRead(i2cname, 29U, &status, 1U, false, true);
1215 memcpy((void *)&output, (void *)&status, (size_t)1 * sizeof(uint8_T));
1216 }
1217
1218 SwappedDataBytes[0] = 42U;
1219 SwappedDataBytes[1] = (uint8_T)(output & 254);
1220 i2cname = 0;
1221 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
1222 SwappedDataBytes[0] = 14U;
1223 SwappedDataBytes[1] = 0U;
1224 i2cname = 0;
1225 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
1226 SwappedDataBytes[0] = 91U;
1227 SwappedDataBytes[1] = 0U;
1228 i2cname = 0;
1229 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
1230 SwappedDataBytes[0] = 42U;
1231 SwappedDataBytes[1] = 1U;
1232 i2cname = 0;
1233 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
1234
1235 /* End of Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */
1236 /* Start for Chart: '<S2>/MPU6050' incorporates:
1237 * Start for SubSystem: '<S17>/i2cWr'
1238 */
1239 integ_Accel_and_Gyr_i2cWr_Start(&integ_Accel_and_Gyro_DW.i2cWr);
1240
1241 /* Start for Chart: '<S2>/MPU6050' incorporates:
1242 * Start for SubSystem: '<S17>/i2cRd'
1243 */
1244 /* Start for MATLABSystem: '<S18>/I2C Master Read' */
1245 integ_Accel_and_Gyro_DW.obj_d.isInitialized = 0;
1246
1247 /* [EOF] */
1248 /* */
1249 integ_Accel_and_Gyro_DW.obj_d.SampleTime =
1250 integ_Accel_and_Gyro_P.I2CMasterRead_SampleTime;
1251 integ_Accel_and_Gyro_DW.obj_d.isInitialized = 1;
1252 i2cname = 0;
1253 MW_I2C_Open(i2cname, 0U);
1254
1255 /* 100KHz. */
1256 /* KHz */
1257 integ_Accel_and_Gyro_DW.obj_d.BusSpeed = 100000U;
1258 i2cname = 0;
1259 MW_I2C_SetBusSpeed(i2cname, integ_Accel_and_Gyro_DW.obj_d.BusSpeed);
1260
1261 /* InitializeConditions for UnitDelay: '<S4>/Unit Delay2' */
1262 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[0] =
1263 integ_Accel_and_Gyro_P.UnitDelay2_InitialCondition[0];
1264 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[1] =
1265 integ_Accel_and_Gyro_P.UnitDelay2_InitialCondition[1];
1266 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[2] =
1267 integ_Accel_and_Gyro_P.UnitDelay2_InitialCondition[2];
1268 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[3] =
1269 integ_Accel_and_Gyro_P.UnitDelay2_InitialCondition[3];
1270
1271 /* InitializeConditions for UnitDelay: '<S4>/Unit Delay3' */
1272 memcpy(&integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[0],
1273 &integ_Accel_and_Gyro_P.UnitDelay3_InitialCondition[0], sizeof
1274 (real32_T) << 4U);
1275 integ_Accel_and_Gyro_DW.is_active_c3_integ_Accel_and_Gy = 0U;
1276 integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro =
1277 integ_Accel__IN_NO_ACTIVE_CHILD;
1278
1279 /* SystemInitialize for Chart: '<S2>/MPU6050' incorporates:
1280 * SystemInitialize for SubSystem: '<S17>/i2cRd'
1281 */
1282 for (i = 0; i < 7; i++) {
1283 /* SystemInitialize for Outport: '<S18>/datar' */
1284 integ_Accel_and_Gyro_B.DataTypeConversion[i] =
1285 integ_Accel_and_Gyro_P.datar_Y0;
1286 }
1287
1288 /* SystemInitialize for Iterator SubSystem: '<S4>/Control' */
1289 /* SystemInitialize for Outport: '<S6>/X_est' */
1290 integ_Accel_and_Gyro_B.Assignment_o[0] = integ_Accel_and_Gyro_P.X_est_Y0;
1291
1292 /* SystemInitialize for Outport: '<S6>/X_prd' */
1293 integ_Accel_and_Gyro_B.Assignment_c[0] = integ_Accel_and_Gyro_P.X_prd_Y0;
1294
1295 /* SystemInitialize for Outport: '<S6>/X_est' */
1296 integ_Accel_and_Gyro_B.Assignment_o[1] = integ_Accel_and_Gyro_P.X_est_Y0;
1297
1298 /* SystemInitialize for Outport: '<S6>/X_prd' */
1299 integ_Accel_and_Gyro_B.Assignment_c[1] = integ_Accel_and_Gyro_P.X_prd_Y0;
1300
1301 /* SystemInitialize for Outport: '<S6>/X_est' */
1302 integ_Accel_and_Gyro_B.Assignment_o[2] = integ_Accel_and_Gyro_P.X_est_Y0;
1303
1304 /* SystemInitialize for Outport: '<S6>/X_prd' */
1305 integ_Accel_and_Gyro_B.Assignment_c[2] = integ_Accel_and_Gyro_P.X_prd_Y0;
1306
1307 /* SystemInitialize for Outport: '<S6>/X_est' */
1308 integ_Accel_and_Gyro_B.Assignment_o[3] = integ_Accel_and_Gyro_P.X_est_Y0;
1309
1310 /* SystemInitialize for Outport: '<S6>/X_prd' */
1311 integ_Accel_and_Gyro_B.Assignment_c[3] = integ_Accel_and_Gyro_P.X_prd_Y0;
1312
1313 /* SystemInitialize for Outport: '<S6>/P_prd' */
1314 for (i = 0; i < 16; i++) {
1315 integ_Accel_and_Gyro_B.Assignment[i] = integ_Accel_and_Gyro_P.P_prd_Y0;
1316 }
1317
1318 /* End of SystemInitialize for Outport: '<S6>/P_prd' */
1319 /* End of SystemInitialize for SubSystem: '<S4>/Control' */
1320
1321 /* SystemInitialize for Enabled SubSystem: '<Root>/Subsystem1' */
1322 /* InitializeConditions for DiscreteIntegrator: '<S3>/Discrete-Time Integrator4' */
1323 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator4_DSTATE =
1324 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator4_IC;
1325
1326 /* InitializeConditions for DiscreteIntegrator: '<S3>/Discrete-Time Integrator6' */
1327 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator6_DSTATE =
1328 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator6_IC;
1329
1330 /* InitializeConditions for DiscreteIntegrator: '<S3>/Discrete-Time Integrator5' */
1331 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator5_DSTATE =
1332 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator5_IC;
1333
1334 /* InitializeConditions for DiscreteIntegrator: '<S3>/Discrete-Time Integrator7' */
1335 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator7_DSTATE =
1336 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator7_IC;
1337
1338 /* InitializeConditions for DiscreteIntegrator: '<S3>/Discrete-Time Integrator' */
1339 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator_DSTATE =
1340 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator_IC;
1341
1342 /* InitializeConditions for DiscreteIntegrator: '<S3>/Discrete-Time Integrator2' */
1343 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator2_DSTATE =
1344 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator2_IC;
1345
1346 /* InitializeConditions for DiscreteIntegrator: '<S3>/Discrete-Time Integrator1' */
1347 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator1_DSTATE =
1348 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator1_IC;
1349
1350 /* InitializeConditions for DiscreteIntegrator: '<S3>/Discrete-Time Integrator3' */
1351 integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator3_DSTATE =
1352 integ_Accel_and_Gyro_P.DiscreteTimeIntegrator3_IC;
1353
1354 /* SystemInitialize for Outport: '<S3>/x_filter' */
1355 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator6 =
1356 integ_Accel_and_Gyro_P.x_filter_Y0;
1357
1358 /* SystemInitialize for Outport: '<S3>/y_filter' */
1359 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator7 =
1360 integ_Accel_and_Gyro_P.y_filter_Y0;
1361
1362 /* SystemInitialize for Outport: '<S3>/x_meas' */
1363 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator2 =
1364 integ_Accel_and_Gyro_P.x_meas_Y0;
1365
1366 /* SystemInitialize for Outport: '<S3>/y_meas' */
1367 integ_Accel_and_Gyro_B.DiscreteTimeIntegrator3 =
1368 integ_Accel_and_Gyro_P.y_meas_Y0;
1369
1370 /* End of SystemInitialize for SubSystem: '<Root>/Subsystem1' */
1371 }
1372}
1373
1374/* Model terminate function */
1375void integ_Accel_and_Gyro_terminate(void)
1376{
1377 uint32_T i2cname;
1378
1379 /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates:
1380 * Terminate for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor'
1381 */
1382 if (integ_Accel_and_Gyro_DW.obj.isInitialized == 1) {
1383 integ_Accel_and_Gyro_DW.obj.isInitialized = 2;
1384 i2cname = 0;
1385 MW_I2C_Close(i2cname);
1386 }
1387
1388 /* End of Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */
1389
1390 /* Terminate for Chart: '<S2>/MPU6050' incorporates:
1391 * Terminate for SubSystem: '<S17>/i2cWr'
1392 */
1393 integ_Accel_and_Gyro_i2cWr_Term(&integ_Accel_and_Gyro_DW.i2cWr);
1394
1395 /* Terminate for Chart: '<S2>/MPU6050' incorporates:
1396 * Terminate for SubSystem: '<S17>/i2cRd'
1397 */
1398 /* Start for MATLABSystem: '<S18>/I2C Master Read' incorporates:
1399 * Terminate for MATLABSystem: '<S18>/I2C Master Read'
1400 */
1401 if (integ_Accel_and_Gyro_DW.obj_d.isInitialized == 1) {
1402 integ_Accel_and_Gyro_DW.obj_d.isInitialized = 2;
1403 i2cname = 0;
1404 MW_I2C_Close(i2cname);
1405 }
1406
1407 /* End of Start for MATLABSystem: '<S18>/I2C Master Read' */
1408}
1409
1410/*
1411 * File trailer for generated code.
1412 *
1413 * [EOF]
1414 */
1415