32 ** |
32 ** |
33 ** @brief |
33 ** @brief |
34 ** |
34 ** |
35 ** |
35 ** |
36 */ |
36 */ |
37 UNS8 sendPDO(CO_Data* d, s_PDO pdo, UNS8 req) |
37 |
38 { |
38 /*! |
39 UNS8 i; |
39 ** |
40 if( d->nodeState == Operational ) { |
|
41 Message m; |
|
42 |
|
43 /* Message copy for sending */ |
|
44 m.cob_id.w = pdo.cobId & 0x7FF; /* Because the cobId is 11 bytes |
|
45 length */ |
|
46 if ( req == NOT_A_REQUEST ) { |
|
47 UNS8 i; |
|
48 m.rtr = NOT_A_REQUEST; |
|
49 m.len = pdo.len; |
|
50 /* memcpy(&m.data, &pdo.data, m.len); */ |
|
51 /* This Memcpy depends on packing structure. Avoid */ |
|
52 for (i = 0 ; i < pdo.len ; i++) |
|
53 m.data[i] = pdo.data[i]; |
|
54 } |
|
55 else { |
|
56 m.rtr = REQUEST; |
|
57 m.len = 0; |
|
58 } |
|
59 |
|
60 MSG_WAR(0x3901, "sendPDO cobId :", m.cob_id.w); |
|
61 MSG_WAR(0x3902, " Nb octets : ", m.len); |
|
62 for (i = 0 ; i < m.len ; i++) { |
|
63 MSG_WAR(0x3903," data : ", m.data[i]); |
|
64 } |
|
65 |
|
66 return canSend(d->canHandle,&m); |
|
67 } /* end if */ |
|
68 return 0xFF; |
|
69 } |
|
70 |
|
71 /*! |
|
72 ** PDO Manager |
|
73 ** |
40 ** |
74 ** @param d |
41 ** @param d |
75 ** @param cobId |
42 ** @param TPDO_com TPDO communication parameters OD entry |
|
43 ** @param TPDO_map TPDO mapping parameters OD entry |
76 ** |
44 ** |
77 ** @return |
45 ** @return |
78 **/ |
46 **/ |
79 UNS8 PDOmGR(CO_Data* d, UNS32 cobId) |
47 |
80 { |
48 UNS8 buildPDO(CO_Data* d, UNS8 numPdo, Message *pdo) |
81 UNS8 res; |
49 { |
82 UNS8 i; |
50 const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo; |
83 s_PDO pdo; |
51 const indextable* TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo; |
84 |
52 |
85 MSG_WAR(0x3905, "PDOmGR",0); |
53 UNS8 prp_j = 0x00; |
86 |
54 UNS8 offset = 0x00; |
87 /* if PDO is waiting for transmission, |
55 const UNS8* pMappingCount = (UNS8*) TPDO_map->pSubindex[0].pObject; |
88 preparation of the message to send */ |
56 |
89 pdo.cobId = cobId; |
57 pdo->cob_id.w = *(UNS32*)TPDO_com->pSubindex[1].pObject; |
90 pdo.len = d->process_var.count; |
58 pdo->rtr = NOT_A_REQUEST; |
91 /* memcpy(&(pdo.data), &(process_var.data), pdo.len); */ |
59 |
92 /* Ce memcpy devrait tre portable */ |
60 MSG_WAR(0x3009, " PDO CobId is : ", *(UNS32*)TPDO_com->pSubindex[1].pObject); |
93 for ( i = 0 ; i < pdo.len ; i++) |
61 MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount ); |
94 pdo.data[i] = d->process_var.data[i]; |
62 |
95 |
63 do{ |
96 res = sendPDO(d, pdo, NOT_A_REQUEST); |
64 UNS8 dataType; /* Unused */ |
97 |
65 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; /* temporary space to hold bits */ |
98 return res; |
66 |
99 } |
67 /* pointer fo the var which holds the mapping parameter of an mapping entry */ |
100 |
68 UNS32* pMappingParameter = (UNS32*) TPDO_map->pSubindex[prp_j + 1].pObject; |
101 #if 0 |
69 UNS16 index = (UNS16)((*pMappingParameter) >> 16); |
102 /*********************************************************************/ |
70 UNS8 Size = (UNS8)(*pMappingParameter); /* Size in bits */ |
103 /* TODO : implement bit mapping */ |
71 UNS8 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ |
104 /*********************************************************************/ |
72 UNS8 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); |
105 |
73 |
106 UNS8 buildPDO(CO_Data* d, UNS16 index) |
74 MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter); |
107 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */ |
75 MSG_WAR(0x3050, " at index : ", TPDO_map->index); |
108 UNS16 ind; |
76 MSG_WAR(0x3051, " sub-index : ", prp_j + 1); |
109 UNS8 subInd; |
77 |
110 |
78 if( getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ) != OD_SUCCESSFUL ){ |
111 UNS8 * pMappingCount = NULL; /* count of mapped objects... */ |
79 MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter)); |
112 /* pointer to the var which is mapped to a pdo */ |
80 return 0xFF; |
113 /* void * pMappedAppObject = NULL; */ |
81 } |
114 /* pointer fo the var which holds the mapping parameter of an mapping entry */ |
82 /* copy bit per bit in little endian*/ |
115 |
83 CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&pdo->data[offset>>3], offset%8, 0); |
116 UNS32 * pMappingParameter = NULL; |
84 |
117 |
85 offset += Size ; |
118 UNS8 Size; |
86 prp_j++; |
119 UNS8 dataType; |
87 }while( prp_j < *pMappingCount ); |
120 UNS8 offset; |
88 |
121 UNS16 offsetObjdict; |
89 pdo->len = 1 + ((offset - 1) >> 3); |
122 UNS16 offsetObjdictPrm; |
90 |
123 UNS32 objDict; |
91 MSG_WAR(0x3015, " End scan mapped variable", 0); |
124 |
92 |
125 subInd=(UNS8)0x00; |
93 return 0; |
126 offset = 0x00; |
94 } |
127 ind = index - 0x1800; |
|
128 |
|
129 MSG_WAR(0x3910,"Prepare PDO to send index :", index); |
|
130 |
|
131 /* only operational state allows PDO transmission */ |
|
132 if( d->nodeState != Operational ) { |
|
133 MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index); |
|
134 return 0xFF; |
|
135 } |
|
136 offsetObjdictPrm = d->firstIndex->PDO_TRS; |
|
137 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
|
138 |
|
139 if (offsetObjdictPrm && offsetObjdict) |
|
140 { |
|
141 /* get mapped objects number to transmit with this PDO */ |
|
142 pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject; |
|
143 MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount); |
|
144 MSG_WAR(0x3913, " at index : ", 0x1A00 + ind); |
|
145 while (subInd < *pMappingCount) { /* Loop on mapped variables */ |
|
146 /* get mapping parameters */ |
|
147 pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject; |
|
148 MSG_WAR(0x3914, "Get the mapping at index : ", (UNS16)0x1A00 + ind); |
|
149 MSG_WAR(0x3915, " subIndex : ", subInd + 1); |
|
150 MSG_WAR(0x3916, " value : ", *(UNS32 *)pMappingParameter); |
|
151 /* Get the mapped variable */ |
|
152 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); |
|
153 objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16), |
|
154 (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF), |
|
155 (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); |
|
156 |
|
157 if (objDict != OD_SUCCESSFUL) { |
|
158 MSG_WAR(0x2919, "error accessing to the mapped var : ", subInd + 1); |
|
159 MSG_WAR(0x2920, " Mapped at index : ", (*pMappingParameter) >> 16); |
|
160 MSG_WAR(0x2921, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
|
161 return 0xFF; |
|
162 } |
|
163 |
|
164 offset += Size; |
|
165 d->process_var.count = offset; |
|
166 subInd++; |
|
167 }/* end Loop on mapped variables */ |
|
168 } |
|
169 return 0; |
|
170 } |
|
171 #endif |
|
172 |
95 |
173 /*! |
96 /*! |
174 ** |
97 ** |
175 ** |
98 ** |
176 ** @param d |
99 ** @param d |
370 break; |
283 break; |
371 |
284 |
372 |
285 |
373 case state4:/* check transmission type (after request?) */ |
286 case state4:/* check transmission type (after request?) */ |
374 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject; |
287 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject; |
375 if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) { |
288 if ( (*pTransmissionType == TRANS_RTR) || |
|
289 (*pTransmissionType == TRANS_RTR_SYNC )) { |
376 status = state5; |
290 status = state5; |
377 break; |
291 break; |
378 } |
292 }else if( |
379 else { |
293 (*pTransmissionType == TRANS_EVENT_PROFILE) || |
|
294 (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) { |
|
295 /* Zap all timers and inhibit flag */ |
|
296 d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer); |
|
297 d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer); |
|
298 d->PDO_status[numPdo].transmit_type_parameter &= ~PDO_INHIBITED; |
|
299 /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */ |
|
300 PDOEventTimerAlarm(d, numPdo); |
|
301 return 0; |
|
302 }else { |
380 /* The requested PDO is not to send on request. So, does |
303 /* The requested PDO is not to send on request. So, does |
381 nothing. */ |
304 nothing. */ |
382 MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w); |
305 MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w); |
383 return 0xFF; |
306 return 0xFF; |
384 } |
307 } |
385 |
308 |
386 case state5:/* get mapped objects number */ |
309 case state5:/* build and send requested PDO */ |
387 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
310 { |
388 lastIndex = d->lastIndex->PDO_TRS_MAP; |
311 Message pdo; |
389 pMappingCount = (UNS8*) (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; |
312 if( buildPDO(d, numPdo, &pdo)) |
390 numMap = 0; |
313 { |
391 while (numMap < *pMappingCount) { |
314 MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter)); |
392 pMappingParameter = (UNS32*) (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; |
315 //return 0xFF; /*No real reason to stop...*/ |
393 /* Get the mapped variable */ |
316 } |
394 Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3)); |
317 canSend(d->canHandle,&pdo); |
395 objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16), |
|
396 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF), |
|
397 (void *)&d->process_var.data[offset], &Size, &dataType, 0 ); |
|
398 if (objDict != OD_SUCCESSFUL) { |
|
399 MSG_ERR(0x1948, "error accessing to the mapped var : ", numMap + 1); |
|
400 MSG_WAR(0x2949, " Mapped at index : ", (*pMappingParameter) >> 16); |
|
401 MSG_WAR(0x2950, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
|
402 return 0xFF; |
|
403 } |
|
404 offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3); |
|
405 d->process_var.count = offset; |
|
406 numMap++; |
|
407 |
|
408 } /* end while */ |
|
409 PDOmGR( d, *pwCobId ); /* Transmit the PDO */ |
|
410 return 0; |
318 return 0; |
411 |
319 } |
412 }/* end switch status */ |
320 }/* end switch status */ |
413 }/* end while */ |
321 }/* end while */ |
414 }/* end if Requete */ |
322 }/* end if Requete */ |
415 |
323 |
416 return 0; |
324 return 0; |
474 /*And decrement counter.*/ |
382 /*And decrement counter.*/ |
475 NbBits -= BitsToCopy; |
383 NbBits -= BitsToCopy; |
476 } |
384 } |
477 |
385 |
478 } |
386 } |
479 |
|
480 #if 0 |
|
481 |
|
482 /*********************************************************************/ |
|
483 /* TODO : reimplement this using CallBacks |
|
484 */ |
|
485 /*********************************************************************/ |
|
486 |
|
487 /*! |
387 /*! |
488 ** |
388 ** |
489 ** |
389 ** |
490 ** @param d |
390 ** @param d |
491 ** @param variable |
|
492 ** |
391 ** |
493 ** @return |
392 ** @return |
494 **/ |
393 **/ |
495 UNS8 sendPDOevent( CO_Data* d, void * variable ) |
394 |
496 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite |
395 UNS8 sendPDOevent( CO_Data* d) |
497 loop if it fails.*/ |
396 { |
498 UNS32 objDict = 0; |
397 /* Calls _sendPDOevent specifying it is not a sync event */ |
499 UNS8 ind, sub_ind; |
398 return _sendPDOevent(d, 0); |
500 UNS8 status; |
399 } |
501 UNS8 offset; |
400 |
502 UNS8 * pMappingCount = NULL; |
401 |
503 UNS32 * pMappingParameter = NULL; |
402 void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum) |
504 void * pMappedAppObject = NULL; |
403 { |
505 UNS8 * pTransmissionType = NULL; /* pointer to the transmission |
404 printf("EV PDOEventTimerAlarm : %d\n", pdoNum); |
506 type */ |
405 |
507 UNS32 * pwCobId = NULL; |
406 /* This is needed to avoid deletion of re-attribuated timer */ |
508 UNS8 * pSize; |
407 d->PDO_status[pdoNum].event_timer = TIMER_NONE; |
509 UNS8 size; |
408 /* force emission of PDO by artificially changing last emitted*/ |
510 UNS8 dataType; |
409 d->PDO_status[pdoNum].last_message.cob_id.w = 0; |
511 UNS16 offsetObjdict; |
410 _sendPDOevent( d, 0 ); /* not a Sync Event*/ |
512 UNS16 offsetObjdictPrm; |
411 } |
513 UNS16 lastIndex; |
412 |
514 UNS8 numMap; |
413 void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum) |
515 ind = 0x00; |
414 { |
516 sub_ind = 1; |
415 printf("EV PDOInhibitTimerAlarm : %d\n", pdoNum); |
517 offset = 0x00; |
416 |
518 pSize = &size; |
417 /* This is needed to avoid deletion of re-attribuated timer */ |
519 status = state1; |
418 d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE; |
520 |
419 /* Remove inhibit flag */ |
521 |
420 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED; |
522 /* look for the index and subindex where the variable is mapped */ |
421 _sendPDOevent( d, 0 ); /* not a Sync Event*/ |
523 /* Then, send the pdo which contains the variable. */ |
422 } |
524 |
423 |
525 MSG_WAR (0x3960, "sendPDOevent", 0); |
424 /*! |
526 offsetObjdictPrm = d->firstIndex->PDO_TRS; |
425 ** |
527 |
426 ** |
528 offsetObjdict = d->firstIndex->PDO_TRS_MAP; |
427 ** @param d |
529 lastIndex = d->lastIndex->PDO_TRS_MAP; |
428 ** @param isSyncEvent |
530 |
429 ** |
531 if (offsetObjdictPrm && offsetObjdict) |
430 ** @return |
532 /* Loop on PDO Transmit */ |
431 **/ |
533 while(offsetObjdict <= lastIndex){ |
432 |
534 /* Check the transmission mode */ |
433 UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent ) |
535 pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject; |
434 { |
536 if (*pTransmissionType != TRANS_EVENT) { |
435 UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */ |
537 ind++; |
436 UNS8* pTransmissionType = NULL; |
538 offsetObjdict++; |
437 UNS8 status = state3; |
539 offsetObjdictPrm++; |
438 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
540 continue; |
439 UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP; |
|
440 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
|
441 |
|
442 /* study all PDO stored in the objects dictionary */ |
|
443 if(offsetObjdict){ |
|
444 Message clean = Message_Initializer; |
|
445 Message pdo = Message_Initializer; |
|
446 while( offsetObjdict <= lastIndex) { |
|
447 switch( status ) { |
|
448 case state3: /* get the PDO transmission type */ |
|
449 if (d->objdict[offsetObjdict].bSubCount <= 2) { |
|
450 MSG_ERR(0x1004, "Subindex 2 not found at index ", 0x1800 + pdoNum); |
|
451 return 0xFF; |
|
452 } |
|
453 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject; |
|
454 MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum); |
|
455 |
|
456 /* check if transmission type is after (this) SYNC */ |
|
457 /* The message may not be transmited every SYNC but every n SYNC */ |
|
458 if( isSyncEvent && |
|
459 (*pTransmissionType >= TRANS_SYNC_MIN) && |
|
460 (*pTransmissionType <= TRANS_SYNC_MAX) && |
|
461 (++d->PDO_status[pdoNum].transmit_type_parameter == *pTransmissionType) ) { |
|
462 d->PDO_status[pdoNum].transmit_type_parameter = 0; |
|
463 MSG_WAR(0x3007, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType); |
|
464 pdo = clean; |
|
465 if(buildPDO(d, pdoNum, &pdo)) |
|
466 { |
|
467 MSG_ERR(0x3006, " Couldn't build TPDO number : ", pdoNum); |
|
468 status = state11; |
|
469 break; |
|
470 } |
|
471 status = state5; |
541 } |
472 } |
542 pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject; |
473 /* If transmission on Event and not inhibited, check for changes */ |
543 numMap = 1; /* mapped variable */ |
474 else if((*pTransmissionType == TRANS_EVENT_PROFILE || |
544 while (numMap <= *pMappingCount) { |
475 *pTransmissionType == TRANS_EVENT_SPECIFIC )&& |
545 pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject; |
476 !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) { |
546 /* Get the variable */ |
477 MSG_WAR(0x3008, " PDO is on EVENT. Trans type : ", *pTransmissionType); |
547 objDict = getODentry( d, |
478 pdo = pdo = clean; |
548 (UNS16)((*pMappingParameter) >> 16), |
479 if(buildPDO(d, pdoNum, &pdo)) |
549 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF), |
480 { |
550 (void * *)&pMappedAppObject, pSize, &dataType, 0 ); |
481 MSG_ERR(0x3007, " Couldn't build TPDO number : ", pdoNum); |
551 if( objDict != OD_SUCCESSFUL ) { |
482 status = state11; |
552 MSG_WAR(0x2961, "Error in dict. at index : ", |
483 break; |
553 (*pMappingParameter) >> (UNS8)16); |
484 } |
554 |
485 |
555 MSG_WAR(0x2962, " subindex : ", |
486 /*Compare new and old PDO*/ |
556 ((*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); |
487 if(d->PDO_status[pdoNum].last_message.cob_id.w == pdo.cob_id.w && |
557 return 0xFF; |
488 d->PDO_status[pdoNum].last_message.len == pdo.len && |
558 } |
489 *(UNS64*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS64*)(&pdo.data[0])){ |
559 if (pMappedAppObject == variable) { // Variable found ! |
490 /* No changes -> go to next pdo*/ |
560 MSG_WAR(0x3963, "Variable to send found at index : ", |
491 status = state11; |
561 (*pMappingParameter) >> 16); |
492 }else{ |
562 MSG_WAR(0x3964, " subIndex : ", |
493 MSG_WAR(0x3008, "Changes TPDO number : ", pdoNum); |
563 ((*pMappingParameter) >> 8 ) & 0x000000FF); |
494 printf("EV Changes TPDO number : %d\n", pdoNum); |
564 buildPDO(d, 0x1800 + ind); |
495 /* Changes detected -> transmit message */ |
565 /* Get the cobId */ |
496 status = state5; |
566 pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject; |
497 |
567 PDOmGR( d, *pwCobId ); /* Send the PDO */ |
498 /* Start both event_timer and inhibit_timer*/ |
568 return 0; |
499 DelAlarm(d->PDO_status[pdoNum].event_timer); |
569 } |
500 d->PDO_status[pdoNum].event_timer = SetAlarm(d, pdoNum, &PDOEventTimerAlarm, MS_TO_TIMEVAL(*(UNS16*)d->objdict[offsetObjdict].pSubindex[5].pObject), 0); |
570 numMap++; |
501 |
571 } /* End loop on mapped variable */ |
502 DelAlarm(d->PDO_status[pdoNum].inhibit_timer); |
572 ind++; |
503 d->PDO_status[pdoNum].inhibit_timer = SetAlarm(d, pdoNum, &PDOInhibitTimerAlarm, US_TO_TIMEVAL(*(UNS16*)d->objdict[offsetObjdict].pSubindex[3].pObject * 100), 0); |
573 offsetObjdict++; |
504 |
574 offsetObjdictPrm++; |
505 /* and inhibit TPDO */ |
575 } /* End loop while on PDO */ |
506 d->PDO_status[pdoNum].transmit_type_parameter |= PDO_INHIBITED; |
576 |
507 } |
577 MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0); |
508 }else{ |
578 return 0xFF; |
509 MSG_WAR(0x3009, " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", *pTransmissionType); |
579 |
510 status = state11; |
580 } |
511 } |
581 #endif |
512 break; |
582 |
513 case state5: /*Send the pdo*/ |
583 |
514 /*store_as_last_message*/ |
584 |
515 d->PDO_status[pdoNum].last_message = pdo; |
|
516 MSG_WAR(0x3901, "sendPDO cobId :", pdo.cob_id.w); |
|
517 MSG_WAR(0x3902, " Nb octets : ", pdo.len); |
|
518 {int i; |
|
519 for (i = 0 ; i < pdo.len ; i++) { |
|
520 MSG_WAR(0x3903," data : ", pdo.data[i]); |
|
521 }} |
|
522 |
|
523 canSend(d->canHandle,&pdo); |
|
524 status = state11; |
|
525 break; |
|
526 case state11: /*Go to next TPDO*/ |
|
527 pdoNum++; |
|
528 offsetObjdict++; |
|
529 offsetObjdictMap++; |
|
530 MSG_WAR(0x3017, "next pdo index : ", pdoNum); |
|
531 status = state3; |
|
532 break; |
|
533 |
|
534 default: |
|
535 MSG_ERR(0x1019,"Unknown state has been reached : %d",status); |
|
536 return 0xFF; |
|
537 }/* end switch case */ |
|
538 |
|
539 }/* end while */ |
|
540 } |
|
541 return 0; |
|
542 } |
|
543 |
|
544 void PDOInit(CO_Data* d) |
|
545 { |
|
546 |
|
547 /* TODO: implement callbacks on 140xh |
|
548 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
|
549 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
|
550 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
|
551 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
|
552 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
|
553 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
|
554 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
|
555 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
|
556 */ |
|
557 |
|
558 _sendPDOevent(d, 0 ); |
|
559 } |
|
560 |
|
561 void PDOStop(CO_Data* d) |
|
562 { |
|
563 UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */ |
|
564 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
|
565 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
|
566 if(offsetObjdict) while( offsetObjdict <= lastIndex) { |
|
567 d->PDO_status[pdoNum].event_timer = DelAlarm(d->PDO_status[pdoNum].event_timer); |
|
568 d->PDO_status[pdoNum].inhibit_timer = DelAlarm(d->PDO_status[pdoNum].inhibit_timer); |
|
569 d->PDO_status[pdoNum].transmit_type_parameter = 0; |
|
570 d->PDO_status[pdoNum].last_message.cob_id.w = 0; |
|
571 pdoNum++; |
|
572 offsetObjdict++; |
|
573 } |
|
574 } |