99 ** @param d |
99 ** @param d |
100 ** @param cobId |
100 ** @param cobId |
101 ** |
101 ** |
102 ** @return |
102 ** @return |
103 **/ |
103 **/ |
104 UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId ) |
104 UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex ) |
105 { |
105 { |
106 UNS32 * pwCobId; |
106 UNS32 * pwCobId; |
107 UNS16 offset; |
107 UNS16 offset = d->firstIndex->PDO_RCV; |
108 UNS16 lastIndex; |
108 UNS16 lastIndex = d->lastIndex->PDO_RCV; |
109 UNS8 err; |
109 |
110 |
|
111 MSG_WAR(0x3930, "sendPDOrequest ",0); |
|
112 /* Sending the request only if the cobid have been found on the PDO |
110 /* Sending the request only if the cobid have been found on the PDO |
113 receive */ |
111 receive */ |
114 /* part dictionary */ |
112 /* part dictionary */ |
115 offset = d->firstIndex->PDO_RCV; |
113 |
116 lastIndex = d->lastIndex->PDO_RCV; |
114 MSG_WAR(0x3930, "sendPDOrequest RPDO Index : ",RPDOIndex); |
117 if (offset) |
115 |
118 while (offset <= lastIndex) { |
116 if (offset && RPDOIndex >= 0x1400){ |
|
117 offset += RPDOIndex - 0x1400; |
|
118 if (offset <= lastIndex) { |
119 /* get the CobId*/ |
119 /* get the CobId*/ |
120 pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
120 pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
121 |
121 |
122 if ( *pwCobId == cobId ) { |
122 MSG_WAR(0x3930, "sendPDOrequest cobId is : ",*pwCobId); |
123 Message pdo = {*pwCobId, REQUEST, 0}; |
123 |
124 return canSend(d->canHandle,&pdo); |
124 Message pdo = {*pwCobId, REQUEST, 0}; |
125 } |
125 return canSend(d->canHandle,&pdo); |
126 offset++; |
|
127 } |
126 } |
128 MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId); |
127 } |
|
128 MSG_ERR(0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex); |
129 return 0xFF; |
129 return 0xFF; |
130 } |
130 } |
131 |
131 |
132 |
132 |
133 /*! |
133 /*! |
281 } |
281 } |
282 status = state1; |
282 status = state1; |
283 break; |
283 break; |
284 |
284 |
285 |
285 |
286 case state4:/* check transmission type (after request?) */ |
286 case state4:/* check transmission type */ |
287 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject; |
287 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject; |
288 if ( (*pTransmissionType == TRANS_RTR) || |
288 /* If PDO is to be sampled and send on RTR, do it*/ |
289 (*pTransmissionType == TRANS_RTR_SYNC )) { |
289 if ( (*pTransmissionType == TRANS_RTR)) { |
290 status = state5; |
290 status = state5; |
|
291 break; |
|
292 /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */ |
|
293 }else if ((*pTransmissionType == TRANS_RTR_SYNC )) { |
|
294 if(d->PDO_status[numPdo].transmit_type_parameter & PDO_RTR_SYNC_READY){ |
|
295 /*Data ready, just send*/ |
|
296 canSend(d->canHandle,&d->PDO_status[numPdo].last_message); |
|
297 return 0; |
|
298 }else{ |
|
299 /* if SYNC did never occur, force emission with current data */ |
|
300 /* DS301 do not tell what to do in such a case...*/ |
|
301 MSG_ERR(0x1947, "Not ready RTR_SYNC TPDO send current data : ", m->cob_id.w); |
|
302 status = state5; |
|
303 } |
291 break; |
304 break; |
292 }else if( |
305 }else if( |
293 (*pTransmissionType == TRANS_EVENT_PROFILE) || |
306 (*pTransmissionType == TRANS_EVENT_PROFILE) || |
294 (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) { |
307 (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) { |
295 /* Zap all timers and inhibit flag */ |
308 /* Zap all timers and inhibit flag */ |
399 } |
412 } |
400 |
413 |
401 |
414 |
402 void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum) |
415 void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum) |
403 { |
416 { |
404 printf("EV PDOEventTimerAlarm : %d\n", pdoNum); |
|
405 |
|
406 /* This is needed to avoid deletion of re-attribuated timer */ |
417 /* This is needed to avoid deletion of re-attribuated timer */ |
407 d->PDO_status[pdoNum].event_timer = TIMER_NONE; |
418 d->PDO_status[pdoNum].event_timer = TIMER_NONE; |
408 /* force emission of PDO by artificially changing last emitted*/ |
419 /* force emission of PDO by artificially changing last emitted*/ |
409 d->PDO_status[pdoNum].last_message.cob_id.w = 0; |
420 d->PDO_status[pdoNum].last_message.cob_id.w = 0; |
410 _sendPDOevent( d, 0 ); /* not a Sync Event*/ |
421 _sendPDOevent( d, 0 ); /* not a Sync Event*/ |
411 } |
422 } |
412 |
423 |
413 void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum) |
424 void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum) |
414 { |
425 { |
415 printf("EV PDOInhibitTimerAlarm : %d\n", pdoNum); |
|
416 |
|
417 /* This is needed to avoid deletion of re-attribuated timer */ |
426 /* This is needed to avoid deletion of re-attribuated timer */ |
418 d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE; |
427 d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE; |
419 /* Remove inhibit flag */ |
428 /* Remove inhibit flag */ |
420 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED; |
429 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED; |
421 _sendPDOevent( d, 0 ); /* not a Sync Event*/ |
430 _sendPDOevent( d, 0 ); /* not a Sync Event*/ |
439 UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP; |
448 UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP; |
440 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
449 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
441 |
450 |
442 /* study all PDO stored in the objects dictionary */ |
451 /* study all PDO stored in the objects dictionary */ |
443 if(offsetObjdict){ |
452 if(offsetObjdict){ |
444 Message clean = Message_Initializer; |
|
445 Message pdo = Message_Initializer; |
453 Message pdo = Message_Initializer; |
446 while( offsetObjdict <= lastIndex) { |
454 while( offsetObjdict <= lastIndex) { |
447 switch( status ) { |
455 switch( status ) { |
448 case state3: /* get the PDO transmission type */ |
456 case state3: |
449 if (d->objdict[offsetObjdict].bSubCount <= 2) { |
457 if (/*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5)*/ |
450 MSG_ERR(0x1004, "Subindex 2 not found at index ", 0x1800 + pdoNum); |
458 /* check if TPDO is not valid */ |
451 return 0xFF; |
459 *(UNS32*)d->objdict[offsetObjdict].pSubindex[0].pObject & 0x8000) { |
|
460 MSG_WAR(0x3960, "Not a valid PDO ", 0x1800 + pdoNum); |
|
461 /*Go next TPDO*/ |
|
462 status = state11; |
|
463 break; |
452 } |
464 } |
|
465 /* get the PDO transmission type */ |
453 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject; |
466 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject; |
454 MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum); |
467 MSG_WAR(0x3962, "Reading PDO at index : ", 0x1800 + pdoNum); |
455 |
468 |
456 /* check if transmission type is after (this) SYNC */ |
469 /* check if transmission type is SYNCRONOUS */ |
457 /* The message may not be transmited every SYNC but every n SYNC */ |
470 /* The message is transmited every n SYNC with n=TransmissionType */ |
458 if( isSyncEvent && |
471 if( isSyncEvent && |
459 (*pTransmissionType >= TRANS_SYNC_MIN) && |
472 (*pTransmissionType >= TRANS_SYNC_MIN) && |
460 (*pTransmissionType <= TRANS_SYNC_MAX) && |
473 (*pTransmissionType <= TRANS_SYNC_MAX) && |
461 (++d->PDO_status[pdoNum].transmit_type_parameter == *pTransmissionType) ) { |
474 (++d->PDO_status[pdoNum].transmit_type_parameter == *pTransmissionType) ) { |
|
475 /*Reset count of SYNC*/ |
462 d->PDO_status[pdoNum].transmit_type_parameter = 0; |
476 d->PDO_status[pdoNum].transmit_type_parameter = 0; |
463 MSG_WAR(0x3007, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType); |
477 MSG_WAR(0x3964, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType); |
464 pdo = clean; |
478 pdo = (Message)Message_Initializer; |
465 if(buildPDO(d, pdoNum, &pdo)) |
479 if(buildPDO(d, pdoNum, &pdo)) |
466 { |
480 { |
467 MSG_ERR(0x3006, " Couldn't build TPDO number : ", pdoNum); |
481 MSG_ERR(0x1906, " Couldn't build TPDO number : ", pdoNum); |
468 status = state11; |
482 status = state11; |
469 break; |
483 break; |
470 } |
484 } |
471 status = state5; |
485 status = state5; |
472 } |
486 /* If transmission RTR, with data sampled on SYNC */ |
|
487 }else if( isSyncEvent && |
|
488 (*pTransmissionType == TRANS_RTR_SYNC)) { |
|
489 if(buildPDO(d, pdoNum, &d->PDO_status[pdoNum].last_message)) |
|
490 { |
|
491 MSG_ERR(0x1966, " Couldn't build TPDO number : ", pdoNum); |
|
492 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_RTR_SYNC_READY; |
|
493 }else{ |
|
494 d->PDO_status[pdoNum].transmit_type_parameter |= PDO_RTR_SYNC_READY; |
|
495 } |
|
496 status = state11; |
|
497 break; |
473 /* If transmission on Event and not inhibited, check for changes */ |
498 /* If transmission on Event and not inhibited, check for changes */ |
474 else if((*pTransmissionType == TRANS_EVENT_PROFILE || |
499 }else if((*pTransmissionType == TRANS_EVENT_PROFILE || |
475 *pTransmissionType == TRANS_EVENT_SPECIFIC )&& |
500 *pTransmissionType == TRANS_EVENT_SPECIFIC )&& |
476 !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) { |
501 !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) { |
477 MSG_WAR(0x3008, " PDO is on EVENT. Trans type : ", *pTransmissionType); |
502 MSG_WAR(0x3968, " PDO is on EVENT. Trans type : ", *pTransmissionType); |
478 pdo = pdo = clean; |
503 pdo = (Message)Message_Initializer; |
479 if(buildPDO(d, pdoNum, &pdo)) |
504 if(buildPDO(d, pdoNum, &pdo)) |
480 { |
505 { |
481 MSG_ERR(0x3007, " Couldn't build TPDO number : ", pdoNum); |
506 MSG_ERR(0x3907, " Couldn't build TPDO number : ", pdoNum); |
482 status = state11; |
507 status = state11; |
483 break; |
508 break; |
484 } |
509 } |
485 |
510 |
486 /*Compare new and old PDO*/ |
511 /*Compare new and old PDO*/ |
504 |
528 |
505 /* and inhibit TPDO */ |
529 /* and inhibit TPDO */ |
506 d->PDO_status[pdoNum].transmit_type_parameter |= PDO_INHIBITED; |
530 d->PDO_status[pdoNum].transmit_type_parameter |= PDO_INHIBITED; |
507 } |
531 } |
508 }else{ |
532 }else{ |
509 MSG_WAR(0x3009, " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", *pTransmissionType); |
533 MSG_WAR(0x306C, " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", *pTransmissionType); |
510 status = state11; |
534 status = state11; |
511 } |
535 } |
512 break; |
536 break; |
513 case state5: /*Send the pdo*/ |
537 case state5: /*Send the pdo*/ |
514 /*store_as_last_message*/ |
538 /*store_as_last_message*/ |
515 d->PDO_status[pdoNum].last_message = pdo; |
539 d->PDO_status[pdoNum].last_message = pdo; |
516 MSG_WAR(0x3901, "sendPDO cobId :", pdo.cob_id.w); |
540 MSG_WAR(0x396D, "sendPDO cobId :", pdo.cob_id.w); |
517 MSG_WAR(0x3902, " Nb octets : ", pdo.len); |
541 MSG_WAR(0x396E, " 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 |
542 |
523 canSend(d->canHandle,&pdo); |
543 canSend(d->canHandle,&pdo); |
524 status = state11; |
544 status = state11; |
525 break; |
545 break; |
526 case state11: /*Go to next TPDO*/ |
546 case state11: /*Go to next TPDO*/ |
527 pdoNum++; |
547 pdoNum++; |
528 offsetObjdict++; |
548 offsetObjdict++; |
529 offsetObjdictMap++; |
549 offsetObjdictMap++; |
530 MSG_WAR(0x3017, "next pdo index : ", pdoNum); |
550 MSG_WAR(0x3970, "next pdo index : ", pdoNum); |
531 status = state3; |
551 status = state3; |
532 break; |
552 break; |
533 |
553 |
534 default: |
554 default: |
535 MSG_ERR(0x1019,"Unknown state has been reached : %d",status); |
555 MSG_ERR(0x1972,"Unknown state has been reached : %d",status); |
536 return 0xFF; |
556 return 0xFF; |
537 }/* end switch case */ |
557 }/* end switch case */ |
538 |
558 |
539 }/* end while */ |
559 }/* end while */ |
540 } |
560 } |
541 return 0; |
561 return 0; |
542 } |
562 } |
543 |
563 |
|
564 /*! |
|
565 ** |
|
566 ** |
|
567 ** @param d |
|
568 ** @param OD_entry |
|
569 ** @param bSubindex |
|
570 ** @return always 0 |
|
571 **/ |
|
572 |
|
573 UNS32 TPDO_Communication_Parameter_Callback(CO_Data* d, const indextable * OD_entry, UNS8 bSubindex) |
|
574 { |
|
575 /* If PDO are actives */ |
|
576 if(d->CurrentCommunicationState.csPDO) switch(bSubindex) |
|
577 { |
|
578 case 2: /* Changed transmition type */ |
|
579 case 3: /* Changed inhibit time */ |
|
580 case 5: /* Changed event time */ |
|
581 { |
|
582 UNS8 pTransmissionType = *(UNS8*) OD_entry->pSubindex[2].pObject; |
|
583 const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS; |
|
584 UNS8 numPdo = OD_entry - TPDO_com; /* number of the actual processed pdo-nr. */ |
|
585 |
|
586 /* Zap all timers and inhibit flag */ |
|
587 d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer); |
|
588 d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer); |
|
589 d->PDO_status[numPdo].transmit_type_parameter = 0; |
|
590 /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */ |
|
591 PDOEventTimerAlarm(d, numPdo); |
|
592 return 0; |
|
593 } |
|
594 |
|
595 default: /* other subindex are ignored*/ |
|
596 break; |
|
597 } |
|
598 return 0; |
|
599 } |
|
600 |
544 void PDOInit(CO_Data* d) |
601 void PDOInit(CO_Data* d) |
545 { |
602 { |
546 |
603 /* For each TPDO mapping parameters */ |
547 /* TODO: implement callbacks on 140xh |
604 UNS16 pdoIndex = 0x1800; /* OD index of TDPO */ |
548 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
605 |
549 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
606 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
550 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
607 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
551 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
608 if(offsetObjdict) while( offsetObjdict <= lastIndex) { |
552 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
609 /* Assign callbacks to sensible TPDO mapping subindexes */ |
553 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
610 UNS32 errorCode; |
554 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
611 ODCallback_t *CallbackList; |
555 * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx |
612 /* Find callback list */ |
556 */ |
613 scanIndexOD (d, pdoIndex, &errorCode, &CallbackList); |
557 |
614 if(errorCode == OD_SUCCESSFUL && CallbackList) |
558 _sendPDOevent(d, 0 ); |
615 { |
|
616 /*Assign callbacks to corresponding subindex*/ |
|
617 /* Transmission type */ |
|
618 CallbackList[2] = &TPDO_Communication_Parameter_Callback; |
|
619 /* Inhibit time */ |
|
620 CallbackList[3] = &TPDO_Communication_Parameter_Callback; |
|
621 /* Event timer */ |
|
622 CallbackList[5] = &TPDO_Communication_Parameter_Callback; |
|
623 } |
|
624 pdoIndex++; |
|
625 offsetObjdict++; |
|
626 } |
|
627 |
|
628 /* Trigger a non-sync event */ |
|
629 _sendPDOevent( d, 0 ); |
559 } |
630 } |
560 |
631 |
561 void PDOStop(CO_Data* d) |
632 void PDOStop(CO_Data* d) |
562 { |
633 { |
|
634 /* For each TPDO mapping parameters */ |
563 UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */ |
635 UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */ |
564 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
636 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
565 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
637 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
566 if(offsetObjdict) while( offsetObjdict <= lastIndex) { |
638 if(offsetObjdict) while( offsetObjdict <= lastIndex) { |
|
639 /* Delete TPDO timers */ |
567 d->PDO_status[pdoNum].event_timer = DelAlarm(d->PDO_status[pdoNum].event_timer); |
640 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); |
641 d->PDO_status[pdoNum].inhibit_timer = DelAlarm(d->PDO_status[pdoNum].inhibit_timer); |
|
642 /* Reset transmit type parameter */ |
569 d->PDO_status[pdoNum].transmit_type_parameter = 0; |
643 d->PDO_status[pdoNum].transmit_type_parameter = 0; |
570 d->PDO_status[pdoNum].last_message.cob_id.w = 0; |
644 d->PDO_status[pdoNum].last_message.cob_id.w = 0; |
571 pdoNum++; |
645 pdoNum++; |
572 offsetObjdict++; |
646 offsetObjdict++; |
573 } |
647 } |