491 { |
507 { |
492 /* Calls _sendPDOevent specifying it is not a sync event */ |
508 /* Calls _sendPDOevent specifying it is not a sync event */ |
493 return _sendPDOevent (d, 0); |
509 return _sendPDOevent (d, 0); |
494 } |
510 } |
495 |
511 |
|
512 UNS8 |
|
513 sendOnePDOevent (CO_Data * d, UNS32 pdoNum) |
|
514 { |
|
515 if (!d->CurrentCommunicationState.csPDO || |
|
516 !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) |
|
517 { |
|
518 return 0; |
|
519 } |
|
520 |
|
521 UNS16 offsetObjdict = d->firstIndex->PDO_TRS + pdoNum; |
|
522 MSG_WAR (0x3968, " PDO is on EVENT. Trans type : ", |
|
523 *pTransmissionType); |
|
524 Message pdo; |
|
525 memset(&pdo, 0, sizeof(pdo)); |
|
526 if (buildPDO (d, pdoNum, &pdo)) |
|
527 { |
|
528 MSG_ERR (0x3907, " Couldn't build TPDO number : ", |
|
529 pdoNum); |
|
530 return; |
|
531 } |
|
532 |
|
533 /*Compare new and old PDO */ |
|
534 if (d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id |
|
535 && d->PDO_status[pdoNum].last_message.len == pdo.len |
|
536 && memcmp(d->PDO_status[pdoNum].last_message.data, |
|
537 pdo.data, 8) == 0 |
|
538 ) |
|
539 { |
|
540 /* No changes -> go to next pdo */ |
|
541 return; |
|
542 } |
|
543 else |
|
544 { |
|
545 |
|
546 TIMEVAL EventTimerDuration; |
|
547 TIMEVAL InhibitTimerDuration; |
|
548 |
|
549 MSG_WAR (0x306A, "Changes TPDO number : ", pdoNum); |
|
550 /* Changes detected -> transmit message */ |
|
551 EventTimerDuration = |
|
552 *(UNS16 *) d->objdict[offsetObjdict].pSubindex[5]. |
|
553 pObject; |
|
554 InhibitTimerDuration = |
|
555 *(UNS16 *) d->objdict[offsetObjdict].pSubindex[3]. |
|
556 pObject; |
|
557 |
|
558 /* Start both event_timer and inhibit_timer */ |
|
559 if (EventTimerDuration) |
|
560 { |
|
561 DelAlarm (d->PDO_status[pdoNum].event_timer); |
|
562 d->PDO_status[pdoNum].event_timer = |
|
563 SetAlarm (d, pdoNum, &PDOEventTimerAlarm, |
|
564 MS_TO_TIMEVAL (EventTimerDuration), 0); |
|
565 } |
|
566 |
|
567 if (InhibitTimerDuration) |
|
568 { |
|
569 DelAlarm (d->PDO_status[pdoNum].inhibit_timer); |
|
570 d->PDO_status[pdoNum].inhibit_timer = |
|
571 SetAlarm (d, pdoNum, &PDOInhibitTimerAlarm, |
|
572 US_TO_TIMEVAL (InhibitTimerDuration * |
|
573 100), 0); |
|
574 /* and inhibit TPDO */ |
|
575 d->PDO_status[pdoNum].transmit_type_parameter |= |
|
576 PDO_INHIBITED; |
|
577 } |
|
578 |
|
579 sendPdo(d, pdoNum, &pdo); |
|
580 } |
|
581 } |
496 |
582 |
497 void |
583 void |
498 PDOEventTimerAlarm (CO_Data * d, UNS32 pdoNum) |
584 PDOEventTimerAlarm (CO_Data * d, UNS32 pdoNum) |
499 { |
585 { |
500 /* This is needed to avoid deletion of re-attribuated timer */ |
586 /* This is needed to avoid deletion of re-attribuated timer */ |
501 d->PDO_status[pdoNum].event_timer = TIMER_NONE; |
587 d->PDO_status[pdoNum].event_timer = TIMER_NONE; |
502 /* force emission of PDO by artificially changing last emitted */ |
588 /* force emission of PDO by artificially changing last emitted */ |
503 d->PDO_status[pdoNum].last_message.cob_id = 0; |
589 d->PDO_status[pdoNum].last_message.cob_id = 0; |
504 _sendPDOevent (d, 0); /* not a Sync Event */ |
590 sendOnePDOevent (d, pdoNum); |
505 } |
591 } |
506 |
592 |
507 void |
593 void |
508 PDOInhibitTimerAlarm (CO_Data * d, UNS32 pdoNum) |
594 PDOInhibitTimerAlarm (CO_Data * d, UNS32 pdoNum) |
509 { |
595 { |
510 /* This is needed to avoid deletion of re-attribuated timer */ |
596 /* This is needed to avoid deletion of re-attribuated timer */ |
511 d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE; |
597 d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE; |
512 /* Remove inhibit flag */ |
598 /* Remove inhibit flag */ |
513 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED; |
599 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED; |
514 _sendPDOevent (d, 0); /* not a Sync Event */ |
600 sendOnePDOevent (d, pdoNum); |
515 } |
601 } |
516 |
602 |
517 /*! |
603 /*! |
518 ** |
604 ** |
519 ** |
605 ** |
604 status = state11; |
696 status = state11; |
605 break; |
697 break; |
606 /* If transmission on Event and not inhibited, check for changes */ |
698 /* If transmission on Event and not inhibited, check for changes */ |
607 } |
699 } |
608 else |
700 else |
609 if ((isSyncEvent |
701 if ( (isSyncEvent && (*pTransmissionType == TRANS_SYNC_ACYCLIC)) |
610 && (*pTransmissionType == TRANS_SYNC_ACYCLIC)) |
702 || |
611 || |
703 (!isSyncEvent && (*pTransmissionType == TRANS_EVENT_PROFILE || *pTransmissionType == TRANS_EVENT_SPECIFIC) |
612 ((*pTransmissionType == TRANS_EVENT_PROFILE |
704 && !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED))) |
613 || *pTransmissionType == TRANS_EVENT_SPECIFIC) |
|
614 && !(d->PDO_status[pdoNum]. |
|
615 transmit_type_parameter & PDO_INHIBITED))) |
|
616 { |
705 { |
617 MSG_WAR (0x3968, " PDO is on EVENT. Trans type : ", |
706 sendOnePDOevent(d, pdoNum); |
618 *pTransmissionType); |
707 status = state11; |
619 memset(&pdo, 0, sizeof(pdo)); |
|
620 /*{ |
|
621 Message msg_init = Message_Initializer; |
|
622 pdo = msg_init; |
|
623 }*/ |
|
624 if (buildPDO (d, pdoNum, &pdo)) |
|
625 { |
|
626 MSG_ERR (0x3907, " Couldn't build TPDO number : ", |
|
627 pdoNum); |
|
628 status = state11; |
|
629 break; |
|
630 } |
|
631 |
|
632 /*Compare new and old PDO */ |
|
633 if (d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id |
|
634 && d->PDO_status[pdoNum].last_message.len == pdo.len |
|
635 && memcmp(d->PDO_status[pdoNum].last_message.data, |
|
636 pdo.data, 8) == 0 |
|
637 ) |
|
638 { |
|
639 /* No changes -> go to next pdo */ |
|
640 status = state11; |
|
641 } |
|
642 else |
|
643 { |
|
644 |
|
645 TIMEVAL EventTimerDuration; |
|
646 TIMEVAL InhibitTimerDuration; |
|
647 |
|
648 MSG_WAR (0x306A, "Changes TPDO number : ", pdoNum); |
|
649 /* Changes detected -> transmit message */ |
|
650 EventTimerDuration = |
|
651 *(UNS16 *) d->objdict[offsetObjdict].pSubindex[5]. |
|
652 pObject; |
|
653 InhibitTimerDuration = |
|
654 *(UNS16 *) d->objdict[offsetObjdict].pSubindex[3]. |
|
655 pObject; |
|
656 |
|
657 status = state5; |
|
658 |
|
659 /* Start both event_timer and inhibit_timer */ |
|
660 if (EventTimerDuration) |
|
661 { |
|
662 DelAlarm (d->PDO_status[pdoNum].event_timer); |
|
663 d->PDO_status[pdoNum].event_timer = |
|
664 SetAlarm (d, pdoNum, &PDOEventTimerAlarm, |
|
665 MS_TO_TIMEVAL (EventTimerDuration), 0); |
|
666 } |
|
667 |
|
668 if (InhibitTimerDuration) |
|
669 { |
|
670 DelAlarm (d->PDO_status[pdoNum].inhibit_timer); |
|
671 d->PDO_status[pdoNum].inhibit_timer = |
|
672 SetAlarm (d, pdoNum, &PDOInhibitTimerAlarm, |
|
673 US_TO_TIMEVAL (InhibitTimerDuration * |
|
674 100), 0); |
|
675 /* and inhibit TPDO */ |
|
676 d->PDO_status[pdoNum].transmit_type_parameter |= |
|
677 PDO_INHIBITED; |
|
678 } |
|
679 |
|
680 } |
|
681 } |
708 } |
682 else |
709 else |
683 { |
710 { |
684 MSG_WAR (0x306C, |
711 MSG_WAR (0x306C, |
685 " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", |
712 " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", |
686 *pTransmissionType); |
713 *pTransmissionType); |
687 status = state11; |
714 status = state11; |
688 } |
715 } |
689 break; |
716 break; |
690 case state5: /*Send the pdo */ |
717 case state5: /*Send the pdo */ |
691 /*store_as_last_message */ |
718 sendPdo(d, pdoNum, &pdo); |
692 d->PDO_status[pdoNum].last_message = pdo; |
|
693 MSG_WAR (0x396D, "sendPDO cobId :", UNS16_LE(pdo.cob_id)); |
|
694 MSG_WAR (0x396E, " Nb octets : ", pdo.len); |
|
695 |
|
696 canSend (d->canHandle, &pdo); |
|
697 status = state11; |
719 status = state11; |
698 break; |
720 break; |
699 case state11: /*Go to next TPDO */ |
721 case state11: /*Go to next TPDO */ |
700 pdoNum++; |
722 pdoNum++; |
701 offsetObjdict++; |
723 offsetObjdict++; |