src/pdo.c
changeset 236 905677ed00f3
parent 235 f812bf6b7237
child 252 b0dd37421d28
equal deleted inserted replaced
235:f812bf6b7237 236:905677ed00f3
    52 	
    52 	
    53 	UNS8 prp_j = 0x00;
    53 	UNS8 prp_j = 0x00;
    54 	UNS8 offset = 0x00;
    54 	UNS8 offset = 0x00;
    55 	const UNS8* pMappingCount = (UNS8*) TPDO_map->pSubindex[0].pObject;
    55 	const UNS8* pMappingCount = (UNS8*) TPDO_map->pSubindex[0].pObject;
    56 
    56 
    57 	pdo->cob_id.w = *(UNS32*)TPDO_com->pSubindex[1].pObject;
    57 	pdo->cob_id.w = *(UNS32*)TPDO_com->pSubindex[1].pObject & 0x7FF;
    58 	pdo->rtr = NOT_A_REQUEST;
    58 	pdo->rtr = NOT_A_REQUEST;
    59 
    59 
    60 	MSG_WAR(0x3009, "  PDO CobId is : ", *(UNS32*)TPDO_com->pSubindex[1].pObject);
    60 	MSG_WAR(0x3009, "  PDO CobId is : ", *(UNS32*)TPDO_com->pSubindex[1].pObject);
    61 	MSG_WAR(0x300D, "  Number of objects mapped : ",*pMappingCount );
    61 	MSG_WAR(0x300D, "  Number of objects mapped : ",*pMappingCount );
    62 	
    62 	
    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 */
   309       case state5:/* build and send requested PDO */
   322       case state5:/* build and send requested PDO */
   310       {
   323       {
   311       	Message pdo;
   324       	Message pdo;
   312     	if( buildPDO(d, numPdo, &pdo))
   325     	if( buildPDO(d, numPdo, &pdo))
   313     	{
   326     	{
   314           MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
   327           MSG_ERR(0x1948, " Couldn't build TPDO n°", numPdo);
   315           //return 0xFF; /*No real reason to stop...*/
   328           return 0xFF;
   316     	}
   329     	}
   317     	canSend(d->canHandle,&pdo);
   330     	canSend(d->canHandle,&pdo);
   318         return 0;
   331         return 0;
   319       }
   332       }
   320       }/* end switch status */
   333       }/* end switch status */
   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*/
   488 	   d->PDO_status[pdoNum].last_message.len == pdo.len &&
   513 	   d->PDO_status[pdoNum].last_message.len == pdo.len &&
   489 	   *(UNS64*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS64*)(&pdo.data[0])){
   514 	   *(UNS64*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS64*)(&pdo.data[0])){
   490 	   	/* No changes -> go to next pdo*/
   515 	   	/* No changes -> go to next pdo*/
   491 		status = state11;
   516 		status = state11;
   492 	}else{
   517 	}else{
   493 		MSG_WAR(0x3008, "Changes TPDO number : ", pdoNum);
   518 		MSG_WAR(0x306A, "Changes TPDO number : ", pdoNum);
   494 		printf("EV Changes TPDO number : %d\n", pdoNum);
       
   495 		/* Changes detected -> transmit message */
   519 		/* Changes detected -> transmit message */
   496 		status = state5;
   520 		status = state5;
   497 		
   521 		
   498 		/* Start both event_timer and inhibit_timer*/
   522 		/* Start both event_timer and inhibit_timer*/
   499 		DelAlarm(d->PDO_status[pdoNum].event_timer);
   523 		DelAlarm(d->PDO_status[pdoNum].event_timer);
   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   }