104 UNS32 objDict; |
123 UNS32 objDict; |
105 |
124 |
106 subInd=(UNS8)0x00; |
125 subInd=(UNS8)0x00; |
107 offset = 0x00; |
126 offset = 0x00; |
108 ind = index - 0x1800; |
127 ind = index - 0x1800; |
109 |
128 |
110 MSG_WAR(0x3910,"Prepare PDO to send index :", index); |
129 MSG_WAR(0x3910,"Prepare PDO to send index :", index); |
111 |
130 |
112 /* only operational state allows PDO transmission */ |
131 /*! only operational state allows PDO transmission */ |
113 if( d->nodeState != Operational ) { |
132 if( d->nodeState != Operational ) { |
114 MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index); |
133 MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index); |
115 return 0xFF; |
134 return 0xFF; |
116 } |
135 } |
117 offsetObjdictPrm = d->firstIndex->PDO_TRS; |
136 offsetObjdictPrm = d->firstIndex->PDO_TRS; |
118 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
137 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
119 |
138 |
120 if (offsetObjdictPrm && offsetObjdict) |
139 if (offsetObjdictPrm && offsetObjdict) |
121 { |
140 { |
122 /* get mapped objects number to transmit with this PDO */ |
141 /*! get mapped objects number to transmit with this PDO */ |
123 pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject; |
142 pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject; |
124 MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount); |
143 MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount); |
125 MSG_WAR(0x3913, " at index : ", 0x1A00 + ind); |
144 MSG_WAR(0x3913, " at index : ", 0x1A00 + ind); |
126 while (subInd < *pMappingCount) { /* Loop on mapped variables */ |
145 while (subInd < *pMappingCount) { /*! Loop on mapped variables */ |
127 /* get mapping parameters */ |
146 /*! get mapping parameters */ |
128 pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject; |
147 pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject; |
129 MSG_WAR(0x3914, "Get the mapping at index : ", (UNS16)0x1A00 + ind); |
148 MSG_WAR(0x3914, "Get the mapping at index : ", (UNS16)0x1A00 + ind); |
130 MSG_WAR(0x3915, " subIndex : ", subInd + 1); |
149 MSG_WAR(0x3915, " subIndex : ", subInd + 1); |
131 MSG_WAR(0x3916, " value : ", *(UNS32 *)pMappingParameter); |
150 MSG_WAR(0x3916, " value : ", *(UNS32 *)pMappingParameter); |
132 /* Get the mapped variable */ |
151 /*! Get the mapped variable */ |
133 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); |
152 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); |
134 objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16), |
153 objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16), |
135 (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF), |
154 (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF), |
136 (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); |
155 (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); |
137 |
156 |
138 if (objDict != OD_SUCCESSFUL) { |
157 if (objDict != OD_SUCCESSFUL) { |
139 MSG_WAR(0x2919, "error accessing to the mapped var : ", subInd + 1); |
158 MSG_WAR(0x2919, "error accessing to the mapped var : ", subInd + 1); |
140 MSG_WAR(0x2920, " Mapped at index : ", (*pMappingParameter) >> 16); |
159 MSG_WAR(0x2920, " Mapped at index : ", (*pMappingParameter) >> 16); |
141 MSG_WAR(0x2921, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
160 MSG_WAR(0x2921, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
142 return 0xFF; |
161 return 0xFF; |
143 } |
162 } |
144 |
163 |
145 offset += Size; |
164 offset += Size; |
146 d->process_var.count = offset; |
165 d->process_var.count = offset; |
147 subInd++; |
166 subInd++; |
148 }/* end Loop on mapped variables */ |
167 }/*! end Loop on mapped variables */ |
149 } |
168 } |
150 return 0; |
169 return 0; |
151 } |
170 } |
152 #endif |
171 #endif |
153 /**************************************************************************/ |
172 |
|
173 /*! |
|
174 ** |
|
175 ** |
|
176 ** @param d |
|
177 ** @param cobId |
|
178 ** |
|
179 ** @return |
|
180 **/ |
154 UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId ) |
181 UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId ) |
155 { |
182 { |
156 UNS32 * pwCobId; |
183 UNS32 * pwCobId; |
157 UNS16 offset; |
184 UNS16 offset; |
158 UNS16 lastIndex; |
185 UNS16 lastIndex; |
159 UNS8 err; |
186 UNS8 err; |
160 |
187 |
161 MSG_WAR(0x3930, "sendPDOrequest ",0); |
188 MSG_WAR(0x3930, "sendPDOrequest ",0); |
162 /* Sending the request only if the cobid have been found on the PDO receive */ |
189 /*! Sending the request only if the cobid have been found on the PDO |
163 /* part dictionary */ |
190 receive */ |
|
191 /*! part dictionary */ |
164 offset = d->firstIndex->PDO_RCV; |
192 offset = d->firstIndex->PDO_RCV; |
165 lastIndex = d->lastIndex->PDO_RCV; |
193 lastIndex = d->lastIndex->PDO_RCV; |
166 if (offset) |
194 if (offset) |
167 while (offset <= lastIndex) { |
195 while (offset <= lastIndex) { |
168 /*get the CobId*/ |
196 /*! get the CobId*/ |
169 pwCobId = d->objdict[offset].pSubindex[1].pObject; |
197 pwCobId = d->objdict[offset].pSubindex[1].pObject; |
170 |
198 |
171 if ( *pwCobId == cobId ) { |
199 if ( *pwCobId == cobId ) { |
172 s_PDO pdo; |
200 s_PDO pdo; |
173 pdo.cobId = *pwCobId; |
201 pdo.cobId = *pwCobId; |
174 pdo.len = 0; |
202 pdo.len = 0; |
175 err = sendPDO(d, pdo, REQUEST); |
203 err = sendPDO(d, pdo, REQUEST); |
176 return err; |
204 return err; |
177 } |
205 } |
178 offset++; |
206 offset++; |
179 } |
207 } |
180 MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId); |
208 MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId); |
181 return 0xFF; |
209 return 0xFF; |
182 } |
210 } |
183 |
211 |
184 |
212 |
185 |
213 /*! |
186 /***********************************************************************/ |
214 ** |
|
215 ** |
|
216 ** @param d |
|
217 ** @param m |
|
218 ** |
|
219 ** @return |
|
220 **/ |
187 UNS8 proceedPDO(CO_Data* d, Message *m) |
221 UNS8 proceedPDO(CO_Data* d, Message *m) |
188 { |
222 { |
189 UNS8 numPdo; |
223 UNS8 numPdo; |
190 UNS8 numMap; /* Number of the mapped varable */ |
224 UNS8 numMap; /*! Number of the mapped varable */ |
191 UNS8 i; |
225 UNS8 i; |
192 UNS8 * pMappingCount = NULL; /* count of mapped objects... */ |
226 UNS8 * pMappingCount = NULL; /*! count of mapped objects... */ |
193 /* pointer to the var which is mapped to a pdo... */ |
227 /*! pointer to the var which is mapped to a pdo... */ |
194 /* void * pMappedAppObject = NULL; */ |
228 /*! void * pMappedAppObject = NULL; */ |
195 /* pointer fo the var which holds the mapping parameter of an mapping entry */ |
229 /*! pointer fo the var which holds the mapping parameter of an |
196 UNS32 * pMappingParameter = NULL; |
230 mapping entry */ |
197 UNS8 * pTransmissionType = NULL; /* pointer to the transmission type */ |
231 UNS32 * pMappingParameter = NULL; |
|
232 UNS8 * pTransmissionType = NULL; /*! pointer to the transmission |
|
233 type */ |
198 UNS32 * pwCobId = NULL; |
234 UNS32 * pwCobId = NULL; |
199 UNS8 Size; |
235 UNS8 Size; |
200 UNS8 dataType; |
236 UNS8 dataType; |
201 UNS8 offset; |
237 UNS8 offset; |
202 UNS8 status; |
238 UNS8 status; |
203 UNS32 objDict; |
239 UNS32 objDict; |
204 UNS16 offsetObjdict; |
240 UNS16 offsetObjdict; |
205 UNS16 lastIndex; |
241 UNS16 lastIndex; |
206 status = state1; |
242 status = state1; |
207 |
243 |
208 MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff)); |
244 MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff)); |
209 offset = 0x00; |
245 offset = 0x00; |
210 numPdo = 0; |
246 numPdo = 0; |
211 numMap = 0; |
247 numMap = 0; |
212 if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a request. */ |
248 if((*m).rtr == NOT_A_REQUEST ) { /*! The PDO received is not a |
|
249 request. */ |
213 offsetObjdict = d->firstIndex->PDO_RCV; |
250 offsetObjdict = d->firstIndex->PDO_RCV; |
214 lastIndex = d->lastIndex->PDO_RCV; |
251 lastIndex = d->lastIndex->PDO_RCV; |
215 |
252 |
216 /* study of all the PDO stored in the dictionary */ |
253 /*! study of all the PDO stored in the dictionary */ |
217 if(offsetObjdict) |
254 if(offsetObjdict) |
218 while (offsetObjdict <= lastIndex) { |
255 while (offsetObjdict <= lastIndex) { |
219 |
256 |
220 switch( status ) { |
257 switch( status ) { |
221 |
258 |
222 case state1: /* data are stored in process_var array */ |
259 case state1:/*! data are stored in process_var array */ |
223 /* memcpy(&(process_var.data), &m->data, (*m).len); */ |
260 /*! memcpy(&(process_var.data), &m->data, (*m).len); */ |
224 /* Ce memcpy devrait être portable. */ |
261 /*! Ce memcpy devrait etre portable */ |
225 for ( i = 0 ; i < m->len ; i++) |
262 for ( i = 0 ; i < m->len ; i++) |
226 d->process_var.data[i] = m->data[i]; |
263 d->process_var.data[i] = m->data[i]; |
227 d->process_var.count = (*m).len; |
264 d->process_var.count = (*m).len; |
228 |
265 |
229 status = state2; |
266 status = state2; |
230 break; |
267 break; |
231 |
268 |
232 case state2: |
269 case state2: |
233 /* get CobId of the dictionary correspondant to the received PDO */ |
270 /*! get CobId of the dictionary correspondant to the received |
234 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; |
271 PDO */ |
235 /* check the CobId coherance */ |
272 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; |
236 /*pwCobId is the cobId read in the dictionary at the state 3 */ |
273 /*! check the CobId coherance */ |
237 if ( *pwCobId == (*m).cob_id.w ){ |
274 /*!pwCobId is the cobId read in the dictionary at the state 3 |
238 /* The cobId is recognized */ |
275 */ |
239 status = state4; |
276 if ( *pwCobId == (*m).cob_id.w ){ |
240 MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo); |
277 /*! The cobId is recognized */ |
241 break; |
278 status = state4; |
242 } |
279 MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo); |
243 else { |
280 break; |
244 /* cobId received does not match with those write in the dictionnary */ |
281 } |
245 numPdo++; |
282 else { |
246 offsetObjdict++; |
283 /*! cobId received does not match with those write in the |
247 status = state2; |
284 dictionnary */ |
248 break; |
285 numPdo++; |
249 } |
286 offsetObjdict++; |
250 |
287 status = state2; |
251 case state4: /* get mapped objects number */ |
288 break; |
252 /* The cobId of the message received has been found in the dictionnary. */ |
289 } |
253 offsetObjdict = d->firstIndex->PDO_RCV_MAP; |
290 |
254 lastIndex = d->lastIndex->PDO_RCV_MAP; |
291 case state4:/*! Get Mapped Objects Number */ |
255 pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; |
292 /*! The cobId of the message received has been found in the |
256 numMap = 0; |
293 dictionnary. */ |
257 while (numMap < *pMappingCount) { |
294 offsetObjdict = d->firstIndex->PDO_RCV_MAP; |
258 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; |
295 lastIndex = d->lastIndex->PDO_RCV_MAP; |
259 UNS8 ByteSize; |
296 pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; |
260 pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; |
297 numMap = 0; |
261 if (pMappingParameter == NULL) { |
298 while (numMap < *pMappingCount) { |
262 MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1); |
299 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; |
263 return 0xFF; |
300 UNS8 ByteSize; |
264 } |
301 pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; |
265 /* Get the addresse of the mapped variable. */ |
302 if (pMappingParameter == NULL) { |
266 /* detail of *pMappingParameter : */ |
303 MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1); |
267 /* The 16 hight bits contains the index, the medium 8 bits contains the subindex, */ |
304 return 0xFF; |
268 /* and the lower 8 bits contains the size of the mapped variable. */ |
305 } |
269 |
306 /*! Get the addresse of the mapped variable. */ |
270 Size = (UNS8)(*pMappingParameter); |
307 /*! detail of *pMappingParameter : */ |
271 |
308 /*! The 16 hight bits contains the index, the medium 8 bits |
272 /* copy bit per bit in little endian */ |
309 contains the subindex, */ |
273 CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0); |
310 /*! and the lower 8 bits contains the size of the mapped |
274 |
311 variable. */ |
275 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ |
312 |
276 |
313 Size = (UNS8)(*pMappingParameter); |
277 objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16), |
314 |
278 (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF), |
315 /*! copy bit per bit in little endian */ |
279 tmp, &ByteSize, 0 ); |
316 CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0); |
280 |
317 |
281 if(objDict != OD_SUCCESSFUL) { |
318 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => |
282 MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1); |
319 2, ... */ |
283 MSG_WAR(0x2939, " Mapped at index : ", (*pMappingParameter) >> 16); |
320 |
284 MSG_WAR(0x2940, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
321 objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16), |
285 return 0xFF; |
322 (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF), |
286 } |
323 tmp, &ByteSize, 0 ); |
287 |
324 |
288 MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w); |
325 if(objDict != OD_SUCCESSFUL) { |
289 MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16); |
326 MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1); |
290 MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
327 MSG_WAR(0x2939, " Mapped at index : ", (*pMappingParameter) >> 16); |
291 /* MSG_WAR(0x3945, " data : ",*((UNS32 *)pMappedAppObject)); */ |
328 MSG_WAR(0x2940, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
292 offset += Size; |
329 return 0xFF; |
293 numMap++; |
330 } |
294 } /* end loop while on mapped variables */ |
331 |
295 |
332 MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w); |
296 offset=0x00; |
333 MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16); |
297 numMap = 0; |
334 MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
298 return 0; |
335 /*! MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */ |
299 |
336 offset += Size; |
300 }/* end switch status */ |
337 numMap++; |
301 }/* end while */ |
338 } /*! end loop while on mapped variables */ |
302 }/* end if Donnees */ |
339 |
303 |
340 offset=0x00; |
304 |
341 numMap = 0; |
305 else if ((*m).rtr == REQUEST ){ |
342 return 0; |
306 MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w); |
343 |
307 status = state1; |
344 }/*! end switch status*/ |
308 offsetObjdict = d->firstIndex->PDO_TRS; |
345 }/*! end while*/ |
309 lastIndex = d->lastIndex->PDO_TRS; |
346 }/*! end if Donnees */ |
310 if(offsetObjdict) while( offsetObjdict <= lastIndex ){ |
347 else if ((*m).rtr == REQUEST ){ |
311 /* study of all PDO stored in the objects dictionary */ |
348 MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w); |
312 |
349 status = state1; |
313 switch( status ){ |
350 offsetObjdict = d->firstIndex->PDO_TRS; |
314 |
351 lastIndex = d->lastIndex->PDO_TRS; |
315 case state1: /* check the CobId */ |
352 if(offsetObjdict) while( offsetObjdict <= lastIndex ){ |
316 /* get CobId of the dictionary which match to the received PDO */ |
353 /*! study of all PDO stored in the objects dictionary */ |
317 pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject; |
354 |
318 if ( *pwCobId == (*m).cob_id.w ) { |
355 switch( status ){ |
319 status = state4; |
356 |
320 break; |
357 case state1:/*! check the CobId */ |
321 } |
358 /*! get CobId of the dictionary which match to the received PDO |
322 else { |
359 */ |
323 numPdo++; |
360 pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject; |
324 offsetObjdict++; |
361 if ( *pwCobId == (*m).cob_id.w ) { |
325 } |
362 status = state4; |
326 status = state1; |
363 break; |
327 break; |
364 } |
328 |
365 else { |
329 |
366 numPdo++; |
330 case state4: /* check transmission type (after request?) */ |
367 offsetObjdict++; |
331 pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject; |
368 } |
332 if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) { |
369 status = state1; |
333 status = state5; |
370 break; |
334 break; |
371 |
335 } |
372 |
336 else { |
373 case state4:/*! check transmission type (after request?) */ |
337 /* The requested PDO is not to send on request. So, does nothing. */ |
374 pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject; |
338 MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w); |
375 if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) { |
339 return 0xFF; |
376 status = state5; |
340 } |
377 break; |
341 |
378 } |
342 case state5: /* get mapped objects number */ |
379 else { |
343 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
380 /*! The requested PDO is not to send on request. So, does |
344 lastIndex = d->lastIndex->PDO_TRS_MAP; |
381 nothing. */ |
345 pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; |
382 MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w); |
346 numMap = 0; |
383 return 0xFF; |
347 while (numMap < *pMappingCount) { |
384 } |
348 pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; |
385 |
349 /* Get the mapped variable */ |
386 case state5:/*! get mapped objects number */ |
350 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); |
387 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
351 objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16), |
388 lastIndex = d->lastIndex->PDO_TRS_MAP; |
352 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF), |
389 pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; |
353 (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); |
390 numMap = 0; |
354 if (objDict != OD_SUCCESSFUL) { |
391 while (numMap < *pMappingCount) { |
355 MSG_ERR(0x1948, "error accessing to the mapped var : ", numMap + 1); |
392 pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; |
356 MSG_WAR(0x2949, " Mapped at index : ", (*pMappingParameter) >> 16); |
393 /*! Get the mapped variable */ |
357 MSG_WAR(0x2950, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
394 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); |
358 return 0xFF; |
395 objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16), |
359 } |
396 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF), |
360 offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3); |
397 (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); |
361 d->process_var.count = offset; |
398 if (objDict != OD_SUCCESSFUL) { |
362 numMap++; |
399 MSG_ERR(0x1948, "error accessing to the mapped var : ", numMap + 1); |
363 |
400 MSG_WAR(0x2949, " Mapped at index : ", (*pMappingParameter) >> 16); |
364 } /* end while */ |
401 MSG_WAR(0x2950, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
365 PDOmGR( d, *pwCobId ); /* Transmit the PDO */ |
402 return 0xFF; |
366 return 0; |
403 } |
367 |
404 offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3); |
368 }/* end switch status */ |
405 d->process_var.count = offset; |
369 }/* end while */ |
406 numMap++; |
370 }/* end if Requete */ |
407 |
371 |
408 } /*! end while */ |
|
409 PDOmGR( d, *pwCobId ); /*! Transmit the PDO */ |
|
410 return 0; |
|
411 |
|
412 }/*! end switch status */ |
|
413 }/*! end while */ |
|
414 }/*! end if Requete */ |
|
415 |
372 return 0; |
416 return 0; |
373 } |
417 } |
374 |
418 |
375 |
419 /*! |
|
420 ** |
|
421 ** |
|
422 ** @param NbBits |
|
423 ** @param SrcByteIndex |
|
424 ** @param SrcBitIndex |
|
425 ** @param SrcBigEndian |
|
426 ** @param DestByteIndex |
|
427 ** @param DestBitIndex |
|
428 ** @param DestBigEndian |
|
429 **/ |
376 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian) |
430 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian) |
377 { |
431 { |
378 //This loop copy as many bits that it can each time, crossing successively bytes |
432 /*! This loop copy as many bits that it can each time, crossing*/ |
379 // boundaries from LSB to MSB. |
433 /*! successively bytes*/ |
380 while(NbBits > 0) |
434 // boundaries from LSB to MSB. |
381 { |
435 while(NbBits > 0) |
382 // Bit missalignement between src and dest |
436 { |
383 INTEGER8 Vect = DestBitIndex - SrcBitIndex; |
437 /*! Bit missalignement between src and dest*/ |
384 |
438 INTEGER8 Vect = DestBitIndex - SrcBitIndex; |
385 // We can now get src and align it to dest |
439 |
386 UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect; |
440 /*! We can now get src and align it to dest*/ |
387 |
441 UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect; |
388 // Compute the nb of bit we will be able to copy |
442 |
389 UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex : 8 - SrcBitIndex ); |
443 /*! Compute the nb of bit we will be able to copy*/ |
390 UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit; |
444 UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex : 8 - SrcBitIndex ); |
391 |
445 UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit; |
392 // Create a mask that will serve in: |
446 |
393 UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex))); |
447 /*! Create a mask that will serve in:*/ |
394 |
448 UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex))); |
395 // - Filtering src |
449 |
396 UNS8 Filtered = Aligned & ~Mask; |
450 /*! - Filtering src*/ |
397 |
451 UNS8 Filtered = Aligned & ~Mask; |
398 // - and erase bits where we write, preserve where we don't |
452 |
399 *DestByteIndex &= Mask; |
453 /*! - and erase bits where we write, preserve where we don't*/ |
400 |
454 *DestByteIndex &= Mask; |
401 // Then write. |
455 |
402 *DestByteIndex |= Filtered ; |
456 /*! Then write.*/ |
403 |
457 *DestByteIndex |= Filtered ; |
404 //Compute next time cursors for src |
458 |
405 if((SrcBitIndex += BitsToCopy)>7) // cross boundary ? |
459 /*!Compute next time cursors for src*/ |
406 { |
460 if((SrcBitIndex += BitsToCopy)>7)/*! cross boundary ?*/ |
407 SrcBitIndex = 0; // First bit |
461 { |
408 SrcByteIndex += (SrcBigEndian ? -1 : 1); // Next byte |
462 SrcBitIndex = 0;/*! First bit*/ |
409 } |
463 SrcByteIndex += (SrcBigEndian ? -1 : 1);/*! Next byte*/ |
410 |
464 } |
411 //Compute next time cursors for dest |
465 |
412 if((DestBitIndex += BitsToCopy)>7) |
466 |
413 { |
467 /*!Compute next time cursors for dest*/ |
414 DestBitIndex = 0; // First bit |
468 if((DestBitIndex += BitsToCopy)>7) |
415 DestByteIndex += (DestBigEndian ? -1 : 1);// Next byte |
469 { |
416 } |
470 DestBitIndex = 0;/*! First bit*/ |
417 |
471 DestByteIndex += (DestBigEndian ? -1 : 1);/*! Next byte*/ |
418 //And decrement counter. |
472 } |
419 NbBits -= BitsToCopy; |
473 |
420 } |
474 /*!And decrement counter.*/ |
|
475 NbBits -= BitsToCopy; |
|
476 } |
421 |
477 |
422 } |
478 } |
423 |
479 |
424 #if 0 |
480 #if 0 |
425 |
481 |
426 /*********************************************************************/ |
482 /*********************************************************************/ |
427 /* TODO : reimplement this using CallBacks */ |
483 /* TODO : reimplement this using CallBacks |
|
484 */ |
428 /*********************************************************************/ |
485 /*********************************************************************/ |
429 |
486 |
|
487 /*! |
|
488 ** |
|
489 ** |
|
490 ** @param d |
|
491 ** @param variable |
|
492 ** |
|
493 ** @return |
|
494 **/ |
430 UNS8 sendPDOevent( CO_Data* d, void * variable ) |
495 UNS8 sendPDOevent( CO_Data* d, void * variable ) |
431 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */ |
496 { /*! DO NOT USE MSG_ERR because the macro may send a PDO -> infinite |
|
497 loop if it fails.*/ |
432 UNS32 objDict = 0; |
498 UNS32 objDict = 0; |
433 UNS8 ind, sub_ind; |
499 UNS8 ind, sub_ind; |
434 UNS8 status; |
500 UNS8 status; |
435 UNS8 offset; |
501 UNS8 offset; |
436 UNS8 * pMappingCount = NULL; |
502 UNS8 * pMappingCount = NULL; |
437 UNS32 * pMappingParameter = NULL; |
503 UNS32 * pMappingParameter = NULL; |
438 void * pMappedAppObject = NULL; |
504 void * pMappedAppObject = NULL; |
439 UNS8 * pTransmissionType = NULL; /* pointer to the transmission type */ |
505 UNS8 * pTransmissionType = NULL; /*! pointer to the transmission |
|
506 type */ |
440 UNS32 * pwCobId = NULL; |
507 UNS32 * pwCobId = NULL; |
441 UNS8 * pSize; |
508 UNS8 * pSize; |
442 UNS8 size; |
509 UNS8 size; |
443 UNS8 dataType; |
510 UNS8 dataType; |
444 UNS16 offsetObjdict; |
511 UNS16 offsetObjdict; |
445 UNS16 offsetObjdictPrm; |
512 UNS16 offsetObjdictPrm; |
446 UNS16 lastIndex; |
513 UNS16 lastIndex; |
447 UNS8 numMap; |
514 UNS8 numMap; |
448 ind = 0x00; |
515 ind = 0x00; |
449 sub_ind = 1; |
516 sub_ind = 1; |
450 offset = 0x00; |
517 offset = 0x00; |
451 pSize = &size; |
518 pSize = &size; |
452 status = state1; |
519 status = state1; |
453 |
520 |
454 /* look for the index and subindex where the variable is mapped */ |
521 |
455 /* Then, send the pdo which contains the variable. */ |
522 /*! look for the index and subindex where the variable is mapped */ |
|
523 /*! Then, send the pdo which contains the variable. */ |
456 |
524 |
457 MSG_WAR (0x3960, "sendPDOevent", 0); |
525 MSG_WAR (0x3960, "sendPDOevent", 0); |
458 offsetObjdictPrm = d->firstIndex->PDO_TRS; |
526 offsetObjdictPrm = d->firstIndex->PDO_TRS; |
459 |
527 |
460 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
528 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
461 lastIndex = d->lastIndex->PDO_TRS_MAP; |
529 lastIndex = d->lastIndex->PDO_TRS_MAP; |
462 |
530 |
463 if (offsetObjdictPrm && offsetObjdict) |
531 if (offsetObjdictPrm && offsetObjdict) |
464 /* Loop on PDO Transmit */ |
532 /*! Loop on PDO Transmit */ |
465 while(offsetObjdict <= lastIndex){ |
533 while(offsetObjdict <= lastIndex){ |
466 /* Check the transmission mode */ |
534 /*! Check the transmission mode */ |
467 pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject; |
535 pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject; |
468 if (*pTransmissionType != TRANS_EVENT) { |
536 if (*pTransmissionType != TRANS_EVENT) { |
469 ind++; |
537 ind++; |
470 offsetObjdict++; |
538 offsetObjdict++; |
471 offsetObjdictPrm++; |
539 offsetObjdictPrm++; |
472 continue; |
540 continue; |
473 } |
541 } |
474 pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject; |
542 pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject; |
475 numMap = 1; /* mapped variable */ |
543 numMap = 1; /*! mapped variable */ |
476 while (numMap <= *pMappingCount) { |
544 while (numMap <= *pMappingCount) { |
477 pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject; |
545 pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject; |
478 /* Get the variable */ |
546 /*! Get the variable */ |
479 objDict = getODentry( d, |
547 objDict = getODentry( d, |
480 (UNS16)((*pMappingParameter) >> 16), |
548 (UNS16)((*pMappingParameter) >> 16), |
481 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF), |
549 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF), |
482 (void * *)&pMappedAppObject, pSize, &dataType, 0 ); |
550 (void * *)&pMappedAppObject, pSize, &dataType, 0 ); |
483 if( objDict != OD_SUCCESSFUL ) { |
551 if( objDict != OD_SUCCESSFUL ) { |
484 MSG_WAR(0x2961, "Error in dict. at index : ", |
552 MSG_WAR(0x2961, "Error in dict. at index : ", |
485 (*pMappingParameter) >> (UNS8)16); |
553 (*pMappingParameter) >> (UNS8)16); |
486 |
554 |
487 MSG_WAR(0x2962, " subindex : ", |
555 MSG_WAR(0x2962, " subindex : ", |
488 ((*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); |
556 ((*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); |
489 return 0xFF; |
557 return 0xFF; |
490 } |
558 } |
491 if (pMappedAppObject == variable) { // Variable found ! |
559 if (pMappedAppObject == variable) { // Variable found ! |
492 MSG_WAR(0x3963, "Variable to send found at index : ", |
560 MSG_WAR(0x3963, "Variable to send found at index : ", |
493 (*pMappingParameter) >> 16); |
561 (*pMappingParameter) >> 16); |
494 MSG_WAR(0x3964, " subIndex : ", |
562 MSG_WAR(0x3964, " subIndex : ", |
495 ((*pMappingParameter) >> 8 ) & 0x000000FF); |
563 ((*pMappingParameter) >> 8 ) & 0x000000FF); |
496 buildPDO(d, 0x1800 + ind); |
564 buildPDO(d, 0x1800 + ind); |
497 /* Get the cobId */ |
565 /*! Get the cobId */ |
498 pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject; |
566 pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject; |
499 PDOmGR( d, *pwCobId ); /* Send the PDO */ |
567 PDOmGR( d, *pwCobId ); /*! Send the PDO */ |
500 return 0; |
568 return 0; |
501 } |
569 } |
502 numMap++; |
570 numMap++; |
503 } /* End loop on mapped variable */ |
571 } /*! End loop on mapped variable */ |
504 ind++; |
572 ind++; |
505 offsetObjdict++; |
573 offsetObjdict++; |
506 offsetObjdictPrm++; |
574 offsetObjdictPrm++; |
507 } /* End loop while on PDO */ |
575 } /*! End loop while on PDO */ |
508 |
576 |
509 MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0); |
577 MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0); |
510 return 0xFF; |
578 return 0xFF; |
511 |
579 |
512 } |
580 } |
513 #endif |
581 #endif |
|
582 |
|
583 |
|
584 |