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) */ |
35 | B_integ_Accel_and_Gyro_T integ_Accel_and_Gyro_B; |
36 | |
37 | /* Block states (auto storage) */ |
38 | DW_integ_Accel_and_Gyro_T integ_Accel_and_Gyro_DW; |
39 | |
40 | /* Real-time model */ |
41 | RT_MODEL_integ_Accel_and_Gyro_T integ_Accel_and_Gyro_M_; |
42 | RT_MODEL_integ_Accel_and_Gyro_T *const integ_Accel_and_Gyro_M = |
43 | &integ_Accel_and_Gyro_M_; |
44 | static 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 | */ |
51 | static 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' */ |
64 | void 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' */ |
85 | void 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' */ |
110 | void 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 | |
126 | void 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 */ |
198 | void 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 */ |
1092 | void 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 */ |
1375 | void 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 | |