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) */ |
25 | B_VisionTest_T VisionTest_B; |
26 | |
27 | /* Block states (auto storage) */ |
28 | DW_VisionTest_T VisionTest_DW; |
29 | |
30 | /* Real-time model */ |
31 | RT_MODEL_VisionTest_T VisionTest_M_; |
32 | RT_MODEL_VisionTest_T *const VisionTest_M = &VisionTest_M_; |
33 | |
34 | /* Model step function */ |
35 | void 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 */ |
202 | void 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 */ |
297 | void VisionTest_terminate(void) |
298 | { |
299 | /* (no terminate code required) */ |
300 | } |
301 | |