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) */ |
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[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' */ |
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 | 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 */ |
932 | void 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 */ |
1120 | void 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 | |