1/*
2 * VisionTest.c
3 *
4 * Classroom License -- for classroom instructional use only. Not for
5 * government, commercial, academic research, or other organizational use.
6 *
7 * Code generation for model "VisionTest".
8 *
9 * Model version : 1.4
10 * Simulink Coder version : 8.11 (R2016b) 25-Aug-2016
11 * C source code generated on : Thu Dec 01 17:32:23 2016
12 *
13 * Target selection: ert.tlc
14 * Note: GRT includes extra infrastructure and instrumentation for prototyping
15 * Embedded hardware selection: ARM Compatible->ARM Cortex
16 * Code generation objectives: Unspecified
17 * Validation result: Not run
18 */
19
20#include "VisionTest.h"
21#include "VisionTest_private.h"
22#include "VisionTest_dt.h"
23
24/* Block signals (auto storage) */
25B_VisionTest_T VisionTest_B;
26
27/* Block states (auto storage) */
28DW_VisionTest_T VisionTest_DW;
29
30/* Real-time model */
31RT_MODEL_VisionTest_T VisionTest_M_;
32RT_MODEL_VisionTest_T *const VisionTest_M = &VisionTest_M_;
33
34/* Model step function */
35void VisionTest_step(void)
36{
37 real_T out_idx_0;
38 real_T out_idx_1;
39 real_T rtb_Delay_idx_0;
40 real_T rtb_Delay_idx_1;
41
42 /* S-Function (sfcn_getFoilCheck): '<Root>/MagMan Foil with error' */
43 sfcn_getFoilCheck_Outputs_wrapper( &VisionTest_B.MagManFoilwitherror_o1,
44 &VisionTest_B.MagManFoilwitherror_o2, &VisionTest_B.MagManFoilwitherror_o3,
45 &VisionTest_B.MagManFoilwitherror_o4,
46 &VisionTest_DW.MagManFoilwitherror_DSTATE);
47
48 /* S-Function (sfcn_posread_shm): '<S1>/Read position' */
49 sfcn_posread_shm_Outputs_wrapper( &VisionTest_B.Readposition_o1,
50 &VisionTest_B.Readposition_o2, &VisionTest_DW.Readposition_DSTATE);
51
52 /* MATLAB Function: '<Root>/Position transformation' incorporates:
53 * Constant: '<Root>/Raspicam Calibration matrix'
54 * DataTypeConversion: '<S1>/Data Type Conversion1'
55 * DataTypeConversion: '<S1>/Data Type Conversion2'
56 */
57 /* MATLAB Function 'Position transformation': '<S2>:1' */
58 /* out=km2*[x;y;x^2;y^2;x*y;1]; */
59 /* '<S2>:1:4' xa = [x,y]; */
60 /* '<S2>:1:5' H = km4(:); */
61 /* '<S2>:1:6' xa = [xa,ones(size(xa,1),1)]; */
62 /* homogeneous coordinates */
63 /* '<S2>:1:7' out = [ (xa*H(1:3))./(xa*H(7:9)), (xa*H(4:6))./(xa*H(7:9)) ]; */
64 /* '<S2>:1:9' X=out(1); */
65 VisionTest_B.X = (((real_T)VisionTest_B.Readposition_o1 *
66 VisionTest_P.km4piCam[0] + (real_T)
67 VisionTest_B.Readposition_o2 * VisionTest_P.km4piCam[1]) +
68 VisionTest_P.km4piCam[2]) / (((real_T)
69 VisionTest_B.Readposition_o1 * VisionTest_P.km4piCam[6] + (real_T)
70 VisionTest_B.Readposition_o2 * VisionTest_P.km4piCam[7]) +
71 VisionTest_P.km4piCam[8]);
72
73 /* '<S2>:1:10' Y=out(2); */
74 VisionTest_B.Y = (((real_T)VisionTest_B.Readposition_o1 *
75 VisionTest_P.km4piCam[3] + (real_T)
76 VisionTest_B.Readposition_o2 * VisionTest_P.km4piCam[4]) +
77 VisionTest_P.km4piCam[5]) / (((real_T)
78 VisionTest_B.Readposition_o1 * VisionTest_P.km4piCam[6] + (real_T)
79 VisionTest_B.Readposition_o2 * VisionTest_P.km4piCam[7]) +
80 VisionTest_P.km4piCam[8]);
81
82 /* MATLAB Function: '<Root>/Position transformation1' incorporates:
83 * Constant: '<Root>/Sentinel Calibration matrix'
84 */
85 /* MATLAB Function 'Position transformation1': '<S3>:1' */
86 /* out=km2*[x;y;x^2;y^2;x*y;1]; */
87 /* '<S3>:1:4' xa = [x,y]; */
88 /* '<S3>:1:5' H = km4(:); */
89 /* '<S3>:1:6' xa = [xa,ones(size(xa,1),1)]; */
90 /* homogeneous coordinates */
91 /* '<S3>:1:7' out = [ (xa*H(1:3))./(xa*H(7:9)), (xa*H(4:6))./(xa*H(7:9)) ]; */
92 out_idx_0 = ((VisionTest_B.MagManFoilwitherror_o1 * VisionTest_P.km4pi[0] +
93 VisionTest_B.MagManFoilwitherror_o2 * VisionTest_P.km4pi[1]) +
94 VisionTest_P.km4pi[2]) / ((VisionTest_B.MagManFoilwitherror_o1 *
95 VisionTest_P.km4pi[6] + VisionTest_B.MagManFoilwitherror_o2 *
96 VisionTest_P.km4pi[7]) + VisionTest_P.km4pi[8]);
97 out_idx_1 = ((VisionTest_B.MagManFoilwitherror_o1 * VisionTest_P.km4pi[3] +
98 VisionTest_B.MagManFoilwitherror_o2 * VisionTest_P.km4pi[4]) +
99 VisionTest_P.km4pi[5]) / ((VisionTest_B.MagManFoilwitherror_o1 *
100 VisionTest_P.km4pi[6] + VisionTest_B.MagManFoilwitherror_o2 *
101 VisionTest_P.km4pi[7]) + VisionTest_P.km4pi[8]);
102
103 /* Delay: '<Root>/Delay' */
104 /* '<S3>:1:9' X=out(1); */
105 /* '<S3>:1:10' Y=out(2); */
106 rtb_Delay_idx_0 = VisionTest_DW.Delay_DSTATE[0];
107 rtb_Delay_idx_1 = VisionTest_DW.Delay_DSTATE[1];
108
109 /* ManualSwitch: '<Root>/Manual Switch1' incorporates:
110 * Delay: '<Root>/Delay1'
111 */
112 if (VisionTest_P.ManualSwitch1_CurrentSetting == 1) {
113 /* ManualSwitch: '<Root>/Manual Switch' incorporates:
114 * Delay: '<Root>/Delay'
115 * MATLAB Function: '<Root>/Position transformation1'
116 */
117 if (VisionTest_P.ManualSwitch_CurrentSetting == 1) {
118 VisionTest_B.ManualSwitch1[0] = out_idx_0;
119 VisionTest_B.ManualSwitch1[1] = out_idx_1;
120 } else {
121 VisionTest_B.ManualSwitch1[0] = VisionTest_DW.Delay_DSTATE[0];
122 VisionTest_B.ManualSwitch1[1] = VisionTest_DW.Delay_DSTATE[1];
123 }
124
125 /* End of ManualSwitch: '<Root>/Manual Switch' */
126 } else {
127 VisionTest_B.ManualSwitch1[0] = VisionTest_DW.Delay1_DSTATE[0];
128 VisionTest_B.ManualSwitch1[1] = VisionTest_DW.Delay1_DSTATE[1];
129 }
130
131 /* End of ManualSwitch: '<Root>/Manual Switch1' */
132
133 /* Sum: '<Root>/Subtract' */
134 VisionTest_B.Subtract[0] = VisionTest_B.X - VisionTest_B.ManualSwitch1[0];
135 VisionTest_B.Subtract[1] = VisionTest_B.Y - VisionTest_B.ManualSwitch1[1];
136
137 /* S-Function "sfcn_getFoilCheck_wrapper" Block: <Root>/MagMan Foil with error */
138 sfcn_getFoilCheck_Update_wrapper( &VisionTest_B.MagManFoilwitherror_o1,
139 &VisionTest_B.MagManFoilwitherror_o2, &VisionTest_B.MagManFoilwitherror_o3,
140 &VisionTest_B.MagManFoilwitherror_o4,
141 &VisionTest_DW.MagManFoilwitherror_DSTATE);
142
143 /* S-Function "sfcn_posread_shm_wrapper" Block: <S1>/Read position */
144 sfcn_posread_shm_Update_wrapper( &VisionTest_B.Readposition_o1,
145 &VisionTest_B.Readposition_o2, &VisionTest_DW.Readposition_DSTATE);
146
147 /* Update for Delay: '<Root>/Delay' incorporates:
148 * MATLAB Function: '<Root>/Position transformation1'
149 */
150 VisionTest_DW.Delay_DSTATE[0] = out_idx_0;
151
152 /* Update for Delay: '<Root>/Delay1' */
153 VisionTest_DW.Delay1_DSTATE[0] = rtb_Delay_idx_0;
154
155 /* Update for Delay: '<Root>/Delay' incorporates:
156 * MATLAB Function: '<Root>/Position transformation1'
157 */
158 VisionTest_DW.Delay_DSTATE[1] = out_idx_1;
159
160 /* Update for Delay: '<Root>/Delay1' */
161 VisionTest_DW.Delay1_DSTATE[1] = rtb_Delay_idx_1;
162
163 /* External mode */
164 rtExtModeUploadCheckTrigger(1);
165
166 { /* Sample time: [0.02s, 0.0s] */
167 rtExtModeUpload(0, VisionTest_M->Timing.taskTime0);
168 }
169
170 /* signal main to stop simulation */
171 { /* Sample time: [0.02s, 0.0s] */
172 if ((rtmGetTFinal(VisionTest_M)!=-1) &&
173 !((rtmGetTFinal(VisionTest_M)-VisionTest_M->Timing.taskTime0) >
174 VisionTest_M->Timing.taskTime0 * (DBL_EPSILON))) {
175 rtmSetErrorStatus(VisionTest_M, "Simulation finished");
176 }
177
178 if (rtmGetStopRequested(VisionTest_M)) {
179 rtmSetErrorStatus(VisionTest_M, "Simulation finished");
180 }
181 }
182
183 /* Update absolute time for base rate */
184 /* The "clockTick0" counts the number of times the code of this task has
185 * been executed. The absolute time is the multiplication of "clockTick0"
186 * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
187 * overflow during the application lifespan selected.
188 * Timer of this task consists of two 32 bit unsigned integers.
189 * The two integers represent the low bits Timing.clockTick0 and the high bits
190 * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
191 */
192 if (!(++VisionTest_M->Timing.clockTick0)) {
193 ++VisionTest_M->Timing.clockTickH0;
194 }
195
196 VisionTest_M->Timing.taskTime0 = VisionTest_M->Timing.clockTick0 *
197 VisionTest_M->Timing.stepSize0 + VisionTest_M->Timing.clockTickH0 *
198 VisionTest_M->Timing.stepSize0 * 4294967296.0;
199}
200
201/* Model initialize function */
202void VisionTest_initialize(void)
203{
204 /* Registration code */
205
206 /* initialize real-time model */
207 (void) memset((void *)VisionTest_M, 0,
208 sizeof(RT_MODEL_VisionTest_T));
209 rtmSetTFinal(VisionTest_M, -1);
210 VisionTest_M->Timing.stepSize0 = 0.02;
211
212 /* External mode info */
213 VisionTest_M->Sizes.checksums[0] = (2761779759U);
214 VisionTest_M->Sizes.checksums[1] = (4156811726U);
215 VisionTest_M->Sizes.checksums[2] = (4141850224U);
216 VisionTest_M->Sizes.checksums[3] = (4278516765U);
217
218 {
219 static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE;
220 static RTWExtModeInfo rt_ExtModeInfo;
221 static const sysRanDType *systemRan[4];
222 VisionTest_M->extModeInfo = (&rt_ExtModeInfo);
223 rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan);
224 systemRan[0] = &rtAlwaysEnabled;
225 systemRan[1] = &rtAlwaysEnabled;
226 systemRan[2] = &rtAlwaysEnabled;
227 systemRan[3] = &rtAlwaysEnabled;
228 rteiSetModelMappingInfoPtr(VisionTest_M->extModeInfo,
229 &VisionTest_M->SpecialInfo.mappingInfo);
230 rteiSetChecksumsPtr(VisionTest_M->extModeInfo, VisionTest_M->Sizes.checksums);
231 rteiSetTPtr(VisionTest_M->extModeInfo, rtmGetTPtr(VisionTest_M));
232 }
233
234 /* block I/O */
235 (void) memset(((void *) &VisionTest_B), 0,
236 sizeof(B_VisionTest_T));
237
238 /* states (dwork) */
239 (void) memset((void *)&VisionTest_DW, 0,
240 sizeof(DW_VisionTest_T));
241
242 /* data type transition information */
243 {
244 static DataTypeTransInfo dtInfo;
245 (void) memset((char_T *) &dtInfo, 0,
246 sizeof(dtInfo));
247 VisionTest_M->SpecialInfo.mappingInfo = (&dtInfo);
248 dtInfo.numDataTypes = 14;
249 dtInfo.dataTypeSizes = &rtDataTypeSizes[0];
250 dtInfo.dataTypeNames = &rtDataTypeNames[0];
251
252 /* Block I/O transition table */
253 dtInfo.BTransTable = &rtBTransTable;
254
255 /* Parameters transition table */
256 dtInfo.PTransTable = &rtPTransTable;
257 }
258
259 /* S-Function Block: <Root>/MagMan Foil with error */
260 {
261 real_T initVector[1] = { 0 };
262
263 {
264 int_T i1;
265 for (i1=0; i1 < 1; i1++) {
266 VisionTest_DW.MagManFoilwitherror_DSTATE = initVector[0];
267 }
268 }
269 }
270
271 /* S-Function Block: <S1>/Read position */
272 {
273 real_T initVector[1] = { 0 };
274
275 {
276 int_T i1;
277 for (i1=0; i1 < 1; i1++) {
278 VisionTest_DW.Readposition_DSTATE = initVector[0];
279 }
280 }
281 }
282
283 /* InitializeConditions for Delay: '<Root>/Delay' */
284 VisionTest_DW.Delay_DSTATE[0] = VisionTest_P.Delay_InitialCondition;
285
286 /* InitializeConditions for Delay: '<Root>/Delay1' */
287 VisionTest_DW.Delay1_DSTATE[0] = VisionTest_P.Delay1_InitialCondition;
288
289 /* InitializeConditions for Delay: '<Root>/Delay' */
290 VisionTest_DW.Delay_DSTATE[1] = VisionTest_P.Delay_InitialCondition;
291
292 /* InitializeConditions for Delay: '<Root>/Delay1' */
293 VisionTest_DW.Delay1_DSTATE[1] = VisionTest_P.Delay1_InitialCondition;
294}
295
296/* Model terminate function */
297void VisionTest_terminate(void)
298{
299 /* (no terminate code required) */
300}
301