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.19
11 * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017
12 * C/C++ source code generated on : Tue Nov 07 14:03:19 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) */
25B_magneto_tarato_T magneto_tarato_B;
26
27/* Block states (auto storage) */
28DW_magneto_tarato_T magneto_tarato_DW;
29
30/* Real-time model */
31RT_MODEL_magneto_tarato_T magneto_tarato_M_;
32RT_MODEL_magneto_tarato_T *const magneto_tarato_M = &magneto_tarato_M_;
33real_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 */
70void 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 */
248void 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 */
370void 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