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.8 |
11 | * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 |
12 | * C/C++ source code generated on : Wed Dec 06 13:43:27 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 | |
45 | /* Start for function-call system: '<S14>/i2cWr' */ |
46 | void integ_Accel_and_Gyr_i2cWr_Start(DW_i2cWr_integ_Accel_and_Gyro_T *localDW) |
47 | { |
48 | uint32_T i2cname; |
49 | |
50 | /* Start for MATLABSystem: '<S16>/I2C Master Write' */ |
51 | localDW->obj.isInitialized = 0; |
52 | |
53 | /* [EOF] */ |
54 | /* */ |
55 | localDW->obj.isInitialized = 1; |
56 | i2cname = 0; |
57 | MW_I2C_Open(i2cname, 0U); |
58 | |
59 | /* 100KHz. */ |
60 | /* KHz */ |
61 | localDW->obj.BusSpeed = 100000U; |
62 | i2cname = 0; |
63 | MW_I2C_SetBusSpeed(i2cname, localDW->obj.BusSpeed); |
64 | } |
65 | |
66 | /* Output and update for function-call system: '<S14>/i2cWr' */ |
67 | void integ_Accel_and_Gyro_i2cWr(uint16_T rtu_dataw, |
68 | DW_i2cWr_integ_Accel_and_Gyro_T *localDW) |
69 | { |
70 | uint8_T SwappedDataBytes[2]; |
71 | uint32_T i2cname; |
72 | uint16_T x; |
73 | uint8_T x_0[2]; |
74 | |
75 | /* Start for MATLABSystem: '<S16>/I2C Master Write' incorporates: |
76 | * MATLABSystem: '<S16>/I2C Master Write' |
77 | */ |
78 | memcpy((void *)&SwappedDataBytes[0], (void *)&rtu_dataw, (size_t)2 * sizeof |
79 | (uint8_T)); |
80 | x_0[0] = SwappedDataBytes[1]; |
81 | x_0[1] = SwappedDataBytes[0]; |
82 | memcpy((void *)&x, (void *)&x_0[0], (size_t)1 * sizeof(uint16_T)); |
83 | memcpy((void *)&SwappedDataBytes[0], (void *)&x, (size_t)2 * sizeof(uint8_T)); |
84 | i2cname = 0; |
85 | MW_I2C_MasterWrite(i2cname, integ_Accel_and_Gy_SlaveAddress, SwappedDataBytes, |
86 | 2U, integ_Accel_and_G_RepeatedStart, |
87 | integ_Accel_and_Gyro_NoAck); |
88 | localDW->i2cWr_SubsysRanBC = 4; |
89 | } |
90 | |
91 | /* Termination for function-call system: '<S14>/i2cWr' */ |
92 | void integ_Accel_and_Gyro_i2cWr_Term(DW_i2cWr_integ_Accel_and_Gyro_T *localDW) |
93 | { |
94 | uint32_T i2cname; |
95 | |
96 | /* Start for MATLABSystem: '<S16>/I2C Master Write' incorporates: |
97 | * Terminate for MATLABSystem: '<S16>/I2C Master Write' |
98 | */ |
99 | if (localDW->obj.isInitialized == 1) { |
100 | localDW->obj.isInitialized = 2; |
101 | i2cname = 0; |
102 | MW_I2C_Close(i2cname); |
103 | } |
104 | |
105 | /* End of Start for MATLABSystem: '<S16>/I2C Master Write' */ |
106 | } |
107 | |
108 | /* Model step function */ |
109 | void integ_Accel_and_Gyro_step(void) |
110 | { |
111 | boolean_T p; |
112 | boolean_T p_0; |
113 | int16_T output[3]; |
114 | uint8_T status; |
115 | uint32_T i2cname; |
116 | uint8_T output_raw[6]; |
117 | uint8_T output_raw_0[14]; |
118 | uint8_T y[2]; |
119 | uint8_T x[2]; |
120 | int32_T i; |
121 | int16_T output_0; |
122 | uint16_T output_1; |
123 | |
124 | /* Reset subsysRan breadcrumbs */ |
125 | srClearBC(integ_Accel_and_Gyro_DW.i2cWr.i2cWr_SubsysRanBC); |
126 | |
127 | /* Reset subsysRan breadcrumbs */ |
128 | srClearBC(integ_Accel_and_Gyro_DW.i2cRd_SubsysRanBC); |
129 | |
130 | /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates: |
131 | * MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' |
132 | */ |
133 | p = false; |
134 | p_0 = true; |
135 | if (!(integ_Accel_and_Gyro_DW.obj.SampleTime == |
136 | integ_Accel_and_Gyro_P.FXOS87006AxesSensor_SampleTime)) { |
137 | p_0 = false; |
138 | } |
139 | |
140 | if (p_0) { |
141 | p = true; |
142 | } |
143 | |
144 | if (!p) { |
145 | integ_Accel_and_Gyro_DW.obj.SampleTime = |
146 | integ_Accel_and_Gyro_P.FXOS87006AxesSensor_SampleTime; |
147 | } |
148 | |
149 | output[0] = 0; |
150 | output[1] = 0; |
151 | output[2] = 0; |
152 | status = 1U; |
153 | i2cname = 0; |
154 | status = MW_I2C_MasterWrite(i2cname, 29U, &status, 1U, true, false); |
155 | if (0 == status) { |
156 | i2cname = 0; |
157 | MW_I2C_MasterRead(i2cname, 29U, output_raw, 6U, false, true); |
158 | memcpy((void *)&output[0], (void *)&output_raw[0], (size_t)3 * sizeof |
159 | (int16_T)); |
160 | output_0 = output[0]; |
161 | memcpy((void *)&y[0], (void *)&output_0, (size_t)2 * sizeof(uint8_T)); |
162 | x[0] = y[1]; |
163 | x[1] = y[0]; |
164 | memcpy((void *)&output_0, (void *)&x[0], (size_t)1 * sizeof(int16_T)); |
165 | output[0] = output_0; |
166 | output_0 = output[1]; |
167 | memcpy((void *)&y[0], (void *)&output_0, (size_t)2 * sizeof(uint8_T)); |
168 | x[0] = y[1]; |
169 | x[1] = y[0]; |
170 | memcpy((void *)&output_0, (void *)&x[0], (size_t)1 * sizeof(int16_T)); |
171 | output[1] = output_0; |
172 | output_0 = output[2]; |
173 | memcpy((void *)&y[0], (void *)&output_0, (size_t)2 * sizeof(uint8_T)); |
174 | x[0] = y[1]; |
175 | x[1] = y[0]; |
176 | memcpy((void *)&output_0, (void *)&x[0], (size_t)1 * sizeof(int16_T)); |
177 | output[2] = output_0; |
178 | } |
179 | |
180 | /* MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates: |
181 | * Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' |
182 | */ |
183 | integ_Accel_and_Gyro_B.FXOS87006AxesSensor[0] = (real_T)(int16_T)(output[0] >> |
184 | 2) * 0.244 / 1000.0; |
185 | integ_Accel_and_Gyro_B.FXOS87006AxesSensor[1] = (real_T)(int16_T)(output[1] >> |
186 | 2) * 0.244 / 1000.0; |
187 | integ_Accel_and_Gyro_B.FXOS87006AxesSensor[2] = (real_T)(int16_T)(output[2] >> |
188 | 2) * 0.244 / 1000.0; |
189 | |
190 | /* Chart: '<S2>/MPU6050' */ |
191 | /* Gateway: MPU6050 sensor/MPU6050 */ |
192 | /* During: MPU6050 sensor/MPU6050 */ |
193 | if (integ_Accel_and_Gyro_DW.is_active_c3_integ_Accel_and_Gy == 0U) { |
194 | /* Entry: MPU6050 sensor/MPU6050 */ |
195 | integ_Accel_and_Gyro_DW.is_active_c3_integ_Accel_and_Gy = 1U; |
196 | |
197 | /* Entry Internal: MPU6050 sensor/MPU6050 */ |
198 | /* Transition: '<S14>:35' */ |
199 | if (integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro != |
200 | integ_Accel_a_IN_initialisation) { |
201 | integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro = |
202 | integ_Accel_a_IN_initialisation; |
203 | |
204 | /* Entry 'initialisation': '<S14>:34' */ |
205 | /* '<S14>:34:1' i2cWr(uint16(hex2dec( '6B00' ))); */ |
206 | /* Simulink Function 'i2cWr': '<S14>:28' */ |
207 | integ_Accel_and_Gyro_B.dataw = 27392U; |
208 | |
209 | /* Outputs for Function Call SubSystem: '<S14>/i2cWr' */ |
210 | integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw, |
211 | &integ_Accel_and_Gyro_DW.i2cWr); |
212 | |
213 | /* End of Outputs for SubSystem: '<S14>/i2cWr' */ |
214 | |
215 | /* Power on */ |
216 | /* '<S14>:34:3' i2cWr(uint16(hex2dec( '1B00' ))); */ |
217 | /* Simulink Function 'i2cWr': '<S14>:28' */ |
218 | integ_Accel_and_Gyro_B.dataw = 6912U; |
219 | |
220 | /* Outputs for Function Call SubSystem: '<S14>/i2cWr' */ |
221 | integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw, |
222 | &integ_Accel_and_Gyro_DW.i2cWr); |
223 | |
224 | /* End of Outputs for SubSystem: '<S14>/i2cWr' */ |
225 | |
226 | /* FSEL= 0 gyro 0x00=250°/s */ |
227 | /* '<S14>:34:4' i2cWr(uint16(hex2dec( '1C08' ))); */ |
228 | /* Simulink Function 'i2cWr': '<S14>:28' */ |
229 | integ_Accel_and_Gyro_B.dataw = 7176U; |
230 | |
231 | /* Outputs for Function Call SubSystem: '<S14>/i2cWr' */ |
232 | integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw, |
233 | &integ_Accel_and_Gyro_DW.i2cWr); |
234 | |
235 | /* End of Outputs for SubSystem: '<S14>/i2cWr' */ |
236 | |
237 | /* acce 0x08= 4G */ |
238 | /* '<S14>:34:5' i2cWr(uint16(hex2dec( '1A06' ))); */ |
239 | /* Simulink Function 'i2cWr': '<S14>:28' */ |
240 | integ_Accel_and_Gyro_B.dataw = 6662U; |
241 | |
242 | /* Outputs for Function Call SubSystem: '<S14>/i2cWr' */ |
243 | integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw, |
244 | &integ_Accel_and_Gyro_DW.i2cWr); |
245 | |
246 | /* End of Outputs for SubSystem: '<S14>/i2cWr' */ |
247 | |
248 | /* filtrage+actu 0x06= 19ms */ |
249 | /* '<S14>:34:6' i2cWr(uint16(hex2dec( '3710' ))); */ |
250 | /* Simulink Function 'i2cWr': '<S14>:28' */ |
251 | integ_Accel_and_Gyro_B.dataw = 14096U; |
252 | |
253 | /* Outputs for Function Call SubSystem: '<S14>/i2cWr' */ |
254 | integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw, |
255 | &integ_Accel_and_Gyro_DW.i2cWr); |
256 | |
257 | /* End of Outputs for SubSystem: '<S14>/i2cWr' */ |
258 | |
259 | /* INT_PIN_CFG */ |
260 | /* '<S14>:34:7' i2cWr(uint16(hex2dec( '3800' ))); */ |
261 | /* Simulink Function 'i2cWr': '<S14>:28' */ |
262 | integ_Accel_and_Gyro_B.dataw = 14336U; |
263 | |
264 | /* Outputs for Function Call SubSystem: '<S14>/i2cWr' */ |
265 | integ_Accel_and_Gyro_i2cWr(integ_Accel_and_Gyro_B.dataw, |
266 | &integ_Accel_and_Gyro_DW.i2cWr); |
267 | |
268 | /* End of Outputs for SubSystem: '<S14>/i2cWr' */ |
269 | |
270 | /* Interrupt Enable 0 désactivé */ |
271 | } |
272 | } else { |
273 | switch (integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro) { |
274 | case i_IN_Lecture_acceleration3_axes: |
275 | /* Outputs for Function Call SubSystem: '<S14>/i2cRd' */ |
276 | /* Start for MATLABSystem: '<S15>/I2C Master Read' incorporates: |
277 | * MATLABSystem: '<S15>/I2C Master Read' |
278 | */ |
279 | /* During 'Lecture_acceleration3_axes': '<S14>:52' */ |
280 | /* '<S14>:52:1' val=i2cRd; */ |
281 | /* Simulink Function 'i2cRd': '<S14>:56' */ |
282 | p = false; |
283 | p_0 = true; |
284 | if (!(integ_Accel_and_Gyro_DW.obj_d.SampleTime == |
285 | integ_Accel_and_Gyro_P.I2CMasterRead_SampleTime)) { |
286 | p_0 = false; |
287 | } |
288 | |
289 | if (p_0) { |
290 | p = true; |
291 | } |
292 | |
293 | if (!p) { |
294 | integ_Accel_and_Gyro_DW.obj_d.SampleTime = |
295 | integ_Accel_and_Gyro_P.I2CMasterRead_SampleTime; |
296 | } |
297 | |
298 | for (i = 0; i < 7; i++) { |
299 | integ_Accel_and_Gyro_B.output[i] = 0U; |
300 | } |
301 | |
302 | status = integ_Accel_and_RegisterAddress; |
303 | i2cname = 0; |
304 | status = MW_I2C_MasterWrite(i2cname, integ_Accel_and__SlaveAddress_j, |
305 | &status, 1U, true, false); |
306 | if (0 == status) { |
307 | i2cname = 0; |
308 | MW_I2C_MasterRead(i2cname, integ_Accel_and__SlaveAddress_j, output_raw_0, |
309 | 14U, false, true); |
310 | memcpy((void *)&integ_Accel_and_Gyro_B.output[0], (void *)&output_raw_0 |
311 | [0], (size_t)7 * sizeof(uint16_T)); |
312 | for (i = 0; i < 7; i++) { |
313 | output_1 = integ_Accel_and_Gyro_B.output[i]; |
314 | memcpy((void *)&y[0], (void *)&output_1, (size_t)2 * sizeof(uint8_T)); |
315 | x[0] = y[1]; |
316 | x[1] = y[0]; |
317 | memcpy((void *)&output_1, (void *)&x[0], (size_t)1 * sizeof(uint16_T)); |
318 | integ_Accel_and_Gyro_B.output[i] = output_1; |
319 | } |
320 | } |
321 | |
322 | /* DataTypeConversion: '<S15>/Data Type Conversion' incorporates: |
323 | * MATLABSystem: '<S15>/I2C Master Read' |
324 | * Start for MATLABSystem: '<S15>/I2C Master Read' |
325 | */ |
326 | for (i = 0; i < 7; i++) { |
327 | integ_Accel_and_Gyro_B.DataTypeConversion[i] = (int16_T) |
328 | integ_Accel_and_Gyro_B.output[i]; |
329 | } |
330 | |
331 | /* End of DataTypeConversion: '<S15>/Data Type Conversion' */ |
332 | integ_Accel_and_Gyro_DW.i2cRd_SubsysRanBC = 4; |
333 | |
334 | /* End of Outputs for SubSystem: '<S14>/i2cRd' */ |
335 | /* '<S14>:52:1' acc=val(1:3); */ |
336 | /* '<S14>:52:1' gyro=val(5:7); */ |
337 | integ_Accel_and_Gyro_B.acc[0] = integ_Accel_and_Gyro_B.DataTypeConversion |
338 | [0]; |
339 | integ_Accel_and_Gyro_B.gyro[0] = |
340 | integ_Accel_and_Gyro_B.DataTypeConversion[4]; |
341 | integ_Accel_and_Gyro_B.acc[1] = integ_Accel_and_Gyro_B.DataTypeConversion |
342 | [1]; |
343 | integ_Accel_and_Gyro_B.gyro[1] = |
344 | integ_Accel_and_Gyro_B.DataTypeConversion[5]; |
345 | integ_Accel_and_Gyro_B.acc[2] = integ_Accel_and_Gyro_B.DataTypeConversion |
346 | [2]; |
347 | integ_Accel_and_Gyro_B.gyro[2] = |
348 | integ_Accel_and_Gyro_B.DataTypeConversion[6]; |
349 | |
350 | /* '<S14>:52:3' temp=val(4:4); */ |
351 | integ_Accel_and_Gyro_B.temp = integ_Accel_and_Gyro_B.DataTypeConversion[3]; |
352 | break; |
353 | |
354 | case integ_Accel_a_IN_initialisation: |
355 | /* During 'initialisation': '<S14>:34' */ |
356 | /* Transition: '<S14>:60' */ |
357 | integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro = |
358 | i_IN_Lecture_acceleration3_axes; |
359 | break; |
360 | } |
361 | } |
362 | |
363 | /* End of Chart: '<S2>/MPU6050' */ |
364 | |
365 | /* Gain: '<S2>/Gain' */ |
366 | integ_Accel_and_Gyro_B.Gain[0] = (real32_T)integ_Accel_and_Gyro_P.Gain_Gain * |
367 | 7.4505806E-9F * (real32_T)integ_Accel_and_Gyro_B.acc[0]; |
368 | integ_Accel_and_Gyro_B.Gain[1] = (real32_T)integ_Accel_and_Gyro_P.Gain_Gain * |
369 | 7.4505806E-9F * (real32_T)integ_Accel_and_Gyro_B.acc[1]; |
370 | integ_Accel_and_Gyro_B.Gain[2] = (real32_T)integ_Accel_and_Gyro_P.Gain_Gain * |
371 | 7.4505806E-9F * (real32_T)integ_Accel_and_Gyro_B.acc[2]; |
372 | |
373 | /* Gain: '<S2>/Gain2' */ |
374 | integ_Accel_and_Gyro_B.Gain2[0] = (real32_T)integ_Accel_and_Gyro_P.Gain2_Gain * |
375 | 9.53674316E-7F * (real32_T)integ_Accel_and_Gyro_B.gyro[0]; |
376 | integ_Accel_and_Gyro_B.Gain2[1] = (real32_T)integ_Accel_and_Gyro_P.Gain2_Gain * |
377 | 9.53674316E-7F * (real32_T)integ_Accel_and_Gyro_B.gyro[1]; |
378 | integ_Accel_and_Gyro_B.Gain2[2] = (real32_T)integ_Accel_and_Gyro_P.Gain2_Gain * |
379 | 9.53674316E-7F * (real32_T)integ_Accel_and_Gyro_B.gyro[2]; |
380 | |
381 | /* Sum: '<S2>/Sum' incorporates: |
382 | * Constant: '<S2>/Constant' |
383 | * Gain: '<S2>/Gain1' |
384 | */ |
385 | integ_Accel_and_Gyro_B.Sum = (real32_T)integ_Accel_and_Gyro_P.Gain1_Gain * |
386 | 1.1920929E-7F * (real32_T)integ_Accel_and_Gyro_B.temp + |
387 | integ_Accel_and_Gyro_P.Constant_Value; |
388 | |
389 | /* DiscreteIntegrator: '<Root>/Discrete-Time Integrator' */ |
390 | integ_Accel_and_Gyro_B.DiscreteTimeIntegrator = |
391 | integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator_DSTATE; |
392 | |
393 | /* DiscreteIntegrator: '<Root>/Discrete-Time Integrator1' */ |
394 | integ_Accel_and_Gyro_B.DiscreteTimeIntegrator1 = |
395 | integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator1_DSTATE; |
396 | |
397 | /* Update for DiscreteIntegrator: '<Root>/Discrete-Time Integrator' */ |
398 | integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator_DSTATE += |
399 | integ_Accel_and_Gyro_P.DiscreteTimeIntegrator_gainval * |
400 | integ_Accel_and_Gyro_B.Gain2[0]; |
401 | |
402 | /* Update for DiscreteIntegrator: '<Root>/Discrete-Time Integrator1' */ |
403 | integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator1_DSTATE += |
404 | integ_Accel_and_Gyro_P.DiscreteTimeIntegrator1_gainval * |
405 | integ_Accel_and_Gyro_B.Gain2[1]; |
406 | |
407 | /* External mode */ |
408 | rtExtModeUploadCheckTrigger(1); |
409 | |
410 | { /* Sample time: [0.05s, 0.0s] */ |
411 | rtExtModeUpload(0, integ_Accel_and_Gyro_M->Timing.taskTime0); |
412 | } |
413 | |
414 | /* signal main to stop simulation */ |
415 | { /* Sample time: [0.05s, 0.0s] */ |
416 | if ((rtmGetTFinal(integ_Accel_and_Gyro_M)!=-1) && |
417 | !((rtmGetTFinal(integ_Accel_and_Gyro_M)- |
418 | integ_Accel_and_Gyro_M->Timing.taskTime0) > |
419 | integ_Accel_and_Gyro_M->Timing.taskTime0 * (DBL_EPSILON))) { |
420 | rtmSetErrorStatus(integ_Accel_and_Gyro_M, "Simulation finished"); |
421 | } |
422 | |
423 | if (rtmGetStopRequested(integ_Accel_and_Gyro_M)) { |
424 | rtmSetErrorStatus(integ_Accel_and_Gyro_M, "Simulation finished"); |
425 | } |
426 | } |
427 | |
428 | /* Update absolute time for base rate */ |
429 | /* The "clockTick0" counts the number of times the code of this task has |
430 | * been executed. The absolute time is the multiplication of "clockTick0" |
431 | * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not |
432 | * overflow during the application lifespan selected. |
433 | */ |
434 | integ_Accel_and_Gyro_M->Timing.taskTime0 = |
435 | (++integ_Accel_and_Gyro_M->Timing.clockTick0) * |
436 | integ_Accel_and_Gyro_M->Timing.stepSize0; |
437 | } |
438 | |
439 | /* Model initialize function */ |
440 | void integ_Accel_and_Gyro_initialize(void) |
441 | { |
442 | /* Registration code */ |
443 | |
444 | /* initialize real-time model */ |
445 | (void) memset((void *)integ_Accel_and_Gyro_M, 0, |
446 | sizeof(RT_MODEL_integ_Accel_and_Gyro_T)); |
447 | rtmSetTFinal(integ_Accel_and_Gyro_M, -1); |
448 | integ_Accel_and_Gyro_M->Timing.stepSize0 = 0.05; |
449 | |
450 | /* External mode info */ |
451 | integ_Accel_and_Gyro_M->Sizes.checksums[0] = (2664078780U); |
452 | integ_Accel_and_Gyro_M->Sizes.checksums[1] = (1100192460U); |
453 | integ_Accel_and_Gyro_M->Sizes.checksums[2] = (2813303645U); |
454 | integ_Accel_and_Gyro_M->Sizes.checksums[3] = (1502856627U); |
455 | |
456 | { |
457 | static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE; |
458 | static RTWExtModeInfo rt_ExtModeInfo; |
459 | static const sysRanDType *systemRan[7]; |
460 | integ_Accel_and_Gyro_M->extModeInfo = (&rt_ExtModeInfo); |
461 | rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan); |
462 | systemRan[0] = &rtAlwaysEnabled; |
463 | systemRan[1] = &rtAlwaysEnabled; |
464 | systemRan[2] = (sysRanDType *)&integ_Accel_and_Gyro_DW.i2cRd_SubsysRanBC; |
465 | systemRan[3] = (sysRanDType *)&integ_Accel_and_Gyro_DW.i2cRd_SubsysRanBC; |
466 | systemRan[4] = (sysRanDType *) |
467 | &integ_Accel_and_Gyro_DW.i2cWr.i2cWr_SubsysRanBC; |
468 | systemRan[5] = (sysRanDType *) |
469 | &integ_Accel_and_Gyro_DW.i2cWr.i2cWr_SubsysRanBC; |
470 | systemRan[6] = &rtAlwaysEnabled; |
471 | rteiSetModelMappingInfoPtr(integ_Accel_and_Gyro_M->extModeInfo, |
472 | &integ_Accel_and_Gyro_M->SpecialInfo.mappingInfo); |
473 | rteiSetChecksumsPtr(integ_Accel_and_Gyro_M->extModeInfo, |
474 | integ_Accel_and_Gyro_M->Sizes.checksums); |
475 | rteiSetTPtr(integ_Accel_and_Gyro_M->extModeInfo, rtmGetTPtr |
476 | (integ_Accel_and_Gyro_M)); |
477 | } |
478 | |
479 | /* block I/O */ |
480 | (void) memset(((void *) &integ_Accel_and_Gyro_B), 0, |
481 | sizeof(B_integ_Accel_and_Gyro_T)); |
482 | |
483 | /* states (dwork) */ |
484 | (void) memset((void *)&integ_Accel_and_Gyro_DW, 0, |
485 | sizeof(DW_integ_Accel_and_Gyro_T)); |
486 | |
487 | /* data type transition information */ |
488 | { |
489 | static DataTypeTransInfo dtInfo; |
490 | (void) memset((char_T *) &dtInfo, 0, |
491 | sizeof(dtInfo)); |
492 | integ_Accel_and_Gyro_M->SpecialInfo.mappingInfo = (&dtInfo); |
493 | dtInfo.numDataTypes = 20; |
494 | dtInfo.dataTypeSizes = &rtDataTypeSizes[0]; |
495 | dtInfo.dataTypeNames = &rtDataTypeNames[0]; |
496 | |
497 | /* Block I/O transition table */ |
498 | dtInfo.BTransTable = &rtBTransTable; |
499 | |
500 | /* Parameters transition table */ |
501 | dtInfo.PTransTable = &rtPTransTable; |
502 | } |
503 | |
504 | { |
505 | freedomk64f_fxos8700_integ_Ac_T *obj; |
506 | uint32_T i2cname; |
507 | uint8_T output; |
508 | uint8_T status; |
509 | uint8_T SwappedDataBytes[2]; |
510 | int32_T i; |
511 | |
512 | /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */ |
513 | obj = &integ_Accel_and_Gyro_DW.obj; |
514 | integ_Accel_and_Gyro_DW.obj.isInitialized = 0; |
515 | obj->i2cobj.isInitialized = 0; |
516 | |
517 | /* [EOF] */ |
518 | /* */ |
519 | integ_Accel_and_Gyro_DW.obj.SampleTime = |
520 | integ_Accel_and_Gyro_P.FXOS87006AxesSensor_SampleTime; |
521 | obj = &integ_Accel_and_Gyro_DW.obj; |
522 | integ_Accel_and_Gyro_DW.obj.isInitialized = 1; |
523 | i2cname = 0; |
524 | MW_I2C_Open(i2cname, 0U); |
525 | |
526 | /* KHz */ |
527 | obj->i2cobj.BusSpeed = 100000U; |
528 | i2cname = 0; |
529 | MW_I2C_SetBusSpeed(i2cname, obj->i2cobj.BusSpeed); |
530 | SwappedDataBytes[0] = 43U; |
531 | SwappedDataBytes[1] = 64U; |
532 | i2cname = 0; |
533 | MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false); |
534 | OSA_TimeDelay(500U); |
535 | output = 0U; |
536 | status = 42U; |
537 | i2cname = 0; |
538 | status = MW_I2C_MasterWrite(i2cname, 29U, &status, 1U, true, false); |
539 | if (0 == status) { |
540 | i2cname = 0; |
541 | MW_I2C_MasterRead(i2cname, 29U, &status, 1U, false, true); |
542 | memcpy((void *)&output, (void *)&status, (size_t)1 * sizeof(uint8_T)); |
543 | } |
544 | |
545 | SwappedDataBytes[0] = 42U; |
546 | SwappedDataBytes[1] = (uint8_T)(output & 254); |
547 | i2cname = 0; |
548 | MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false); |
549 | SwappedDataBytes[0] = 14U; |
550 | SwappedDataBytes[1] = 0U; |
551 | i2cname = 0; |
552 | MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false); |
553 | SwappedDataBytes[0] = 91U; |
554 | SwappedDataBytes[1] = 0U; |
555 | i2cname = 0; |
556 | MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false); |
557 | SwappedDataBytes[0] = 42U; |
558 | SwappedDataBytes[1] = 1U; |
559 | i2cname = 0; |
560 | MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false); |
561 | |
562 | /* End of Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */ |
563 | /* Start for Chart: '<S2>/MPU6050' incorporates: |
564 | * Start for SubSystem: '<S14>/i2cWr' |
565 | */ |
566 | integ_Accel_and_Gyr_i2cWr_Start(&integ_Accel_and_Gyro_DW.i2cWr); |
567 | |
568 | /* Start for Chart: '<S2>/MPU6050' incorporates: |
569 | * Start for SubSystem: '<S14>/i2cRd' |
570 | */ |
571 | /* Start for MATLABSystem: '<S15>/I2C Master Read' */ |
572 | integ_Accel_and_Gyro_DW.obj_d.isInitialized = 0; |
573 | |
574 | /* [EOF] */ |
575 | /* */ |
576 | integ_Accel_and_Gyro_DW.obj_d.SampleTime = |
577 | integ_Accel_and_Gyro_P.I2CMasterRead_SampleTime; |
578 | integ_Accel_and_Gyro_DW.obj_d.isInitialized = 1; |
579 | i2cname = 0; |
580 | MW_I2C_Open(i2cname, 0U); |
581 | |
582 | /* 100KHz. */ |
583 | /* KHz */ |
584 | integ_Accel_and_Gyro_DW.obj_d.BusSpeed = 100000U; |
585 | i2cname = 0; |
586 | MW_I2C_SetBusSpeed(i2cname, integ_Accel_and_Gyro_DW.obj_d.BusSpeed); |
587 | |
588 | /* InitializeConditions for DiscreteIntegrator: '<Root>/Discrete-Time Integrator' */ |
589 | integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator_DSTATE = |
590 | integ_Accel_and_Gyro_P.DiscreteTimeIntegrator_IC; |
591 | |
592 | /* InitializeConditions for DiscreteIntegrator: '<Root>/Discrete-Time Integrator1' */ |
593 | integ_Accel_and_Gyro_DW.DiscreteTimeIntegrator1_DSTATE = |
594 | integ_Accel_and_Gyro_P.DiscreteTimeIntegrator1_IC; |
595 | integ_Accel_and_Gyro_DW.is_active_c3_integ_Accel_and_Gy = 0U; |
596 | integ_Accel_and_Gyro_DW.is_c3_integ_Accel_and_Gyro = |
597 | integ_Accel__IN_NO_ACTIVE_CHILD; |
598 | |
599 | /* SystemInitialize for Chart: '<S2>/MPU6050' incorporates: |
600 | * SystemInitialize for SubSystem: '<S14>/i2cRd' |
601 | */ |
602 | for (i = 0; i < 7; i++) { |
603 | /* SystemInitialize for Outport: '<S15>/datar' */ |
604 | integ_Accel_and_Gyro_B.DataTypeConversion[i] = |
605 | integ_Accel_and_Gyro_P.datar_Y0; |
606 | } |
607 | } |
608 | } |
609 | |
610 | /* Model terminate function */ |
611 | void integ_Accel_and_Gyro_terminate(void) |
612 | { |
613 | uint32_T i2cname; |
614 | |
615 | /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates: |
616 | * Terminate for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' |
617 | */ |
618 | if (integ_Accel_and_Gyro_DW.obj.isInitialized == 1) { |
619 | integ_Accel_and_Gyro_DW.obj.isInitialized = 2; |
620 | i2cname = 0; |
621 | MW_I2C_Close(i2cname); |
622 | } |
623 | |
624 | /* End of Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */ |
625 | |
626 | /* Terminate for Chart: '<S2>/MPU6050' incorporates: |
627 | * Terminate for SubSystem: '<S14>/i2cWr' |
628 | */ |
629 | integ_Accel_and_Gyro_i2cWr_Term(&integ_Accel_and_Gyro_DW.i2cWr); |
630 | |
631 | /* Terminate for Chart: '<S2>/MPU6050' incorporates: |
632 | * Terminate for SubSystem: '<S14>/i2cRd' |
633 | */ |
634 | /* Start for MATLABSystem: '<S15>/I2C Master Read' incorporates: |
635 | * Terminate for MATLABSystem: '<S15>/I2C Master Read' |
636 | */ |
637 | if (integ_Accel_and_Gyro_DW.obj_d.isInitialized == 1) { |
638 | integ_Accel_and_Gyro_DW.obj_d.isInitialized = 2; |
639 | i2cname = 0; |
640 | MW_I2C_Close(i2cname); |
641 | } |
642 | |
643 | /* End of Start for MATLABSystem: '<S15>/I2C Master Read' */ |
644 | } |
645 | |
646 | /* |
647 | * File trailer for generated code. |
648 | * |
649 | * [EOF] |
650 | */ |
651 | |