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: magneto_tarato.c |
7 | * |
8 | * Code generated for Simulink model 'magneto_tarato'. |
9 | * |
10 | * Model version : 1.20 |
11 | * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 |
12 | * C/C++ source code generated on : Mon Nov 13 17:07:50 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 "magneto_tarato.h" |
21 | #include "magneto_tarato_private.h" |
22 | #include "magneto_tarato_dt.h" |
23 | |
24 | /* Block signals (auto storage) */ |
25 | B_magneto_tarato_T magneto_tarato_B; |
26 | |
27 | /* Block states (auto storage) */ |
28 | DW_magneto_tarato_T magneto_tarato_DW; |
29 | |
30 | /* Real-time model */ |
31 | RT_MODEL_magneto_tarato_T magneto_tarato_M_; |
32 | RT_MODEL_magneto_tarato_T *const magneto_tarato_M = &magneto_tarato_M_; |
33 | real_T rt_atan2d_snf(real_T u0, real_T u1) |
34 | { |
35 | real_T y; |
36 | int32_T u0_0; |
37 | int32_T u1_0; |
38 | if (rtIsNaN(u0) || rtIsNaN(u1)) { |
39 | y = (rtNaN); |
40 | } else if (rtIsInf(u0) && rtIsInf(u1)) { |
41 | if (u0 > 0.0) { |
42 | u0_0 = 1; |
43 | } else { |
44 | u0_0 = -1; |
45 | } |
46 | |
47 | if (u1 > 0.0) { |
48 | u1_0 = 1; |
49 | } else { |
50 | u1_0 = -1; |
51 | } |
52 | |
53 | y = atan2(u0_0, u1_0); |
54 | } else if (u1 == 0.0) { |
55 | if (u0 > 0.0) { |
56 | y = RT_PI / 2.0; |
57 | } else if (u0 < 0.0) { |
58 | y = -(RT_PI / 2.0); |
59 | } else { |
60 | y = 0.0; |
61 | } |
62 | } else { |
63 | y = atan2(u0, u1); |
64 | } |
65 | |
66 | return y; |
67 | } |
68 | |
69 | /* Model step function */ |
70 | void magneto_tarato_step(void) |
71 | { |
72 | boolean_T p; |
73 | boolean_T p_0; |
74 | int16_T output[3]; |
75 | uint8_T status; |
76 | uint8_T output_raw[6]; |
77 | uint8_T y[2]; |
78 | uint8_T x[2]; |
79 | int32_T i; |
80 | int16_T output_0; |
81 | real_T tmp; |
82 | real_T tmp_0; |
83 | real_T tmp_1; |
84 | |
85 | /* Constant: '<Root>/Constant' */ |
86 | magneto_tarato_B.VectorConcatenate[0] = magneto_tarato_P.Constant_Value; |
87 | |
88 | /* Constant: '<Root>/Constant6' */ |
89 | magneto_tarato_B.VectorConcatenate[1] = magneto_tarato_P.Constant6_Value; |
90 | |
91 | /* Constant: '<Root>/Constant5' */ |
92 | magneto_tarato_B.VectorConcatenate[2] = magneto_tarato_P.Constant5_Value; |
93 | |
94 | /* Constant: '<Root>/Constant2' */ |
95 | magneto_tarato_B.VectorConcatenate[3] = magneto_tarato_P.Constant2_Value; |
96 | |
97 | /* Product: '<Root>/Divide' incorporates: |
98 | * Constant: '<Root>/Constant8' |
99 | * Constant: '<Root>/Constant9' |
100 | */ |
101 | magneto_tarato_B.VectorConcatenate[4] = magneto_tarato_P.Constant8_Value / |
102 | magneto_tarato_P.Constant9_Value; |
103 | |
104 | /* Constant: '<Root>/Constant4' */ |
105 | magneto_tarato_B.VectorConcatenate[5] = magneto_tarato_P.Constant4_Value; |
106 | |
107 | /* Constant: '<Root>/Constant3' */ |
108 | magneto_tarato_B.VectorConcatenate[6] = magneto_tarato_P.Constant3_Value; |
109 | |
110 | /* Constant: '<Root>/Constant7' */ |
111 | magneto_tarato_B.VectorConcatenate[7] = magneto_tarato_P.Constant7_Value; |
112 | |
113 | /* Constant: '<Root>/Constant1' */ |
114 | magneto_tarato_B.VectorConcatenate[8] = magneto_tarato_P.Constant1_Value; |
115 | |
116 | /* Reshape: '<S1>/Reshape (9) to [3x3] column-major' */ |
117 | memcpy(&magneto_tarato_B.Reshape9to3x3columnmajor[0], |
118 | &magneto_tarato_B.VectorConcatenate[0], 9U * sizeof(real_T)); |
119 | |
120 | /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates: |
121 | * MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' |
122 | */ |
123 | p = false; |
124 | p_0 = true; |
125 | if (!(magneto_tarato_DW.obj.SampleTime == |
126 | magneto_tarato_P.FXOS87006AxesSensor_SampleTime)) { |
127 | p_0 = false; |
128 | } |
129 | |
130 | if (p_0) { |
131 | p = true; |
132 | } |
133 | |
134 | if (!p) { |
135 | magneto_tarato_DW.obj.SampleTime = |
136 | magneto_tarato_P.FXOS87006AxesSensor_SampleTime; |
137 | } |
138 | |
139 | output[0] = 0; |
140 | output[1] = 0; |
141 | output[2] = 0; |
142 | status = 51U; |
143 | magneto_tarato_B.i2cname = 0; |
144 | status = MW_I2C_MasterWrite(magneto_tarato_B.i2cname, 29U, &status, 1U, true, |
145 | false); |
146 | if (0 == status) { |
147 | magneto_tarato_B.i2cname = 0; |
148 | MW_I2C_MasterRead(magneto_tarato_B.i2cname, 29U, output_raw, 6U, false, true); |
149 | memcpy((void *)&output[0], (void *)&output_raw[0], (size_t)3 * sizeof |
150 | (int16_T)); |
151 | output_0 = output[0]; |
152 | memcpy((void *)&y[0], (void *)&output_0, (size_t)2 * sizeof(uint8_T)); |
153 | x[0] = y[1]; |
154 | x[1] = y[0]; |
155 | memcpy((void *)&output_0, (void *)&x[0], (size_t)1 * sizeof(int16_T)); |
156 | output[0] = output_0; |
157 | output_0 = output[1]; |
158 | memcpy((void *)&y[0], (void *)&output_0, (size_t)2 * sizeof(uint8_T)); |
159 | x[0] = y[1]; |
160 | x[1] = y[0]; |
161 | memcpy((void *)&output_0, (void *)&x[0], (size_t)1 * sizeof(int16_T)); |
162 | output[1] = output_0; |
163 | output_0 = output[2]; |
164 | memcpy((void *)&y[0], (void *)&output_0, (size_t)2 * sizeof(uint8_T)); |
165 | x[0] = y[1]; |
166 | x[1] = y[0]; |
167 | memcpy((void *)&output_0, (void *)&x[0], (size_t)1 * sizeof(int16_T)); |
168 | output[2] = output_0; |
169 | } |
170 | |
171 | /* MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates: |
172 | * Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' |
173 | */ |
174 | magneto_tarato_B.FXOS87006AxesSensor[0] = (real_T)output[0] * 0.1; |
175 | magneto_tarato_B.FXOS87006AxesSensor[1] = (real_T)output[1] * 0.1; |
176 | magneto_tarato_B.FXOS87006AxesSensor[2] = (real_T)output[2] * 0.1; |
177 | |
178 | /* MATLAB Function: '<Root>/MATLAB Function' incorporates: |
179 | * Bias: '<Root>/Bias' |
180 | * Bias: '<Root>/Bias1' |
181 | * Bias: '<Root>/Bias2' |
182 | */ |
183 | /* MATLAB Function 'MATLAB Function': '<S2>:1' */ |
184 | /* '<S2>:1:2' Raw_B=[RawX_B,RawY_B,RawZ_B]; */ |
185 | /* '<S2>:1:4' CorrMag = Raw_B*S; */ |
186 | tmp = magneto_tarato_B.FXOS87006AxesSensor[0] + magneto_tarato_P.Bias_Bias; |
187 | tmp_0 = magneto_tarato_B.FXOS87006AxesSensor[1] + magneto_tarato_P.Bias1_Bias; |
188 | tmp_1 = magneto_tarato_B.FXOS87006AxesSensor[2] + magneto_tarato_P.Bias2_Bias; |
189 | for (i = 0; i < 3; i++) { |
190 | magneto_tarato_B.CorrMag[i] = 0.0; |
191 | magneto_tarato_B.CorrMag[i] += magneto_tarato_B.Reshape9to3x3columnmajor[3 * |
192 | i] * tmp; |
193 | magneto_tarato_B.CorrMag[i] += magneto_tarato_B.Reshape9to3x3columnmajor[3 * |
194 | i + 1] * tmp_0; |
195 | magneto_tarato_B.CorrMag[i] += magneto_tarato_B.Reshape9to3x3columnmajor[3 * |
196 | i + 2] * tmp_1; |
197 | } |
198 | |
199 | /* End of MATLAB Function: '<Root>/MATLAB Function' */ |
200 | |
201 | /* Gain: '<S3>/Gain' incorporates: |
202 | * Gain: '<Root>/Gain' |
203 | * Trigonometry: '<Root>/Atan2' |
204 | */ |
205 | magneto_tarato_B.Gain = rt_atan2d_snf(magneto_tarato_P.Gain_Gain * |
206 | magneto_tarato_B.CorrMag[1], magneto_tarato_B.CorrMag[0]) * |
207 | magneto_tarato_P.Gain_Gain_j; |
208 | |
209 | /* Gain: '<S4>/Gain' incorporates: |
210 | * Gain: '<Root>/Gain1' |
211 | * Trigonometry: '<Root>/Atan1' |
212 | */ |
213 | magneto_tarato_B.Gain_g = rt_atan2d_snf(magneto_tarato_P.Gain1_Gain * |
214 | magneto_tarato_B.FXOS87006AxesSensor[1], |
215 | magneto_tarato_B.FXOS87006AxesSensor[0]) * magneto_tarato_P.Gain_Gain_h; |
216 | |
217 | /* External mode */ |
218 | rtExtModeUploadCheckTrigger(1); |
219 | |
220 | { /* Sample time: [0.02s, 0.0s] */ |
221 | rtExtModeUpload(0, magneto_tarato_M->Timing.taskTime0); |
222 | } |
223 | |
224 | /* signal main to stop simulation */ |
225 | { /* Sample time: [0.02s, 0.0s] */ |
226 | if ((rtmGetTFinal(magneto_tarato_M)!=-1) && |
227 | !((rtmGetTFinal(magneto_tarato_M)-magneto_tarato_M->Timing.taskTime0) > |
228 | magneto_tarato_M->Timing.taskTime0 * (DBL_EPSILON))) { |
229 | rtmSetErrorStatus(magneto_tarato_M, "Simulation finished"); |
230 | } |
231 | |
232 | if (rtmGetStopRequested(magneto_tarato_M)) { |
233 | rtmSetErrorStatus(magneto_tarato_M, "Simulation finished"); |
234 | } |
235 | } |
236 | |
237 | /* Update absolute time for base rate */ |
238 | /* The "clockTick0" counts the number of times the code of this task has |
239 | * been executed. The absolute time is the multiplication of "clockTick0" |
240 | * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not |
241 | * overflow during the application lifespan selected. |
242 | */ |
243 | magneto_tarato_M->Timing.taskTime0 = |
244 | (++magneto_tarato_M->Timing.clockTick0) * magneto_tarato_M->Timing.stepSize0; |
245 | } |
246 | |
247 | /* Model initialize function */ |
248 | void magneto_tarato_initialize(void) |
249 | { |
250 | /* Registration code */ |
251 | |
252 | /* initialize non-finites */ |
253 | rt_InitInfAndNaN(sizeof(real_T)); |
254 | |
255 | /* initialize real-time model */ |
256 | (void) memset((void *)magneto_tarato_M, 0, |
257 | sizeof(RT_MODEL_magneto_tarato_T)); |
258 | rtmSetTFinal(magneto_tarato_M, -1); |
259 | magneto_tarato_M->Timing.stepSize0 = 0.02; |
260 | |
261 | /* External mode info */ |
262 | magneto_tarato_M->Sizes.checksums[0] = (359932345U); |
263 | magneto_tarato_M->Sizes.checksums[1] = (1492314972U); |
264 | magneto_tarato_M->Sizes.checksums[2] = (3572482939U); |
265 | magneto_tarato_M->Sizes.checksums[3] = (30296138U); |
266 | |
267 | { |
268 | static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE; |
269 | static RTWExtModeInfo rt_ExtModeInfo; |
270 | static const sysRanDType *systemRan[3]; |
271 | magneto_tarato_M->extModeInfo = (&rt_ExtModeInfo); |
272 | rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan); |
273 | systemRan[0] = &rtAlwaysEnabled; |
274 | systemRan[1] = &rtAlwaysEnabled; |
275 | systemRan[2] = &rtAlwaysEnabled; |
276 | rteiSetModelMappingInfoPtr(magneto_tarato_M->extModeInfo, |
277 | &magneto_tarato_M->SpecialInfo.mappingInfo); |
278 | rteiSetChecksumsPtr(magneto_tarato_M->extModeInfo, |
279 | magneto_tarato_M->Sizes.checksums); |
280 | rteiSetTPtr(magneto_tarato_M->extModeInfo, rtmGetTPtr(magneto_tarato_M)); |
281 | } |
282 | |
283 | /* block I/O */ |
284 | (void) memset(((void *) &magneto_tarato_B), 0, |
285 | sizeof(B_magneto_tarato_T)); |
286 | |
287 | /* states (dwork) */ |
288 | (void) memset((void *)&magneto_tarato_DW, 0, |
289 | sizeof(DW_magneto_tarato_T)); |
290 | |
291 | /* data type transition information */ |
292 | { |
293 | static DataTypeTransInfo dtInfo; |
294 | (void) memset((char_T *) &dtInfo, 0, |
295 | sizeof(dtInfo)); |
296 | magneto_tarato_M->SpecialInfo.mappingInfo = (&dtInfo); |
297 | dtInfo.numDataTypes = 15; |
298 | dtInfo.dataTypeSizes = &rtDataTypeSizes[0]; |
299 | dtInfo.dataTypeNames = &rtDataTypeNames[0]; |
300 | |
301 | /* Block I/O transition table */ |
302 | dtInfo.BTransTable = &rtBTransTable; |
303 | |
304 | /* Parameters transition table */ |
305 | dtInfo.PTransTable = &rtPTransTable; |
306 | } |
307 | |
308 | { |
309 | freedomk64f_fxos8700_magneto__T *obj; |
310 | uint32_T i2cname; |
311 | uint8_T output; |
312 | uint8_T status; |
313 | uint8_T SwappedDataBytes[2]; |
314 | |
315 | /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */ |
316 | obj = &magneto_tarato_DW.obj; |
317 | magneto_tarato_DW.obj.isInitialized = 0; |
318 | obj->i2cobj.isInitialized = 0; |
319 | |
320 | /* [EOF] */ |
321 | /* */ |
322 | magneto_tarato_DW.obj.SampleTime = |
323 | magneto_tarato_P.FXOS87006AxesSensor_SampleTime; |
324 | obj = &magneto_tarato_DW.obj; |
325 | magneto_tarato_DW.obj.isInitialized = 1; |
326 | i2cname = 0; |
327 | MW_I2C_Open(i2cname, 0U); |
328 | |
329 | /* KHz */ |
330 | obj->i2cobj.BusSpeed = 100000U; |
331 | i2cname = 0; |
332 | MW_I2C_SetBusSpeed(i2cname, obj->i2cobj.BusSpeed); |
333 | SwappedDataBytes[0] = 43U; |
334 | SwappedDataBytes[1] = 64U; |
335 | i2cname = 0; |
336 | MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false); |
337 | OSA_TimeDelay(500U); |
338 | output = 0U; |
339 | status = 42U; |
340 | i2cname = 0; |
341 | status = MW_I2C_MasterWrite(i2cname, 29U, &status, 1U, true, false); |
342 | if (0 == status) { |
343 | i2cname = 0; |
344 | MW_I2C_MasterRead(i2cname, 29U, &status, 1U, false, true); |
345 | memcpy((void *)&output, (void *)&status, (size_t)1 * sizeof(uint8_T)); |
346 | } |
347 | |
348 | SwappedDataBytes[0] = 42U; |
349 | SwappedDataBytes[1] = (uint8_T)(output & 254); |
350 | i2cname = 0; |
351 | MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false); |
352 | SwappedDataBytes[0] = 14U; |
353 | SwappedDataBytes[1] = 1U; |
354 | i2cname = 0; |
355 | MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false); |
356 | SwappedDataBytes[0] = 91U; |
357 | SwappedDataBytes[1] = 1U; |
358 | i2cname = 0; |
359 | MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false); |
360 | SwappedDataBytes[0] = 42U; |
361 | SwappedDataBytes[1] = 9U; |
362 | i2cname = 0; |
363 | MW_I2C_MasterWrite(i2cname, 29U, SwappedDataBytes, 2U, false, false); |
364 | |
365 | /* End of Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */ |
366 | } |
367 | } |
368 | |
369 | /* Model terminate function */ |
370 | void magneto_tarato_terminate(void) |
371 | { |
372 | uint32_T i2cname; |
373 | |
374 | /* Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' incorporates: |
375 | * Terminate for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' |
376 | */ |
377 | if (magneto_tarato_DW.obj.isInitialized == 1) { |
378 | magneto_tarato_DW.obj.isInitialized = 2; |
379 | i2cname = 0; |
380 | MW_I2C_Close(i2cname); |
381 | } |
382 | |
383 | /* End of Start for MATLABSystem: '<Root>/FXOS8700 6-Axes Sensor' */ |
384 | } |
385 | |
386 | /* |
387 | * File trailer for generated code. |
388 | * |
389 | * [EOF] |
390 | */ |
391 | |