38 { |
38 { |
39 UNS8 i; |
39 UNS8 i; |
40 if( d->nodeState == Operational ) { |
40 if( d->nodeState == Operational ) { |
41 Message m; |
41 Message m; |
42 |
42 |
43 /*! Message copy for sending */ |
43 /* Message copy for sending */ |
44 m.cob_id.w = pdo.cobId & 0x7FF; /*! Because the cobId is 11 bytes |
44 m.cob_id.w = pdo.cobId & 0x7FF; /* Because the cobId is 11 bytes |
45 length */ |
45 length */ |
46 if ( req == NOT_A_REQUEST ) { |
46 if ( req == NOT_A_REQUEST ) { |
47 UNS8 i; |
47 UNS8 i; |
48 m.rtr = NOT_A_REQUEST; |
48 m.rtr = NOT_A_REQUEST; |
49 m.len = pdo.len; |
49 m.len = pdo.len; |
50 /*! memcpy(&m.data, &pdo.data, m.len); */ |
50 /* memcpy(&m.data, &pdo.data, m.len); */ |
51 /*! This Memcpy depends on packing structure. Avoid */ |
51 /* This Memcpy depends on packing structure. Avoid */ |
52 for (i = 0 ; i < pdo.len ; i++) |
52 for (i = 0 ; i < pdo.len ; i++) |
53 m.data[i] = pdo.data[i]; |
53 m.data[i] = pdo.data[i]; |
54 } |
54 } |
55 else { |
55 else { |
56 m.rtr = REQUEST; |
56 m.rtr = REQUEST; |
82 UNS8 i; |
82 UNS8 i; |
83 s_PDO pdo; |
83 s_PDO pdo; |
84 |
84 |
85 MSG_WAR(0x3905, "PDOmGR",0); |
85 MSG_WAR(0x3905, "PDOmGR",0); |
86 |
86 |
87 /*! if PDO is waiting for transmission, |
87 /* if PDO is waiting for transmission, |
88 preparation of the message to send */ |
88 preparation of the message to send */ |
89 pdo.cobId = cobId; |
89 pdo.cobId = cobId; |
90 pdo.len = d->process_var.count; |
90 pdo.len = d->process_var.count; |
91 /*! memcpy(&(pdo.data), &(process_var.data), pdo.len); */ |
91 /* memcpy(&(pdo.data), &(process_var.data), pdo.len); */ |
92 /* Ce memcpy devrait tre portable */ |
92 /* Ce memcpy devrait tre portable */ |
93 for ( i = 0 ; i < pdo.len ; i++) |
93 for ( i = 0 ; i < pdo.len ; i++) |
94 pdo.data[i] = d->process_var.data[i]; |
94 pdo.data[i] = d->process_var.data[i]; |
95 |
95 |
96 res = sendPDO(d, pdo, NOT_A_REQUEST); |
96 res = sendPDO(d, pdo, NOT_A_REQUEST); |
126 offset = 0x00; |
126 offset = 0x00; |
127 ind = index - 0x1800; |
127 ind = index - 0x1800; |
128 |
128 |
129 MSG_WAR(0x3910,"Prepare PDO to send index :", index); |
129 MSG_WAR(0x3910,"Prepare PDO to send index :", index); |
130 |
130 |
131 /*! only operational state allows PDO transmission */ |
131 /* only operational state allows PDO transmission */ |
132 if( d->nodeState != Operational ) { |
132 if( d->nodeState != Operational ) { |
133 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); |
134 return 0xFF; |
134 return 0xFF; |
135 } |
135 } |
136 offsetObjdictPrm = d->firstIndex->PDO_TRS; |
136 offsetObjdictPrm = d->firstIndex->PDO_TRS; |
137 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
137 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
138 |
138 |
139 if (offsetObjdictPrm && offsetObjdict) |
139 if (offsetObjdictPrm && offsetObjdict) |
140 { |
140 { |
141 /*! get mapped objects number to transmit with this PDO */ |
141 /* get mapped objects number to transmit with this PDO */ |
142 pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject; |
142 pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject; |
143 MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount); |
143 MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount); |
144 MSG_WAR(0x3913, " at index : ", 0x1A00 + ind); |
144 MSG_WAR(0x3913, " at index : ", 0x1A00 + ind); |
145 while (subInd < *pMappingCount) { /*! Loop on mapped variables */ |
145 while (subInd < *pMappingCount) { /* Loop on mapped variables */ |
146 /*! get mapping parameters */ |
146 /* get mapping parameters */ |
147 pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject; |
147 pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject; |
148 MSG_WAR(0x3914, "Get the mapping at index : ", (UNS16)0x1A00 + ind); |
148 MSG_WAR(0x3914, "Get the mapping at index : ", (UNS16)0x1A00 + ind); |
149 MSG_WAR(0x3915, " subIndex : ", subInd + 1); |
149 MSG_WAR(0x3915, " subIndex : ", subInd + 1); |
150 MSG_WAR(0x3916, " value : ", *(UNS32 *)pMappingParameter); |
150 MSG_WAR(0x3916, " value : ", *(UNS32 *)pMappingParameter); |
151 /*! Get the mapped variable */ |
151 /* Get the mapped variable */ |
152 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); |
152 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); |
153 objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16), |
153 objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16), |
154 (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF), |
154 (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF), |
155 (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); |
155 (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); |
156 |
156 |
184 UNS16 offset; |
184 UNS16 offset; |
185 UNS16 lastIndex; |
185 UNS16 lastIndex; |
186 UNS8 err; |
186 UNS8 err; |
187 |
187 |
188 MSG_WAR(0x3930, "sendPDOrequest ",0); |
188 MSG_WAR(0x3930, "sendPDOrequest ",0); |
189 /*! Sending the request only if the cobid have been found on the PDO |
189 /* Sending the request only if the cobid have been found on the PDO |
190 receive */ |
190 receive */ |
191 /*! part dictionary */ |
191 /* part dictionary */ |
192 offset = d->firstIndex->PDO_RCV; |
192 offset = d->firstIndex->PDO_RCV; |
193 lastIndex = d->lastIndex->PDO_RCV; |
193 lastIndex = d->lastIndex->PDO_RCV; |
194 if (offset) |
194 if (offset) |
195 while (offset <= lastIndex) { |
195 while (offset <= lastIndex) { |
196 /*! get the CobId*/ |
196 /* get the CobId*/ |
197 pwCobId = d->objdict[offset].pSubindex[1].pObject; |
197 pwCobId = d->objdict[offset].pSubindex[1].pObject; |
198 |
198 |
199 if ( *pwCobId == cobId ) { |
199 if ( *pwCobId == cobId ) { |
200 s_PDO pdo; |
200 s_PDO pdo; |
201 pdo.cobId = *pwCobId; |
201 pdo.cobId = *pwCobId; |
219 ** @return |
219 ** @return |
220 **/ |
220 **/ |
221 UNS8 proceedPDO(CO_Data* d, Message *m) |
221 UNS8 proceedPDO(CO_Data* d, Message *m) |
222 { |
222 { |
223 UNS8 numPdo; |
223 UNS8 numPdo; |
224 UNS8 numMap; /*! Number of the mapped varable */ |
224 UNS8 numMap; /* Number of the mapped varable */ |
225 UNS8 i; |
225 UNS8 i; |
226 UNS8 * pMappingCount = NULL; /*! count of mapped objects... */ |
226 UNS8 * pMappingCount = NULL; /* count of mapped objects... */ |
227 /*! pointer to the var which is mapped to a pdo... */ |
227 /* pointer to the var which is mapped to a pdo... */ |
228 /*! void * pMappedAppObject = NULL; */ |
228 /* void * pMappedAppObject = NULL; */ |
229 /*! pointer fo the var which holds the mapping parameter of an |
229 /* pointer fo the var which holds the mapping parameter of an |
230 mapping entry */ |
230 mapping entry */ |
231 UNS32 * pMappingParameter = NULL; |
231 UNS32 * pMappingParameter = NULL; |
232 UNS8 * pTransmissionType = NULL; /*! pointer to the transmission |
232 UNS8 * pTransmissionType = NULL; /* pointer to the transmission |
233 type */ |
233 type */ |
234 UNS32 * pwCobId = NULL; |
234 UNS32 * pwCobId = NULL; |
235 UNS8 Size; |
235 UNS8 Size; |
236 UNS8 dataType; |
236 UNS8 dataType; |
237 UNS8 offset; |
237 UNS8 offset; |
243 |
243 |
244 MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff)); |
244 MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff)); |
245 offset = 0x00; |
245 offset = 0x00; |
246 numPdo = 0; |
246 numPdo = 0; |
247 numMap = 0; |
247 numMap = 0; |
248 if((*m).rtr == NOT_A_REQUEST ) { /*! The PDO received is not a |
248 if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a |
249 request. */ |
249 request. */ |
250 offsetObjdict = d->firstIndex->PDO_RCV; |
250 offsetObjdict = d->firstIndex->PDO_RCV; |
251 lastIndex = d->lastIndex->PDO_RCV; |
251 lastIndex = d->lastIndex->PDO_RCV; |
252 |
252 |
253 /*! study of all the PDO stored in the dictionary */ |
253 /* study of all the PDO stored in the dictionary */ |
254 if(offsetObjdict) |
254 if(offsetObjdict) |
255 while (offsetObjdict <= lastIndex) { |
255 while (offsetObjdict <= lastIndex) { |
256 |
256 |
257 switch( status ) { |
257 switch( status ) { |
258 |
258 |
259 case state1:/*! data are stored in process_var array */ |
259 case state1:/* data are stored in process_var array */ |
260 /*! memcpy(&(process_var.data), &m->data, (*m).len); */ |
260 /* memcpy(&(process_var.data), &m->data, (*m).len); */ |
261 /*! Ce memcpy devrait etre portable */ |
261 /* Ce memcpy devrait etre portable */ |
262 for ( i = 0 ; i < m->len ; i++) |
262 for ( i = 0 ; i < m->len ; i++) |
263 d->process_var.data[i] = m->data[i]; |
263 d->process_var.data[i] = m->data[i]; |
264 d->process_var.count = (*m).len; |
264 d->process_var.count = (*m).len; |
265 |
265 |
266 status = state2; |
266 status = state2; |
267 break; |
267 break; |
268 |
268 |
269 case state2: |
269 case state2: |
270 /*! get CobId of the dictionary correspondant to the received |
270 /* get CobId of the dictionary correspondant to the received |
271 PDO */ |
271 PDO */ |
272 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; |
272 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; |
273 /*! check the CobId coherance */ |
273 /* check the CobId coherance */ |
274 /*!pwCobId is the cobId read in the dictionary at the state 3 |
274 /*pwCobId is the cobId read in the dictionary at the state 3 |
275 */ |
275 */ |
276 if ( *pwCobId == (*m).cob_id.w ){ |
276 if ( *pwCobId == (*m).cob_id.w ){ |
277 /*! The cobId is recognized */ |
277 /* The cobId is recognized */ |
278 status = state4; |
278 status = state4; |
279 MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo); |
279 MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo); |
280 break; |
280 break; |
281 } |
281 } |
282 else { |
282 else { |
283 /*! cobId received does not match with those write in the |
283 /* cobId received does not match with those write in the |
284 dictionnary */ |
284 dictionnary */ |
285 numPdo++; |
285 numPdo++; |
286 offsetObjdict++; |
286 offsetObjdict++; |
287 status = state2; |
287 status = state2; |
288 break; |
288 break; |
289 } |
289 } |
290 |
290 |
291 case state4:/*! Get Mapped Objects Number */ |
291 case state4:/* Get Mapped Objects Number */ |
292 /*! The cobId of the message received has been found in the |
292 /* The cobId of the message received has been found in the |
293 dictionnary. */ |
293 dictionnary. */ |
294 offsetObjdict = d->firstIndex->PDO_RCV_MAP; |
294 offsetObjdict = d->firstIndex->PDO_RCV_MAP; |
295 lastIndex = d->lastIndex->PDO_RCV_MAP; |
295 lastIndex = d->lastIndex->PDO_RCV_MAP; |
296 pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; |
296 pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; |
297 numMap = 0; |
297 numMap = 0; |
301 pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; |
301 pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; |
302 if (pMappingParameter == NULL) { |
302 if (pMappingParameter == NULL) { |
303 MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1); |
303 MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1); |
304 return 0xFF; |
304 return 0xFF; |
305 } |
305 } |
306 /*! Get the addresse of the mapped variable. */ |
306 /* Get the addresse of the mapped variable. */ |
307 /*! detail of *pMappingParameter : */ |
307 /* detail of *pMappingParameter : */ |
308 /*! The 16 hight bits contains the index, the medium 8 bits |
308 /* The 16 hight bits contains the index, the medium 8 bits |
309 contains the subindex, */ |
309 contains the subindex, */ |
310 /*! and the lower 8 bits contains the size of the mapped |
310 /* and the lower 8 bits contains the size of the mapped |
311 variable. */ |
311 variable. */ |
312 |
312 |
313 Size = (UNS8)(*pMappingParameter); |
313 Size = (UNS8)(*pMappingParameter); |
314 |
314 |
315 /*! copy bit per bit in little endian */ |
315 /* copy bit per bit in little endian */ |
316 CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0); |
316 CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0); |
317 |
317 |
318 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => |
318 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => |
319 2, ... */ |
319 2, ... */ |
320 |
320 |
330 } |
330 } |
331 |
331 |
332 MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w); |
332 MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w); |
333 MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16); |
333 MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16); |
334 MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
334 MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
335 /*! MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */ |
335 /* MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */ |
336 offset += Size; |
336 offset += Size; |
337 numMap++; |
337 numMap++; |
338 } /*! end loop while on mapped variables */ |
338 } /* end loop while on mapped variables */ |
339 |
339 |
340 offset=0x00; |
340 offset=0x00; |
341 numMap = 0; |
341 numMap = 0; |
342 return 0; |
342 return 0; |
343 |
343 |
344 }/*! end switch status*/ |
344 }/* end switch status*/ |
345 }/*! end while*/ |
345 }/* end while*/ |
346 }/*! end if Donnees */ |
346 }/* end if Donnees */ |
347 else if ((*m).rtr == REQUEST ){ |
347 else if ((*m).rtr == REQUEST ){ |
348 MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w); |
348 MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w); |
349 status = state1; |
349 status = state1; |
350 offsetObjdict = d->firstIndex->PDO_TRS; |
350 offsetObjdict = d->firstIndex->PDO_TRS; |
351 lastIndex = d->lastIndex->PDO_TRS; |
351 lastIndex = d->lastIndex->PDO_TRS; |
352 if(offsetObjdict) while( offsetObjdict <= lastIndex ){ |
352 if(offsetObjdict) while( offsetObjdict <= lastIndex ){ |
353 /*! study of all PDO stored in the objects dictionary */ |
353 /* study of all PDO stored in the objects dictionary */ |
354 |
354 |
355 switch( status ){ |
355 switch( status ){ |
356 |
356 |
357 case state1:/*! check the CobId */ |
357 case state1:/* check the CobId */ |
358 /*! get CobId of the dictionary which match to the received PDO |
358 /* get CobId of the dictionary which match to the received PDO |
359 */ |
359 */ |
360 pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject; |
360 pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject; |
361 if ( *pwCobId == (*m).cob_id.w ) { |
361 if ( *pwCobId == (*m).cob_id.w ) { |
362 status = state4; |
362 status = state4; |
363 break; |
363 break; |
368 } |
368 } |
369 status = state1; |
369 status = state1; |
370 break; |
370 break; |
371 |
371 |
372 |
372 |
373 case state4:/*! check transmission type (after request?) */ |
373 case state4:/* check transmission type (after request?) */ |
374 pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject; |
374 pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject; |
375 if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) { |
375 if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) { |
376 status = state5; |
376 status = state5; |
377 break; |
377 break; |
378 } |
378 } |
379 else { |
379 else { |
380 /*! The requested PDO is not to send on request. So, does |
380 /* The requested PDO is not to send on request. So, does |
381 nothing. */ |
381 nothing. */ |
382 MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w); |
382 MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w); |
383 return 0xFF; |
383 return 0xFF; |
384 } |
384 } |
385 |
385 |
386 case state5:/*! get mapped objects number */ |
386 case state5:/* get mapped objects number */ |
387 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
387 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
388 lastIndex = d->lastIndex->PDO_TRS_MAP; |
388 lastIndex = d->lastIndex->PDO_TRS_MAP; |
389 pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; |
389 pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; |
390 numMap = 0; |
390 numMap = 0; |
391 while (numMap < *pMappingCount) { |
391 while (numMap < *pMappingCount) { |
392 pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; |
392 pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; |
393 /*! Get the mapped variable */ |
393 /* Get the mapped variable */ |
394 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); |
394 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); |
395 objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16), |
395 objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16), |
396 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF), |
396 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF), |
397 (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); |
397 (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); |
398 if (objDict != OD_SUCCESSFUL) { |
398 if (objDict != OD_SUCCESSFUL) { |
427 ** @param DestBitIndex |
427 ** @param DestBitIndex |
428 ** @param DestBigEndian |
428 ** @param DestBigEndian |
429 **/ |
429 **/ |
430 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) |
431 { |
431 { |
432 /*! This loop copy as many bits that it can each time, crossing*/ |
432 /* This loop copy as many bits that it can each time, crossing*/ |
433 /*! successively bytes*/ |
433 /* successively bytes*/ |
434 // boundaries from LSB to MSB. |
434 // boundaries from LSB to MSB. |
435 while(NbBits > 0) |
435 while(NbBits > 0) |
436 { |
436 { |
437 /*! Bit missalignement between src and dest*/ |
437 /* Bit missalignement between src and dest*/ |
438 INTEGER8 Vect = DestBitIndex - SrcBitIndex; |
438 INTEGER8 Vect = DestBitIndex - SrcBitIndex; |
439 |
439 |
440 /*! We can now get src and align it to dest*/ |
440 /* We can now get src and align it to dest*/ |
441 UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect; |
441 UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect; |
442 |
442 |
443 /*! Compute the nb of bit we will be able to copy*/ |
443 /* Compute the nb of bit we will be able to copy*/ |
444 UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex : 8 - SrcBitIndex ); |
444 UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex : 8 - SrcBitIndex ); |
445 UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit; |
445 UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit; |
446 |
446 |
447 /*! Create a mask that will serve in:*/ |
447 /* Create a mask that will serve in:*/ |
448 UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex))); |
448 UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex))); |
449 |
449 |
450 /*! - Filtering src*/ |
450 /* - Filtering src*/ |
451 UNS8 Filtered = Aligned & ~Mask; |
451 UNS8 Filtered = Aligned & ~Mask; |
452 |
452 |
453 /*! - and erase bits where we write, preserve where we don't*/ |
453 /* - and erase bits where we write, preserve where we don't*/ |
454 *DestByteIndex &= Mask; |
454 *DestByteIndex &= Mask; |
455 |
455 |
456 /*! Then write.*/ |
456 /* Then write.*/ |
457 *DestByteIndex |= Filtered ; |
457 *DestByteIndex |= Filtered ; |
458 |
458 |
459 /*!Compute next time cursors for src*/ |
459 /*Compute next time cursors for src*/ |
460 if((SrcBitIndex += BitsToCopy)>7)/*! cross boundary ?*/ |
460 if((SrcBitIndex += BitsToCopy)>7)/* cross boundary ?*/ |
461 { |
461 { |
462 SrcBitIndex = 0;/*! First bit*/ |
462 SrcBitIndex = 0;/* First bit*/ |
463 SrcByteIndex += (SrcBigEndian ? -1 : 1);/*! Next byte*/ |
463 SrcByteIndex += (SrcBigEndian ? -1 : 1);/* Next byte*/ |
464 } |
464 } |
465 |
465 |
466 |
466 |
467 /*!Compute next time cursors for dest*/ |
467 /*Compute next time cursors for dest*/ |
468 if((DestBitIndex += BitsToCopy)>7) |
468 if((DestBitIndex += BitsToCopy)>7) |
469 { |
469 { |
470 DestBitIndex = 0;/*! First bit*/ |
470 DestBitIndex = 0;/* First bit*/ |
471 DestByteIndex += (DestBigEndian ? -1 : 1);/*! Next byte*/ |
471 DestByteIndex += (DestBigEndian ? -1 : 1);/* Next byte*/ |
472 } |
472 } |
473 |
473 |
474 /*!And decrement counter.*/ |
474 /*And decrement counter.*/ |
475 NbBits -= BitsToCopy; |
475 NbBits -= BitsToCopy; |
476 } |
476 } |
477 |
477 |
478 } |
478 } |
479 |
479 |
491 ** @param variable |
491 ** @param variable |
492 ** |
492 ** |
493 ** @return |
493 ** @return |
494 **/ |
494 **/ |
495 UNS8 sendPDOevent( CO_Data* d, void * variable ) |
495 UNS8 sendPDOevent( CO_Data* d, void * variable ) |
496 { /*! DO NOT USE MSG_ERR because the macro may send a PDO -> infinite |
496 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite |
497 loop if it fails.*/ |
497 loop if it fails.*/ |
498 UNS32 objDict = 0; |
498 UNS32 objDict = 0; |
499 UNS8 ind, sub_ind; |
499 UNS8 ind, sub_ind; |
500 UNS8 status; |
500 UNS8 status; |
501 UNS8 offset; |
501 UNS8 offset; |
502 UNS8 * pMappingCount = NULL; |
502 UNS8 * pMappingCount = NULL; |
503 UNS32 * pMappingParameter = NULL; |
503 UNS32 * pMappingParameter = NULL; |
504 void * pMappedAppObject = NULL; |
504 void * pMappedAppObject = NULL; |
505 UNS8 * pTransmissionType = NULL; /*! pointer to the transmission |
505 UNS8 * pTransmissionType = NULL; /* pointer to the transmission |
506 type */ |
506 type */ |
507 UNS32 * pwCobId = NULL; |
507 UNS32 * pwCobId = NULL; |
508 UNS8 * pSize; |
508 UNS8 * pSize; |
509 UNS8 size; |
509 UNS8 size; |
510 UNS8 dataType; |
510 UNS8 dataType; |
517 offset = 0x00; |
517 offset = 0x00; |
518 pSize = &size; |
518 pSize = &size; |
519 status = state1; |
519 status = state1; |
520 |
520 |
521 |
521 |
522 /*! look for the index and subindex where the variable is mapped */ |
522 /* look for the index and subindex where the variable is mapped */ |
523 /*! Then, send the pdo which contains the variable. */ |
523 /* Then, send the pdo which contains the variable. */ |
524 |
524 |
525 MSG_WAR (0x3960, "sendPDOevent", 0); |
525 MSG_WAR (0x3960, "sendPDOevent", 0); |
526 offsetObjdictPrm = d->firstIndex->PDO_TRS; |
526 offsetObjdictPrm = d->firstIndex->PDO_TRS; |
527 |
527 |
528 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
528 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
529 lastIndex = d->lastIndex->PDO_TRS_MAP; |
529 lastIndex = d->lastIndex->PDO_TRS_MAP; |
530 |
530 |
531 if (offsetObjdictPrm && offsetObjdict) |
531 if (offsetObjdictPrm && offsetObjdict) |
532 /*! Loop on PDO Transmit */ |
532 /* Loop on PDO Transmit */ |
533 while(offsetObjdict <= lastIndex){ |
533 while(offsetObjdict <= lastIndex){ |
534 /*! Check the transmission mode */ |
534 /* Check the transmission mode */ |
535 pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject; |
535 pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject; |
536 if (*pTransmissionType != TRANS_EVENT) { |
536 if (*pTransmissionType != TRANS_EVENT) { |
537 ind++; |
537 ind++; |
538 offsetObjdict++; |
538 offsetObjdict++; |
539 offsetObjdictPrm++; |
539 offsetObjdictPrm++; |
540 continue; |
540 continue; |
541 } |
541 } |
542 pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject; |
542 pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject; |
543 numMap = 1; /*! mapped variable */ |
543 numMap = 1; /* mapped variable */ |
544 while (numMap <= *pMappingCount) { |
544 while (numMap <= *pMappingCount) { |
545 pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject; |
545 pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject; |
546 /*! Get the variable */ |
546 /* Get the variable */ |
547 objDict = getODentry( d, |
547 objDict = getODentry( d, |
548 (UNS16)((*pMappingParameter) >> 16), |
548 (UNS16)((*pMappingParameter) >> 16), |
549 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF), |
549 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF), |
550 (void * *)&pMappedAppObject, pSize, &dataType, 0 ); |
550 (void * *)&pMappedAppObject, pSize, &dataType, 0 ); |
551 if( objDict != OD_SUCCESSFUL ) { |
551 if( objDict != OD_SUCCESSFUL ) { |
560 MSG_WAR(0x3963, "Variable to send found at index : ", |
560 MSG_WAR(0x3963, "Variable to send found at index : ", |
561 (*pMappingParameter) >> 16); |
561 (*pMappingParameter) >> 16); |
562 MSG_WAR(0x3964, " subIndex : ", |
562 MSG_WAR(0x3964, " subIndex : ", |
563 ((*pMappingParameter) >> 8 ) & 0x000000FF); |
563 ((*pMappingParameter) >> 8 ) & 0x000000FF); |
564 buildPDO(d, 0x1800 + ind); |
564 buildPDO(d, 0x1800 + ind); |
565 /*! Get the cobId */ |
565 /* Get the cobId */ |
566 pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject; |
566 pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject; |
567 PDOmGR( d, *pwCobId ); /*! Send the PDO */ |
567 PDOmGR( d, *pwCobId ); /* Send the PDO */ |
568 return 0; |
568 return 0; |
569 } |
569 } |
570 numMap++; |
570 numMap++; |
571 } /*! End loop on mapped variable */ |
571 } /* End loop on mapped variable */ |
572 ind++; |
572 ind++; |
573 offsetObjdict++; |
573 offsetObjdict++; |
574 offsetObjdictPrm++; |
574 offsetObjdictPrm++; |
575 } /*! End loop while on PDO */ |
575 } /* End loop while on PDO */ |
576 |
576 |
577 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); |
578 return 0xFF; |
578 return 0xFF; |
579 |
579 |
580 } |
580 } |