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 16:13:16 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[1])++;
58 if ((integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[1]) > 9) {/* Sample time: [0.5s, 0.0s] */
59 integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[1] = 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 boolean_T p;
201 boolean_T p_0;
202 uint8_T status;
203 uint8_T y[2];
204 uint8_T x[2];
205 int32_T i;
206 int16_T output;
207 uint16_T output_0;
208
209 /* Reset subsysRan breadcrumbs */
210 srClearBC(integ_Accel_and_Gyro_DW.i2cWr.i2cWr_SubsysRanBC);
211
212 /* Reset subsysRan breadcrumbs */
213 srClearBC(integ_Accel_and_Gyro_DW.i2cRd_SubsysRanBC);
214 if (integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[1] == 0) {
215 /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates:
216 * MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor'
217 */
218 p = false;
219 p_0 = true;
220 if (!(integ_Accel_and_Gyro_DW.obj.SampleTime ==
221 integ_Accel_and_Gyro_P.FXOS87006AxesSensor_SampleTime)) {
222 p_0 = false;
223 }
224
225 if (p_0) {
226 p = true;
227 }
228
229 if (!p) {
230 integ_Accel_and_Gyro_DW.obj.SampleTime =
231 integ_Accel_and_Gyro_P.FXOS87006AxesSensor_SampleTime;
232 }
233
234 integ_Accel_and_Gyro_B.output_k[0] = 0;
235 integ_Accel_and_Gyro_B.output_k[1] = 0;
236 integ_Accel_and_Gyro_B.output_k[2] = 0;
237 status = 1U;
238 integ_Accel_and_Gyro_B.i2cname = 0;
239 status = MW_I2C_MasterWrite(integ_Accel_and_Gyro_B.i2cname, 29U, &status, 1U,
240 true, false);
241 if (0 == status) {
242 integ_Accel_and_Gyro_B.i2cname = 0;
243 MW_I2C_MasterRead(integ_Accel_and_Gyro_B.i2cname, 29U,
244 integ_Accel_and_Gyro_B.output_raw_c, 6U, false, true);
245 memcpy((void *)&integ_Accel_and_Gyro_B.output_k[0], (void *)
246 &integ_Accel_and_Gyro_B.output_raw_c[0], (size_t)3 * sizeof(int16_T));
247 output = integ_Accel_and_Gyro_B.output_k[0];
248 memcpy((void *)&y[0], (void *)&output, (size_t)2 * sizeof(uint8_T));
249 x[0] = y[1];
250 x[1] = y[0];
251 memcpy((void *)&output, (void *)&x[0], (size_t)1 * sizeof(int16_T));
252 integ_Accel_and_Gyro_B.output_k[0] = output;
253 output = integ_Accel_and_Gyro_B.output_k[1];
254 memcpy((void *)&y[0], (void *)&output, (size_t)2 * sizeof(uint8_T));
255 x[0] = y[1];
256 x[1] = y[0];
257 memcpy((void *)&output, (void *)&x[0], (size_t)1 * sizeof(int16_T));
258 integ_Accel_and_Gyro_B.output_k[1] = output;
259 output = integ_Accel_and_Gyro_B.output_k[2];
260 memcpy((void *)&y[0], (void *)&output, (size_t)2 * sizeof(uint8_T));
261 x[0] = y[1];
262 x[1] = y[0];
263 memcpy((void *)&output, (void *)&x[0], (size_t)1 * sizeof(int16_T));
264 integ_Accel_and_Gyro_B.output_k[2] = output;
265 }
266
267 /* MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates:
268 * Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor'
269 */
270 integ_Accel_and_Gyro_B.FXOS87006AxesSensor[0] = (real_T)(int16_T)
271 (integ_Accel_and_Gyro_B.output_k[0] >> 2) * 0.244 / 1000.0;
272 integ_Accel_and_Gyro_B.FXOS87006AxesSensor[1] = (real_T)(int16_T)
273 (integ_Accel_and_Gyro_B.output_k[1] >> 2) * 0.244 / 1000.0;
274 integ_Accel_and_Gyro_B.FXOS87006AxesSensor[2] = (real_T)(int16_T)
275 (integ_Accel_and_Gyro_B.output_k[2] >> 2) * 0.244 / 1000.0;
276
277 /* Chart: '<S2>/MPU6050' */
278 /* Gateway: MPU6050 sensor/MPU6050 */
279 /* During: MPU6050 sensor/MPU6050 */
280 if (integ_Accel_and_Gyro_DW.is_active_c3_integ_Accel_and_Gy == 0U) {
281 /* Entry: MPU6050 sensor/MPU6050 */
282 integ_Accel_and_Gyro_DW.is_active_c3_integ_Accel_and_Gy = 1U;
283
284 /* Entry Internal: MPU6050 sensor/MPU6050 */
285 /* Transition: '<S17>:35' */
286 if (integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro !=
287 integ_Accel_a_IN_initialisation) {
288 integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro =
289 integ_Accel_a_IN_initialisation;
290
291 /* Entry 'initialisation': '<S17>:34' */
292 /* '<S17>:34:1' i2cWr(uint16(hex2dec( '6B00' ))); */
293 /* Simulink Function 'i2cWr': '<S17>:28' */
294 integ_Accel_and_Gyro_B.dataw = 27392U;
295
296 /* Outputs for Function Call SubSystem: '<S17>/i2cWr' */
297 integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw,
298 &integ_Accel_and_Gyro_DW.i2cWr);
299
300 /* End of Outputs for SubSystem: '<S17>/i2cWr' */
301
302 /* Power on */
303 /* '<S17>:34:3' i2cWr(uint16(hex2dec( '1B00' ))); */
304 /* Simulink Function 'i2cWr': '<S17>:28' */
305 integ_Accel_and_Gyro_B.dataw = 6912U;
306
307 /* Outputs for Function Call SubSystem: '<S17>/i2cWr' */
308 integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw,
309 &integ_Accel_and_Gyro_DW.i2cWr);
310
311 /* End of Outputs for SubSystem: '<S17>/i2cWr' */
312
313 /* FSEL= 0 gyro 0x00=250°/s */
314 /* '<S17>:34:4' i2cWr(uint16(hex2dec( '1C08' ))); */
315 /* Simulink Function 'i2cWr': '<S17>:28' */
316 integ_Accel_and_Gyro_B.dataw = 7176U;
317
318 /* Outputs for Function Call SubSystem: '<S17>/i2cWr' */
319 integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw,
320 &integ_Accel_and_Gyro_DW.i2cWr);
321
322 /* End of Outputs for SubSystem: '<S17>/i2cWr' */
323
324 /* acce 0x08= 4G */
325 /* '<S17>:34:5' i2cWr(uint16(hex2dec( '1A06' ))); */
326 /* Simulink Function 'i2cWr': '<S17>:28' */
327 integ_Accel_and_Gyro_B.dataw = 6662U;
328
329 /* Outputs for Function Call SubSystem: '<S17>/i2cWr' */
330 integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw,
331 &integ_Accel_and_Gyro_DW.i2cWr);
332
333 /* End of Outputs for SubSystem: '<S17>/i2cWr' */
334
335 /* filtrage+actu 0x06= 19ms */
336 /* '<S17>:34:6' i2cWr(uint16(hex2dec( '3710' ))); */
337 /* Simulink Function 'i2cWr': '<S17>:28' */
338 integ_Accel_and_Gyro_B.dataw = 14096U;
339
340 /* Outputs for Function Call SubSystem: '<S17>/i2cWr' */
341 integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw,
342 &integ_Accel_and_Gyro_DW.i2cWr);
343
344 /* End of Outputs for SubSystem: '<S17>/i2cWr' */
345
346 /* INT_PIN_CFG */
347 /* '<S17>:34:7' i2cWr(uint16(hex2dec( '3800' ))); */
348 /* Simulink Function 'i2cWr': '<S17>:28' */
349 integ_Accel_and_Gyro_B.dataw = 14336U;
350
351 /* Outputs for Function Call SubSystem: '<S17>/i2cWr' */
352 integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw,
353 &integ_Accel_and_Gyro_DW.i2cWr);
354
355 /* End of Outputs for SubSystem: '<S17>/i2cWr' */
356
357 /* Interrupt Enable 0 désactivé */
358 }
359 } else {
360 switch (integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro) {
361 case i_IN_Lecture_acceleration3_axes:
362 /* Outputs for Function Call SubSystem: '<S17>/i2cRd' */
363 /* Start for MATLABSystem: '<S18>/I2C Master Read' incorporates:
364 * MATLABSystem: '<S18>/I2C Master Read'
365 */
366 /* During 'Lecture_acceleration3_axes': '<S17>:52' */
367 /* '<S17>:52:1' val=i2cRd; */
368 /* Simulink Function 'i2cRd': '<S17>:56' */
369 p = false;
370 p_0 = true;
371 if (!(integ_Accel_and_Gyro_DW.obj_d.SampleTime ==
372 integ_Accel_and_Gyro_P.I2CMasterRead_SampleTime)) {
373 p_0 = false;
374 }
375
376 if (p_0) {
377 p = true;
378 }
379
380 if (!p) {
381 integ_Accel_and_Gyro_DW.obj_d.SampleTime =
382 integ_Accel_and_Gyro_P.I2CMasterRead_SampleTime;
383 }
384
385 for (i = 0; i < 7; i++) {
386 integ_Accel_and_Gyro_B.output[i] = 0U;
387 }
388
389 status = integ_Accel_and_RegisterAddress;
390 integ_Accel_and_Gyro_B.i2cname = 0;
391 status = MW_I2C_MasterWrite(integ_Accel_and_Gyro_B.i2cname,
392 integ_Accel_and__SlaveAddress_j, &status, 1U, true, false);
393 if (0 == status) {
394 integ_Accel_and_Gyro_B.i2cname = 0;
395 MW_I2C_MasterRead(integ_Accel_and_Gyro_B.i2cname,
396 integ_Accel_and__SlaveAddress_j,
397 integ_Accel_and_Gyro_B.output_raw, 14U, false, true);
398 memcpy((void *)&integ_Accel_and_Gyro_B.output[0], (void *)
399 &integ_Accel_and_Gyro_B.output_raw[0], (size_t)7 * sizeof
400 (uint16_T));
401 for (i = 0; i < 7; i++) {
402 output_0 = integ_Accel_and_Gyro_B.output[i];
403 memcpy((void *)&y[0], (void *)&output_0, (size_t)2 * sizeof(uint8_T));
404 x[0] = y[1];
405 x[1] = y[0];
406 memcpy((void *)&output_0, (void *)&x[0], (size_t)1 * sizeof(uint16_T));
407 integ_Accel_and_Gyro_B.output[i] = output_0;
408 }
409 }
410
411 /* DataTypeConversion: '<S18>/Data Type Conversion' incorporates:
412 * MATLABSystem: '<S18>/I2C Master Read'
413 * Start for MATLABSystem: '<S18>/I2C Master Read'
414 */
415 for (i = 0; i < 7; i++) {
416 integ_Accel_and_Gyro_B.DataTypeConversion[i] = (int16_T)
417 integ_Accel_and_Gyro_B.output[i];
418 }
419
420 /* End of DataTypeConversion: '<S18>/Data Type Conversion' */
421 integ_Accel_and_Gyro_DW.i2cRd_SubsysRanBC = 4;
422
423 /* End of Outputs for SubSystem: '<S17>/i2cRd' */
424 /* '<S17>:52:1' acc=val(1:3); */
425 /* '<S17>:52:1' gyro=val(5:7); */
426 integ_Accel_and_Gyro_B.acc[0] =
427 integ_Accel_and_Gyro_B.DataTypeConversion[0];
428 integ_Accel_and_Gyro_B.gyro[0] =
429 integ_Accel_and_Gyro_B.DataTypeConversion[4];
430 integ_Accel_and_Gyro_B.acc[1] =
431 integ_Accel_and_Gyro_B.DataTypeConversion[1];
432 integ_Accel_and_Gyro_B.gyro[1] =
433 integ_Accel_and_Gyro_B.DataTypeConversion[5];
434 integ_Accel_and_Gyro_B.acc[2] =
435 integ_Accel_and_Gyro_B.DataTypeConversion[2];
436 integ_Accel_and_Gyro_B.gyro[2] =
437 integ_Accel_and_Gyro_B.DataTypeConversion[6];
438
439 /* '<S17>:52:3' temp=val(4:4); */
440 integ_Accel_and_Gyro_B.temp = integ_Accel_and_Gyro_B.DataTypeConversion
441 [3];
442 break;
443
444 case integ_Accel_a_IN_initialisation:
445 /* During 'initialisation': '<S17>:34' */
446 /* Transition: '<S17>:60' */
447 integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro =
448 i_IN_Lecture_acceleration3_axes;
449 break;
450 }
451 }
452
453 /* End of Chart: '<S2>/MPU6050' */
454
455 /* Gain: '<S2>/Gain2' */
456 integ_Accel_and_Gyro_B.Gain2[0] = (real32_T)
457 integ_Accel_and_Gyro_P.Gain2_Gain * 9.53674316E-7F * (real32_T)
458 integ_Accel_and_Gyro_B.gyro[0];
459 integ_Accel_and_Gyro_B.Gain2[1] = (real32_T)
460 integ_Accel_and_Gyro_P.Gain2_Gain * 9.53674316E-7F * (real32_T)
461 integ_Accel_and_Gyro_B.gyro[1];
462 integ_Accel_and_Gyro_B.Gain2[2] = (real32_T)
463 integ_Accel_and_Gyro_P.Gain2_Gain * 9.53674316E-7F * (real32_T)
464 integ_Accel_and_Gyro_B.gyro[2];
465
466 /* Outputs for Iterator SubSystem: '<S4>/Control' incorporates:
467 * ForIterator: '<S6>/Iterator'
468 */
469 /* UnitDelay: '<S4>/Unit Delay3' */
470 memcpy(&integ_Accel_and_Gyro_B.Assignment_m[0],
471 &integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[0], sizeof(real32_T) << 4U);
472
473 /* UnitDelay: '<S4>/Unit Delay2' */
474 integ_Accel_and_Gyro_B.Assignment_c[0] =
475 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[0];
476 integ_Accel_and_Gyro_B.Assignment_c[1] =
477 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[1];
478 integ_Accel_and_Gyro_B.Assignment_c[2] =
479 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[2];
480 integ_Accel_and_Gyro_B.Assignment_c[3] =
481 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[3];
482
483 /* Selector: '<S7>/Selector' incorporates:
484 * Math: '<S13>/Math Function'
485 * S-Function (sdspdiag2): '<S13>/Extract Diagonal'
486 *
487 * About '<S13>/Math Function':
488 * Operator: reciprocal
489 */
490 integ_Accel_and_Gyro_B.s6_iter = 0;
491 while (integ_Accel_and_Gyro_B.s6_iter + 1 <=
492 integ_Accel_and_Gyro_P.Iterator_IterationLimit) {
493 for (i = 0; i < 2; i++) {
494 /* Product: '<S8>/H*P_prdt' incorporates:
495 * Constant: '<S8>/Hcst'
496 * Math: '<S8>/P_prd Trans'
497 * UnitDelay: '<S4>/Unit Delay3'
498 */
499 for (integ_Accel_and_Gyro_B.idxV = 0; integ_Accel_and_Gyro_B.idxV < 4;
500 integ_Accel_and_Gyro_B.idxV++) {
501 integ_Accel_and_Gyro_B.y[i + (integ_Accel_and_Gyro_B.idxV << 1)] =
502 0.0F;
503 integ_Accel_and_Gyro_B.y[i + (integ_Accel_and_Gyro_B.idxV << 1)] +=
504 integ_Accel_and_Gyro_P.Hcst_Value[i] *
505 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[integ_Accel_and_Gyro_B.idxV];
506 integ_Accel_and_Gyro_B.y[i + (integ_Accel_and_Gyro_B.idxV << 1)] +=
507 integ_Accel_and_Gyro_P.Hcst_Value[i + 2] *
508 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[integ_Accel_and_Gyro_B.idxV
509 + 4];
510 integ_Accel_and_Gyro_B.y[i + (integ_Accel_and_Gyro_B.idxV << 1)] +=
511 integ_Accel_and_Gyro_P.Hcst_Value[i + 4] *
512 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[integ_Accel_and_Gyro_B.idxV
513 + 8];
514 integ_Accel_and_Gyro_B.y[i + (integ_Accel_and_Gyro_B.idxV << 1)] +=
515 integ_Accel_and_Gyro_P.Hcst_Value[i + 6] *
516 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[integ_Accel_and_Gyro_B.idxV
517 + 12];
518 }
519
520 /* End of Product: '<S8>/H*P_prdt' */
521
522 /* Sum: '<S8>/H*P_prdt*Ht+R' incorporates:
523 * Constant: '<S8>/Constant2'
524 * Constant: '<S8>/Hcst_t'
525 * Product: '<S8>/H*P_prdt*Ht'
526 */
527 for (integ_Accel_and_Gyro_B.idxV = 0; integ_Accel_and_Gyro_B.idxV < 2;
528 integ_Accel_and_Gyro_B.idxV++) {
529 integ_Accel_and_Gyro_B.Assignment_c[i + (integ_Accel_and_Gyro_B.idxV <<
530 1)] = (((integ_Accel_and_Gyro_P.Hcst_t_Value
531 [(integ_Accel_and_Gyro_B.idxV << 2) + 1] *
532 integ_Accel_and_Gyro_B.y[i + 2] +
533 integ_Accel_and_Gyro_P.Hcst_t_Value[integ_Accel_and_Gyro_B.idxV
534 << 2] * integ_Accel_and_Gyro_B.y[i]) +
535 integ_Accel_and_Gyro_P.Hcst_t_Value
536 [(integ_Accel_and_Gyro_B.idxV << 2) + 2] *
537 integ_Accel_and_Gyro_B.y[i + 4]) +
538 integ_Accel_and_Gyro_P.Hcst_t_Value
539 [(integ_Accel_and_Gyro_B.idxV << 2) + 3] *
540 integ_Accel_and_Gyro_B.y[i + 6]) +
541 integ_Accel_and_Gyro_P.Constant2_Value[(integ_Accel_and_Gyro_B.idxV <<
542 1) + i];
543 }
544
545 /* End of Sum: '<S8>/H*P_prdt*Ht+R' */
546 }
547
548 /* S-Function (sdspldl2): '<S13>/LDL Factorization' */
549 LDLf_int32_Treal32_T(&integ_Accel_and_Gyro_B.Assignment_c[0U],
550 &integ_Accel_and_Gyro_DW.LDLFactorization_VMX[0U], 2,
551 &integ_Accel_and_Gyro_B.Assignment_c[0U]);
552
553 /* S-Function (sdspfbsub2): '<S13>/Forward Substitution' */
554 integ_Accel_and_Gyro_B.idxV = 1;
555 integ_Accel_and_Gyro_B.accTmp = integ_Accel_and_Gyro_B.y[1];
556 i = 0;
557 while (i < 1) {
558 integ_Accel_and_Gyro_B.accTmp -=
559 integ_Accel_and_Gyro_B.Assignment_c[integ_Accel_and_Gyro_B.idxV] *
560 integ_Accel_and_Gyro_B.y[0];
561 integ_Accel_and_Gyro_B.idxV += 2;
562 i = 1;
563 }
564
565 integ_Accel_and_Gyro_B.y[1] = integ_Accel_and_Gyro_B.accTmp;
566 integ_Accel_and_Gyro_B.idxV = 1;
567 integ_Accel_and_Gyro_B.accTmp = integ_Accel_and_Gyro_B.y[3];
568 i = 0;
569 while (i < 1) {
570 integ_Accel_and_Gyro_B.accTmp -=
571 integ_Accel_and_Gyro_B.Assignment_c[integ_Accel_and_Gyro_B.idxV] *
572 integ_Accel_and_Gyro_B.y[2];
573 integ_Accel_and_Gyro_B.idxV += 2;
574 i = 1;
575 }
576
577 integ_Accel_and_Gyro_B.y[3] = integ_Accel_and_Gyro_B.accTmp;
578 integ_Accel_and_Gyro_B.idxV = 1;
579 integ_Accel_and_Gyro_B.accTmp = integ_Accel_and_Gyro_B.y[5];
580 i = 0;
581 while (i < 1) {
582 integ_Accel_and_Gyro_B.accTmp -=
583 integ_Accel_and_Gyro_B.Assignment_c[integ_Accel_and_Gyro_B.idxV] *
584 integ_Accel_and_Gyro_B.y[4];
585 integ_Accel_and_Gyro_B.idxV += 2;
586 i = 1;
587 }
588
589 integ_Accel_and_Gyro_B.y[5] = integ_Accel_and_Gyro_B.accTmp;
590 integ_Accel_and_Gyro_B.idxV = 1;
591 integ_Accel_and_Gyro_B.accTmp = integ_Accel_and_Gyro_B.y[7];
592 i = 0;
593 while (i < 1) {
594 integ_Accel_and_Gyro_B.accTmp -=
595 integ_Accel_and_Gyro_B.Assignment_c[integ_Accel_and_Gyro_B.idxV] *
596 integ_Accel_and_Gyro_B.y[6];
597 integ_Accel_and_Gyro_B.idxV += 2;
598 i = 1;
599 }
600
601 integ_Accel_and_Gyro_B.y[7] = integ_Accel_and_Gyro_B.accTmp;
602
603 /* End of S-Function (sdspfbsub2): '<S13>/Forward Substitution' */
604 integ_Accel_and_Gyro_B.ZHX_prd[0] = 1.0F /
605 integ_Accel_and_Gyro_B.Assignment_c[0];
606
607 /* Math: '<S13>/Math Function' incorporates:
608 * S-Function (sdspdiag2): '<S13>/Extract Diagonal'
609 *
610 * About '<S13>/Math Function':
611 * Operator: reciprocal
612 */
613 integ_Accel_and_Gyro_B.accTmp = 1.0F /
614 integ_Accel_and_Gyro_B.Assignment_c[3];
615
616 /* S-Function (sdspdmult2): '<S13>/Matrix Scaling' */
617 integ_Accel_and_Gyro_B.idxV = 0;
618 for (i = 0; i < 4; i++) {
619 integ_Accel_and_Gyro_B.BackwardSubstitution[integ_Accel_and_Gyro_B.idxV]
620 = integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.idxV] *
621 integ_Accel_and_Gyro_B.ZHX_prd[0];
622 integ_Accel_and_Gyro_B.idxV++;
623 integ_Accel_and_Gyro_B.BackwardSubstitution[integ_Accel_and_Gyro_B.idxV]
624 = integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.idxV] *
625 integ_Accel_and_Gyro_B.accTmp;
626 integ_Accel_and_Gyro_B.idxV++;
627 }
628
629 /* End of S-Function (sdspdmult2): '<S13>/Matrix Scaling' */
630
631 /* S-Function (sdspfbsub2): '<S13>/Backward Substitution' */
632 integ_Accel_and_Gyro_B.idxV = 0;
633 integ_Accel_and_Gyro_B.accTmp =
634 integ_Accel_and_Gyro_B.BackwardSubstitution[0];
635 i = 1;
636 while (i > 0) {
637 integ_Accel_and_Gyro_B.accTmp -=
638 integ_Accel_and_Gyro_B.Assignment_c[integ_Accel_and_Gyro_B.idxV + 2] *
639 integ_Accel_and_Gyro_B.BackwardSubstitution[1];
640 integ_Accel_and_Gyro_B.idxV -= 2;
641 i = 0;
642 }
643
644 integ_Accel_and_Gyro_B.BackwardSubstitution[0] =
645 integ_Accel_and_Gyro_B.accTmp;
646 integ_Accel_and_Gyro_B.idxV = 0;
647 integ_Accel_and_Gyro_B.accTmp =
648 integ_Accel_and_Gyro_B.BackwardSubstitution[2];
649 i = 1;
650 while (i > 0) {
651 integ_Accel_and_Gyro_B.accTmp -=
652 integ_Accel_and_Gyro_B.Assignment_c[integ_Accel_and_Gyro_B.idxV + 2] *
653 integ_Accel_and_Gyro_B.BackwardSubstitution[3];
654 integ_Accel_and_Gyro_B.idxV -= 2;
655 i = 0;
656 }
657
658 integ_Accel_and_Gyro_B.BackwardSubstitution[2] =
659 integ_Accel_and_Gyro_B.accTmp;
660 integ_Accel_and_Gyro_B.idxV = 0;
661 integ_Accel_and_Gyro_B.accTmp =
662 integ_Accel_and_Gyro_B.BackwardSubstitution[4];
663 i = 1;
664 while (i > 0) {
665 integ_Accel_and_Gyro_B.accTmp -=
666 integ_Accel_and_Gyro_B.Assignment_c[integ_Accel_and_Gyro_B.idxV + 2] *
667 integ_Accel_and_Gyro_B.BackwardSubstitution[5];
668 integ_Accel_and_Gyro_B.idxV -= 2;
669 i = 0;
670 }
671
672 integ_Accel_and_Gyro_B.BackwardSubstitution[4] =
673 integ_Accel_and_Gyro_B.accTmp;
674 integ_Accel_and_Gyro_B.idxV = 0;
675 integ_Accel_and_Gyro_B.accTmp =
676 integ_Accel_and_Gyro_B.BackwardSubstitution[6];
677 i = 1;
678 while (i > 0) {
679 integ_Accel_and_Gyro_B.accTmp -=
680 integ_Accel_and_Gyro_B.Assignment_c[integ_Accel_and_Gyro_B.idxV + 2] *
681 integ_Accel_and_Gyro_B.BackwardSubstitution[7];
682 integ_Accel_and_Gyro_B.idxV -= 2;
683 i = 0;
684 }
685
686 integ_Accel_and_Gyro_B.BackwardSubstitution[6] =
687 integ_Accel_and_Gyro_B.accTmp;
688
689 /* End of S-Function (sdspfbsub2): '<S13>/Backward Substitution' */
690 for (i = 0; i < 2; i++) {
691 for (integ_Accel_and_Gyro_B.idxV = 0; integ_Accel_and_Gyro_B.idxV < 4;
692 integ_Accel_and_Gyro_B.idxV++) {
693 /* Math: '<S8>/K Trans' */
694 integ_Accel_and_Gyro_B.y[integ_Accel_and_Gyro_B.idxV + (i << 2)] =
695 integ_Accel_and_Gyro_B.BackwardSubstitution
696 [(integ_Accel_and_Gyro_B.idxV << 1) + i];
697
698 /* Product: '<S8>/K*H*P_prd' incorporates:
699 * Constant: '<S8>/Hcst'
700 */
701 integ_Accel_and_Gyro_B.fv1[i + (integ_Accel_and_Gyro_B.idxV << 1)] =
702 0.0F;
703 integ_Accel_and_Gyro_B.fv1[i + (integ_Accel_and_Gyro_B.idxV << 1)] +=
704 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[integ_Accel_and_Gyro_B.idxV
705 << 2] * integ_Accel_and_Gyro_P.Hcst_Value[i];
706 integ_Accel_and_Gyro_B.fv1[i + (integ_Accel_and_Gyro_B.idxV << 1)] +=
707 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE
708 [(integ_Accel_and_Gyro_B.idxV << 2) + 1] *
709 integ_Accel_and_Gyro_P.Hcst_Value[i + 2];
710 integ_Accel_and_Gyro_B.fv1[i + (integ_Accel_and_Gyro_B.idxV << 1)] +=
711 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE
712 [(integ_Accel_and_Gyro_B.idxV << 2) + 2] *
713 integ_Accel_and_Gyro_P.Hcst_Value[i + 4];
714 integ_Accel_and_Gyro_B.fv1[i + (integ_Accel_and_Gyro_B.idxV << 1)] +=
715 integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE
716 [(integ_Accel_and_Gyro_B.idxV << 2) + 3] *
717 integ_Accel_and_Gyro_P.Hcst_Value[i + 6];
718 }
719 }
720
721 for (i = 0; i < 4; i++) {
722 /* Sum: '<S8>/P_prd-K*H*P_prd' incorporates:
723 * Product: '<S8>/A*P*At'
724 * Product: '<S8>/K*H*P_prd'
725 * UnitDelay: '<S4>/Unit Delay3'
726 */
727 for (integ_Accel_and_Gyro_B.idxV = 0; integ_Accel_and_Gyro_B.idxV < 4;
728 integ_Accel_and_Gyro_B.idxV++) {
729 integ_Accel_and_Gyro_B.Assignment_m[i + (integ_Accel_and_Gyro_B.idxV <<
730 2)] = integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE
731 [(integ_Accel_and_Gyro_B.idxV << 2) + i] -
732 (integ_Accel_and_Gyro_B.fv1[(integ_Accel_and_Gyro_B.idxV << 1) + 1] *
733 integ_Accel_and_Gyro_B.y[i + 4] +
734 integ_Accel_and_Gyro_B.fv1[integ_Accel_and_Gyro_B.idxV << 1] *
735 integ_Accel_and_Gyro_B.y[i]);
736 }
737
738 /* End of Sum: '<S8>/P_prd-K*H*P_prd' */
739
740 /* Product: '<S8>/A*P*At' incorporates:
741 * Constant: '<S8>/Constant4'
742 */
743 for (integ_Accel_and_Gyro_B.idxV = 0; integ_Accel_and_Gyro_B.idxV < 4;
744 integ_Accel_and_Gyro_B.idxV++) {
745 integ_Accel_and_Gyro_B.fv0[i + (integ_Accel_and_Gyro_B.idxV << 2)] =
746 0.0F;
747 integ_Accel_and_Gyro_B.fv0[i + (integ_Accel_and_Gyro_B.idxV << 2)] +=
748 integ_Accel_and_Gyro_P.Constant4_Value[integ_Accel_and_Gyro_B.idxV <<
749 2] * integ_Accel_and_Gyro_B.Assignment_m[i];
750 integ_Accel_and_Gyro_B.fv0[i + (integ_Accel_and_Gyro_B.idxV << 2)] +=
751 integ_Accel_and_Gyro_P.Constant4_Value[(integ_Accel_and_Gyro_B.idxV <<
752 2) + 1] * integ_Accel_and_Gyro_B.Assignment_m[i + 4];
753 integ_Accel_and_Gyro_B.fv0[i + (integ_Accel_and_Gyro_B.idxV << 2)] +=
754 integ_Accel_and_Gyro_P.Constant4_Value[(integ_Accel_and_Gyro_B.idxV <<
755 2) + 2] * integ_Accel_and_Gyro_B.Assignment_m[i + 8];
756 integ_Accel_and_Gyro_B.fv0[i + (integ_Accel_and_Gyro_B.idxV << 2)] +=
757 integ_Accel_and_Gyro_P.Constant4_Value[(integ_Accel_and_Gyro_B.idxV <<
758 2) + 3] * integ_Accel_and_Gyro_B.Assignment_m[i + 12];
759 }
760 }
761
762 /* Reshape: '<S8>/P_prd_Reshape' incorporates:
763 * Constant: '<S8>/Constant3'
764 * Constant: '<S8>/Constant5'
765 * Constant: '<S8>/Hcst'
766 * Product: '<S8>/A*P*At'
767 * Product: '<S8>/K*H*P_prd'
768 * Sum: '<S8>/A*P*At+Q'
769 * UnitDelay: '<S4>/Unit Delay3'
770 */
771 for (i = 0; i < 4; i++) {
772 for (integ_Accel_and_Gyro_B.idxV = 0; integ_Accel_and_Gyro_B.idxV < 4;
773 integ_Accel_and_Gyro_B.idxV++) {
774 integ_Accel_and_Gyro_B.P_prd_Reshape[i + (integ_Accel_and_Gyro_B.idxV <<
775 2)] = (((integ_Accel_and_Gyro_B.fv0[(integ_Accel_and_Gyro_B.idxV <<
776 2) + 1] * integ_Accel_and_Gyro_P.Constant3_Value[i + 4] +
777 integ_Accel_and_Gyro_B.fv0[integ_Accel_and_Gyro_B.idxV << 2]
778 * integ_Accel_and_Gyro_P.Constant3_Value[i]) +
779 integ_Accel_and_Gyro_B.fv0[(integ_Accel_and_Gyro_B.idxV << 2)
780 + 2] * integ_Accel_and_Gyro_P.Constant3_Value[i + 8]) +
781 integ_Accel_and_Gyro_B.fv0[(integ_Accel_and_Gyro_B.idxV << 2)
782 + 3] * integ_Accel_and_Gyro_P.Constant3_Value[i + 12]) +
783 integ_Accel_and_Gyro_P.Constant5_Value[(integ_Accel_and_Gyro_B.idxV <<
784 2) + i];
785 }
786 }
787
788 /* End of Reshape: '<S8>/P_prd_Reshape' */
789
790 /* Sum: '<S8>/Z-H*X_prd' incorporates:
791 * Constant: '<S8>/Hcst'
792 * Product: '<S8>/H*X_prd'
793 * UnitDelay: '<S4>/Unit Delay2'
794 */
795 for (i = 0; i < 2; i++) {
796 integ_Accel_and_Gyro_B.ZHX_prd[i] = integ_Accel_and_Gyro_B.Gain2[i] -
797 (((integ_Accel_and_Gyro_P.Hcst_Value[i + 2] *
798 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[1] +
799 integ_Accel_and_Gyro_P.Hcst_Value[i] *
800 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[0]) +
801 integ_Accel_and_Gyro_P.Hcst_Value[i + 4] *
802 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[2]) +
803 integ_Accel_and_Gyro_P.Hcst_Value[i + 6] *
804 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[3]);
805 }
806
807 /* End of Sum: '<S8>/Z-H*X_prd' */
808
809 /* Sum: '<S8>/X_prd+K*(Z-H*X_prd)' incorporates:
810 * Product: '<S8>/K*(Z-H*X_prd)'
811 * UnitDelay: '<S4>/Unit Delay2'
812 */
813 for (i = 0; i < 4; i++) {
814 integ_Accel_and_Gyro_B.X_prdKZHX_prd[i] = (integ_Accel_and_Gyro_B.y[i +
815 4] * integ_Accel_and_Gyro_B.ZHX_prd[1] + integ_Accel_and_Gyro_B.y[i] *
816 integ_Accel_and_Gyro_B.ZHX_prd[0]) +
817 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[i];
818 }
819
820 /* End of Sum: '<S8>/X_prd+K*(Z-H*X_prd)' */
821
822 /* Assignment: '<S9>/Assignment' */
823 memcpy(&integ_Accel_and_Gyro_B.Assignment_m[0],
824 &integ_Accel_and_Gyro_B.P_prd_Reshape[0], sizeof(real32_T) << 4U);
825 for (i = 0; i < 4; i++) {
826 /* Assignment: '<S10>/Assignment' */
827 integ_Accel_and_Gyro_B.Assignment[i] =
828 integ_Accel_and_Gyro_B.X_prdKZHX_prd[i];
829
830 /* Product: '<S8>/A*X' incorporates:
831 * Constant: '<S8>/Constant3'
832 */
833 integ_Accel_and_Gyro_B.accTmp = integ_Accel_and_Gyro_P.Constant3_Value[i
834 + 12] * integ_Accel_and_Gyro_B.X_prdKZHX_prd[3] +
835 (integ_Accel_and_Gyro_P.Constant3_Value[i + 8] *
836 integ_Accel_and_Gyro_B.X_prdKZHX_prd[2] +
837 (integ_Accel_and_Gyro_P.Constant3_Value[i + 4] *
838 integ_Accel_and_Gyro_B.X_prdKZHX_prd[1] +
839 integ_Accel_and_Gyro_P.Constant3_Value[i] *
840 integ_Accel_and_Gyro_B.X_prdKZHX_prd[0]));
841
842 /* Assignment: '<S11>/Assignment' incorporates:
843 * Product: '<S8>/A*X'
844 */
845 integ_Accel_and_Gyro_B.Assignment_c[i] = integ_Accel_and_Gyro_B.accTmp;
846 }
847
848 integ_Accel_and_Gyro_B.s6_iter++;
849 }
850
851 /* End of Selector: '<S7>/Selector' */
852 /* End of Outputs for SubSystem: '<S4>/Control' */
853 /* Gain: '<S2>/Gain' */
854 integ_Accel_and_Gyro_B.Gain[0] = (real32_T)integ_Accel_and_Gyro_P.Gain_Gain *
855 7.4505806E-9F * (real32_T)integ_Accel_and_Gyro_B.acc[0];
856 integ_Accel_and_Gyro_B.Gain[1] = (real32_T)integ_Accel_and_Gyro_P.Gain_Gain *
857 7.4505806E-9F * (real32_T)integ_Accel_and_Gyro_B.acc[1];
858 integ_Accel_and_Gyro_B.Gain[2] = (real32_T)integ_Accel_and_Gyro_P.Gain_Gain *
859 7.4505806E-9F * (real32_T)integ_Accel_and_Gyro_B.acc[2];
860
861 /* Sum: '<S2>/Sum' incorporates:
862 * Constant: '<S2>/Constant'
863 * Gain: '<S2>/Gain1'
864 */
865 integ_Accel_and_Gyro_B.Sum = (real32_T)integ_Accel_and_Gyro_P.Gain1_Gain *
866 1.1920929E-7F * (real32_T)integ_Accel_and_Gyro_B.temp +
867 integ_Accel_and_Gyro_P.Constant_Value;
868
869 /* Update for UnitDelay: '<S4>/Unit Delay2' */
870 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[0] =
871 integ_Accel_and_Gyro_B.Assignment_c[0];
872 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[1] =
873 integ_Accel_and_Gyro_B.Assignment_c[1];
874 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[2] =
875 integ_Accel_and_Gyro_B.Assignment_c[2];
876 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[3] =
877 integ_Accel_and_Gyro_B.Assignment_c[3];
878
879 /* Update for UnitDelay: '<S4>/Unit Delay3' */
880 memcpy(&integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[0],
881 &integ_Accel_and_Gyro_B.Assignment_m[0], sizeof(real32_T) << 4U);
882 }
883
884 /* External mode */
885 rtExtModeUploadCheckTrigger(2);
886
887 { /* Sample time: [0.05s, 0.0s] */
888 rtExtModeUpload(0, integ_Accel_and_Gyro_M->Timing.taskTime0);
889 }
890
891 if (integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[1] == 0) {/* Sample time: [0.5s, 0.0s] */
892 rtExtModeUpload(1, ((integ_Accel_and_Gyro_M->Timing.clockTick1) * 0.5));
893 }
894
895 /* signal main to stop simulation */
896 { /* Sample time: [0.05s, 0.0s] */
897 if ((rtmGetTFinal(integ_Accel_and_Gyro_M)!=-1) &&
898 !((rtmGetTFinal(integ_Accel_and_Gyro_M)-
899 integ_Accel_and_Gyro_M->Timing.taskTime0) >
900 integ_Accel_and_Gyro_M->Timing.taskTime0 * (DBL_EPSILON))) {
901 rtmSetErrorStatus(integ_Accel_and_Gyro_M, "Simulation finished");
902 }
903
904 if (rtmGetStopRequested(integ_Accel_and_Gyro_M)) {
905 rtmSetErrorStatus(integ_Accel_and_Gyro_M, "Simulation finished");
906 }
907 }
908
909 /* Update absolute time for base rate */
910 /* The "clockTick0" counts the number of times the code of this task has
911 * been executed. The absolute time is the multiplication of "clockTick0"
912 * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
913 * overflow during the application lifespan selected.
914 */
915 integ_Accel_and_Gyro_M->Timing.taskTime0 =
916 (++integ_Accel_and_Gyro_M->Timing.clockTick0) *
917 integ_Accel_and_Gyro_M->Timing.stepSize0;
918 if (integ_Accel_and_Gyro_M->Timing.TaskCounters.TID[1] == 0) {
919 /* Update absolute timer for sample time: [0.5s, 0.0s] */
920 /* The "clockTick1" counts the number of times the code of this task has
921 * been executed. The resolution of this integer timer is 0.5, which is the step size
922 * of the task. Size of "clockTick1" ensures timer will not overflow during the
923 * application lifespan selected.
924 */
925 integ_Accel_and_Gyro_M->Timing.clockTick1++;
926 }
927
928 rate_scheduler();
929}
930
931/* Model initialize function */
932void integ_Accel_and_Gyro_initialize(void)
933{
934 /* Registration code */
935
936 /* initialize real-time model */
937 (void) memset((void *)integ_Accel_and_Gyro_M, 0,
938 sizeof(RT_MODEL_integ_Accel_and_Gyro_T));
939 rtmSetTFinal(integ_Accel_and_Gyro_M, -1);
940 integ_Accel_and_Gyro_M->Timing.stepSize0 = 0.05;
941
942 /* External mode info */
943 integ_Accel_and_Gyro_M->Sizes.checksums[0] = (3129528732U);
944 integ_Accel_and_Gyro_M->Sizes.checksums[1] = (1353015024U);
945 integ_Accel_and_Gyro_M->Sizes.checksums[2] = (265458018U);
946 integ_Accel_and_Gyro_M->Sizes.checksums[3] = (3302255513U);
947
948 {
949 static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE;
950 static RTWExtModeInfo rt_ExtModeInfo;
951 static const sysRanDType *systemRan[8];
952 integ_Accel_and_Gyro_M->extModeInfo = (&rt_ExtModeInfo);
953 rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan);
954 systemRan[0] = &rtAlwaysEnabled;
955 systemRan[1] = &rtAlwaysEnabled;
956 systemRan[2] = &rtAlwaysEnabled;
957 systemRan[3] = (sysRanDType *)&integ_Accel_and_Gyro_DW.i2cRd_SubsysRanBC;
958 systemRan[4] = (sysRanDType *)&integ_Accel_and_Gyro_DW.i2cRd_SubsysRanBC;
959 systemRan[5] = (sysRanDType *)
960 &integ_Accel_and_Gyro_DW.i2cWr.i2cWr_SubsysRanBC;
961 systemRan[6] = (sysRanDType *)
962 &integ_Accel_and_Gyro_DW.i2cWr.i2cWr_SubsysRanBC;
963 systemRan[7] = &rtAlwaysEnabled;
964 rteiSetModelMappingInfoPtr(integ_Accel_and_Gyro_M->extModeInfo,
965 &integ_Accel_and_Gyro_M->SpecialInfo.mappingInfo);
966 rteiSetChecksumsPtr(integ_Accel_and_Gyro_M->extModeInfo,
967 integ_Accel_and_Gyro_M->Sizes.checksums);
968 rteiSetTPtr(integ_Accel_and_Gyro_M->extModeInfo, rtmGetTPtr
969 (integ_Accel_and_Gyro_M));
970 }
971
972 /* block I/O */
973 (void) memset(((void *) &integ_Accel_and_Gyro_B), 0,
974 sizeof(B_integ_Accel_and_Gyro_T));
975
976 /* states (dwork) */
977 (void) memset((void *)&integ_Accel_and_Gyro_DW, 0,
978 sizeof(DW_integ_Accel_and_Gyro_T));
979
980 /* data type transition information */
981 {
982 static DataTypeTransInfo dtInfo;
983 (void) memset((char_T *) &dtInfo, 0,
984 sizeof(dtInfo));
985 integ_Accel_and_Gyro_M->SpecialInfo.mappingInfo = (&dtInfo);
986 dtInfo.numDataTypes = 20;
987 dtInfo.dataTypeSizes = &rtDataTypeSizes[0];
988 dtInfo.dataTypeNames = &rtDataTypeNames[0];
989
990 /* Block I/O transition table */
991 dtInfo.BTransTable = &rtBTransTable;
992
993 /* Parameters transition table */
994 dtInfo.PTransTable = &rtPTransTable;
995 }
996
997 {
998 freedomk64f_fxos8700_integ_Ac_T *obj;
999 uint32_T i2cname;
1000 uint8_T output;
1001 uint8_T status;
1002 uint8_T SwappedDataBytes[2];
1003 int32_T i;
1004
1005 /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */
1006 obj = &integ_Accel_and_Gyro_DW.obj;
1007 integ_Accel_and_Gyro_DW.obj.isInitialized = 0;
1008 obj->i2cobj.isInitialized = 0;
1009
1010 /* [EOF] */
1011 /* */
1012 integ_Accel_and_Gyro_DW.obj.SampleTime =
1013 integ_Accel_and_Gyro_P.FXOS87006AxesSensor_SampleTime;
1014 obj = &integ_Accel_and_Gyro_DW.obj;
1015 integ_Accel_and_Gyro_DW.obj.isInitialized = 1;
1016 i2cname = 0;
1017 MW_I2C_Open(i2cname, 0U);
1018
1019 /* KHz */
1020 obj->i2cobj.BusSpeed = 100000U;
1021 i2cname = 0;
1022 MW_I2C_SetBusSpeed(i2cname, obj->i2cobj.BusSpeed);
1023 SwappedDataBytes[0] = 43U;
1024 SwappedDataBytes[1] = 64U;
1025 i2cname = 0;
1026 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
1027 OSA_TimeDelay(500U);
1028 output = 0U;
1029 status = 42U;
1030 i2cname = 0;
1031 status = MW_I2C_MasterWrite(i2cname, 29U, &status, 1U, true, false);
1032 if (0 == status) {
1033 i2cname = 0;
1034 MW_I2C_MasterRead(i2cname, 29U, &status, 1U, false, true);
1035 memcpy((void *)&output, (void *)&status, (size_t)1 * sizeof(uint8_T));
1036 }
1037
1038 SwappedDataBytes[0] = 42U;
1039 SwappedDataBytes[1] = (uint8_T)(output & 254);
1040 i2cname = 0;
1041 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
1042 SwappedDataBytes[0] = 14U;
1043 SwappedDataBytes[1] = 0U;
1044 i2cname = 0;
1045 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
1046 SwappedDataBytes[0] = 91U;
1047 SwappedDataBytes[1] = 0U;
1048 i2cname = 0;
1049 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
1050 SwappedDataBytes[0] = 42U;
1051 SwappedDataBytes[1] = 1U;
1052 i2cname = 0;
1053 MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false);
1054
1055 /* End of Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */
1056 /* Start for Chart: '<S2>/MPU6050' incorporates:
1057 * Start for SubSystem: '<S17>/i2cWr'
1058 */
1059 integ_Accel_and_Gyr_i2cWr_Start(&integ_Accel_and_Gyro_DW.i2cWr);
1060
1061 /* Start for Chart: '<S2>/MPU6050' incorporates:
1062 * Start for SubSystem: '<S17>/i2cRd'
1063 */
1064 /* Start for MATLABSystem: '<S18>/I2C Master Read' */
1065 integ_Accel_and_Gyro_DW.obj_d.isInitialized = 0;
1066
1067 /* [EOF] */
1068 /* */
1069 integ_Accel_and_Gyro_DW.obj_d.SampleTime =
1070 integ_Accel_and_Gyro_P.I2CMasterRead_SampleTime;
1071 integ_Accel_and_Gyro_DW.obj_d.isInitialized = 1;
1072 i2cname = 0;
1073 MW_I2C_Open(i2cname, 0U);
1074
1075 /* 100KHz. */
1076 /* KHz */
1077 integ_Accel_and_Gyro_DW.obj_d.BusSpeed = 100000U;
1078 i2cname = 0;
1079 MW_I2C_SetBusSpeed(i2cname, integ_Accel_and_Gyro_DW.obj_d.BusSpeed);
1080
1081 /* InitializeConditions for UnitDelay: '<S4>/Unit Delay2' */
1082 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[0] =
1083 integ_Accel_and_Gyro_P.UnitDelay2_InitialCondition[0];
1084 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[1] =
1085 integ_Accel_and_Gyro_P.UnitDelay2_InitialCondition[1];
1086 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[2] =
1087 integ_Accel_and_Gyro_P.UnitDelay2_InitialCondition[2];
1088 integ_Accel_and_Gyro_DW.UnitDelay2_DSTATE[3] =
1089 integ_Accel_and_Gyro_P.UnitDelay2_InitialCondition[3];
1090
1091 /* InitializeConditions for UnitDelay: '<S4>/Unit Delay3' */
1092 memcpy(&integ_Accel_and_Gyro_DW.UnitDelay3_DSTATE[0],
1093 &integ_Accel_and_Gyro_P.UnitDelay3_InitialCondition[0], sizeof
1094 (real32_T) << 4U);
1095 integ_Accel_and_Gyro_DW.is_active_c3_integ_Accel_and_Gy = 0U;
1096 integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro =
1097 integ_Accel__IN_NO_ACTIVE_CHILD;
1098
1099 /* SystemInitialize for Chart: '<S2>/MPU6050' incorporates:
1100 * SystemInitialize for SubSystem: '<S17>/i2cRd'
1101 */
1102 for (i = 0; i < 7; i++) {
1103 /* SystemInitialize for Outport: '<S18>/datar' */
1104 integ_Accel_and_Gyro_B.DataTypeConversion[i] =
1105 integ_Accel_and_Gyro_P.datar_Y0;
1106 }
1107
1108 /* SystemInitialize for Iterator SubSystem: '<S4>/Control' */
1109 /* SystemInitialize for Outport: '<S6>/X_est' */
1110 integ_Accel_and_Gyro_B.Assignment[0] = integ_Accel_and_Gyro_P.X_est_Y0;
1111 integ_Accel_and_Gyro_B.Assignment[1] = integ_Accel_and_Gyro_P.X_est_Y0;
1112 integ_Accel_and_Gyro_B.Assignment[2] = integ_Accel_and_Gyro_P.X_est_Y0;
1113 integ_Accel_and_Gyro_B.Assignment[3] = integ_Accel_and_Gyro_P.X_est_Y0;
1114
1115 /* End of SystemInitialize for SubSystem: '<S4>/Control' */
1116 }
1117}
1118
1119/* Model terminate function */
1120void integ_Accel_and_Gyro_terminate(void)
1121{
1122 uint32_T i2cname;
1123
1124 /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates:
1125 * Terminate for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor'
1126 */
1127 if (integ_Accel_and_Gyro_DW.obj.isInitialized == 1) {
1128 integ_Accel_and_Gyro_DW.obj.isInitialized = 2;
1129 i2cname = 0;
1130 MW_I2C_Close(i2cname);
1131 }
1132
1133 /* End of Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */
1134
1135 /* Terminate for Chart: '<S2>/MPU6050' incorporates:
1136 * Terminate for SubSystem: '<S17>/i2cWr'
1137 */
1138 integ_Accel_and_Gyro_i2cWr_Term(&integ_Accel_and_Gyro_DW.i2cWr);
1139
1140 /* Terminate for Chart: '<S2>/MPU6050' incorporates:
1141 * Terminate for SubSystem: '<S17>/i2cRd'
1142 */
1143 /* Start for MATLABSystem: '<S18>/I2C Master Read' incorporates:
1144 * Terminate for MATLABSystem: '<S18>/I2C Master Read'
1145 */
1146 if (integ_Accel_and_Gyro_DW.obj_d.isInitialized == 1) {
1147 integ_Accel_and_Gyro_DW.obj_d.isInitialized = 2;
1148 i2cname = 0;
1149 MW_I2C_Close(i2cname);
1150 }
1151
1152 /* End of Start for MATLABSystem: '<S18>/I2C Master Read' */
1153}
1154
1155/*
1156 * File trailer for generated code.
1157 *
1158 * [EOF]
1159 */
1160