etisserant@0: /*
nico@208:   This file is part of CanFestival, a library implementing CanOpen
nico@208:   Stack.
nico@208: 
nico@208:   Copyright (C): Edouard TISSERANT and Francis DUPIN
nico@208: 
nico@208:   See COPYING file for copyrights details.
nico@208: 
nico@208:   This library is free software; you can redistribute it and/or
nico@208:   modify it under the terms of the GNU Lesser General Public
nico@208:   License as published by the Free Software Foundation; either
nico@208:   version 2.1 of the License, or (at your option) any later version.
nico@208: 
nico@208:   This library is distributed in the hope that it will be useful,
nico@208:   but WITHOUT ANY WARRANTY; without even the implied warranty of
nico@208:   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
nico@208:   Lesser General Public License for more details.
nico@208: 
nico@208:   You should have received a copy of the GNU Lesser General Public
nico@208:   License along with this library; if not, write to the Free Software
nico@208:   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
nico@208:   USA
etisserant@0: */
etisserant@0: #include "pdo.h"
etisserant@0: #include "objacces.h"
etisserant@149: #include "canfestival.h"
etisserant@0: 
nico@208: /*!
nico@208: ** @file   pdo.c
nico@208: ** @author Edouard TISSERANT and Francis DUPIN
nico@208: ** @date   Tue Jun  5 09:32:32 2007
nico@208: **
nico@208: ** @brief
nico@208: **
nico@208: **
nico@208: */
etisserant@235: 
etisserant@235: /*!
etisserant@235: **
nico@208: **
nico@208: ** @param d
etisserant@235: ** @param TPDO_com TPDO communication parameters OD entry
etisserant@235: ** @param TPDO_map TPDO mapping parameters OD entry
nico@208: **
nico@208: ** @return
nico@208: **/
etisserant@235: 
etisserant@235: UNS8 buildPDO(CO_Data* d, UNS8 numPdo, Message *pdo)
etisserant@235: {
etisserant@235: 	const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo; 
etisserant@235: 	const indextable* TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;
etisserant@235: 	
etisserant@235: 	UNS8 prp_j = 0x00;
etisserant@235: 	UNS8 offset = 0x00;
etisserant@235: 	const UNS8* pMappingCount = (UNS8*) TPDO_map->pSubindex[0].pObject;
etisserant@235: 
etisserant@236: 	pdo->cob_id.w = *(UNS32*)TPDO_com->pSubindex[1].pObject & 0x7FF;
etisserant@235: 	pdo->rtr = NOT_A_REQUEST;
etisserant@235: 
etisserant@235: 	MSG_WAR(0x3009, "  PDO CobId is : ", *(UNS32*)TPDO_com->pSubindex[1].pObject);
etisserant@235: 	MSG_WAR(0x300D, "  Number of objects mapped : ",*pMappingCount );
etisserant@235: 	
etisserant@235: 	do{
etisserant@235: 		UNS8 dataType; /* Unused */
etisserant@235: 		UNS8 tmp[]= {0,0,0,0,0,0,0,0}; /* temporary space to hold bits */
etisserant@235: 
etisserant@235: 		/* pointer fo the var which holds the mapping parameter of an mapping entry  */
etisserant@235: 		UNS32* pMappingParameter = (UNS32*) TPDO_map->pSubindex[prp_j + 1].pObject;
etisserant@235: 		UNS16 index = (UNS16)((*pMappingParameter) >> 16);
etisserant@235: 		UNS8 Size = (UNS8)(*pMappingParameter); /* Size in bits */
etisserant@235: 		
luis@350: 		/* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */
luis@350: 		if(Size && ((offset + Size) <= 64)) {
luis@350: 			UNS8 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
luis@350: 			UNS8 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
luis@350: 			
luis@350: 			MSG_WAR(0x300F, "  got mapping parameter : ", *pMappingParameter);
luis@350: 			MSG_WAR(0x3050, "    at index : ", TPDO_map->index);
luis@350: 			MSG_WAR(0x3051, "    sub-index : ", prp_j + 1);
luis@350: 			
luis@350: 			if( getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ) != OD_SUCCESSFUL ){
luis@350: 				MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
luis@350: 				return 0xFF;
luis@350: 			}
luis@350: 			/* copy bit per bit in little endian*/
luis@350: 			CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&pdo->data[offset>>3], offset%8, 0);
luis@350: 
luis@350: 			offset += Size ;
etisserant@235: 		}
etisserant@235: 		prp_j++;
etisserant@235: 	}while( prp_j < *pMappingCount );
etisserant@235: 
etisserant@235: 	pdo->len = 1 + ((offset - 1) >> 3);
etisserant@235: 
etisserant@235: 	MSG_WAR(0x3015, "  End scan mapped variable", 0);
etisserant@235: 
etisserant@235: 	return 0;
etisserant@235: }
nico@208: 
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: ** @param cobId
nico@208: **
nico@208: ** @return
nico@208: **/
etisserant@236: UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex )
nico@208: {
nico@208:   UNS32 * pwCobId;
etisserant@236:   UNS16 offset = d->firstIndex->PDO_RCV;
etisserant@236:   UNS16 lastIndex = d->lastIndex->PDO_RCV;
etisserant@236: 
nico@215:   /* Sending the request only if the cobid have been found on the PDO
nico@208:      receive */
nico@215:   /* part dictionary */
etisserant@236:   
etisserant@236:   MSG_WAR(0x3930, "sendPDOrequest RPDO Index : ",RPDOIndex);
etisserant@236:   
etisserant@236:   if (offset && RPDOIndex >= 0x1400){
etisserant@236:     offset += RPDOIndex - 0x1400;  
etisserant@236:     if (offset <= lastIndex) {
nico@215:       /* get the CobId*/
etisserant@217:       pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject;
nico@208: 
etisserant@236:       MSG_WAR(0x3930, "sendPDOrequest cobId is : ",*pwCobId);
leonid@252:       {
etisserant@285:           Message pdo;
etisserant@285:           pdo.cob_id.w = *pwCobId;
etisserant@285:           pdo.rtr = REQUEST;
etisserant@285:           pdo.len = 0;      
etisserant@285:           return canSend(d->canHandle,&pdo);
leonid@252:       }
nico@208:     }
etisserant@236:   }
etisserant@236:   MSG_ERR(0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex);
etisserant@0:   return 0xFF;
etisserant@0: }
etisserant@0: 
etisserant@0: 
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: ** @param m
nico@208: **
nico@208: ** @return
nico@208: **/
etisserant@0: UNS8 proceedPDO(CO_Data* d, Message *m)
nico@208: {
etisserant@0:   UNS8   numPdo;
nico@215:   UNS8   numMap;  /* Number of the mapped varable */
nico@215:   UNS8 *     pMappingCount = NULL;    /* count of mapped objects... */
nico@215:   /* pointer to the var which is mapped to a pdo... */
nico@215:   /*  void *     pMappedAppObject = NULL;   */
nico@215:   /* pointer fo the var which holds the mapping parameter of an
nico@208:      mapping entry */
nico@208:   UNS32 *    pMappingParameter = NULL;
nico@215:   UNS8  *    pTransmissionType = NULL; /* pointer to the transmission
nico@208:                                          type */
etisserant@0:   UNS32 *    pwCobId = NULL;
etisserant@0:   UNS8       Size;
etisserant@0:   UNS8       offset;
etisserant@0:   UNS8       status;
etisserant@0:   UNS32      objDict;
etisserant@0:   UNS16      offsetObjdict;
etisserant@0:   UNS16      lastIndex;
etisserant@235:   
etisserant@235:   status = state2;
nico@208: 
nico@208:   MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));
etisserant@0:   offset = 0x00;
etisserant@0:   numPdo = 0;
etisserant@0:   numMap = 0;
nico@215:   if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a
nico@208:                                      request. */
etisserant@235: 
etisserant@0:     offsetObjdict = d->firstIndex->PDO_RCV;
etisserant@0:     lastIndex = d->lastIndex->PDO_RCV;
nico@208: 
nico@215:     /* study of all the PDO stored in the dictionary */
etisserant@0:     if(offsetObjdict)
nico@208:       while (offsetObjdict <= lastIndex) {
nico@208: 
nico@208:         switch( status ) {
nico@208: 
nico@208:         case state2:
nico@215:           /* get CobId of the dictionary correspondant to the received
nico@208:              PDO */
etisserant@217:           pwCobId = (UNS32*) d->objdict[offsetObjdict].pSubindex[1].pObject;
nico@215:           /* check the CobId coherance */
nico@215:           /*pwCobId is the cobId read in the dictionary at the state 3
nico@208:             */
nico@208:           if ( *pwCobId == (*m).cob_id.w ){
nico@215:             /* The cobId is recognized */
nico@208:             status = state4;
nico@208:             MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
nico@208:             break;
nico@208:           }
nico@208:           else {
nico@215:             /* cobId received does not match with those write in the
nico@208:               dictionnary */
nico@208:             numPdo++;
nico@208:             offsetObjdict++;
nico@208:             status = state2;
nico@208:             break;
nico@208:           }
nico@208: 
nico@215:             case state4:/* Get Mapped Objects Number */
nico@215:                /* The cobId of the message received has been found in the
nico@208:                  dictionnary. */
nico@208:                offsetObjdict = d->firstIndex->PDO_RCV_MAP;
nico@208:              lastIndex = d->lastIndex->PDO_RCV_MAP;
etisserant@217:              pMappingCount = (UNS8*) (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
nico@208:              numMap = 0;
nico@208:              while (numMap < *pMappingCount) {
nico@208:                UNS8 tmp[]= {0,0,0,0,0,0,0,0};
nico@208:                UNS8 ByteSize;
etisserant@217:                pMappingParameter = (UNS32*) (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
nico@208:                if (pMappingParameter == NULL) {
nico@208:                  MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
nico@208:                  return 0xFF;
nico@208:                }
nico@215:                /* Get the addresse of the mapped variable. */
nico@215:                /* detail of *pMappingParameter : */
nico@215:                /* The 16 hight bits contains the index, the medium 8 bits
nico@208:                  contains the subindex, */
nico@215:                /* and the lower 8 bits contains the size of the mapped
nico@208:                  variable. */
nico@208: 
nico@208:                Size = (UNS8)(*pMappingParameter);
luis@350: 					
luis@350: 					/* set variable only if Size != 0 and Size is lower than remaining bits in the PDO */
luis@350:                if(Size && ((offset + Size) <= (m->len << 3))) {
luis@350:                  /* copy bit per bit in little endian */
luis@350:                  CopyBits(Size, (UNS8*)&m->data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
luis@350: 
luis@350:                  ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 =>
luis@350:                                                      2, ... */
luis@350: 
luis@350:                  objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16),
luis@350:                                       (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),
luis@350:                                    tmp, &ByteSize, 0 );
luis@350: 
luis@350:                  if(objDict != OD_SUCCESSFUL) {
luis@350:                    MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);
luis@350:                    MSG_WAR(0x2939, "         Mapped at index : ", (*pMappingParameter) >> 16);
luis@350:                    MSG_WAR(0x2940, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
luis@350:                    return 0xFF;
luis@350:                  }
luis@350: 
luis@350:                  MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w);
luis@350:                  MSG_WAR(0x3943, "         Mapped at index : ", (*pMappingParameter) >> 16);
luis@350:                  MSG_WAR(0x3944, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
luis@350:                  /* MSG_WAR(0x3945, "                data : ",*((UNS32*)pMappedAppObject)); */
luis@350:                  offset += Size;
luis@350: 					}
nico@208:                numMap++;
nico@215:              } /* end loop while on mapped variables */
nico@208: 
nico@208:              offset=0x00;
nico@208:              numMap = 0;
nico@208:              return 0;
nico@208: 
nico@215:         }/* end switch status*/
nico@215:       }/* end while*/
nico@215:   }/* end if Donnees */
nico@208:   else if ((*m).rtr == REQUEST ){
nico@208:     MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);
nico@208:     status = state1;
nico@208:     offsetObjdict = d->firstIndex->PDO_TRS;
nico@208:     lastIndex = d->lastIndex->PDO_TRS;
nico@208:     if(offsetObjdict) while( offsetObjdict  <= lastIndex ){
nico@215:       /* study of all PDO stored in the objects dictionary */
nico@208: 
nico@208:       switch( status ){
nico@208: 
nico@215:       case state1:/* check the CobId */
nico@215:         /* get CobId of the dictionary which match to the received PDO
nico@208:          */
etisserant@217:         pwCobId = (UNS32*) (d->objdict + offsetObjdict)->pSubindex[1].pObject;
nico@208:         if ( *pwCobId == (*m).cob_id.w ) {
nico@208:           status = state4;
nico@208:           break;
nico@208:         }
nico@208:         else {
nico@208:           numPdo++;
nico@208:           offsetObjdict++;
nico@208:         }
nico@208:         status = state1;
nico@208:         break;
nico@208: 
nico@208: 
etisserant@236:       case state4:/* check transmission type */
etisserant@217:         pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject;
etisserant@236:         /* If PDO is to be sampled and send on RTR, do it*/
etisserant@236:         if ( (*pTransmissionType == TRANS_RTR)) {
nico@208:           status = state5;
nico@208:           break;
etisserant@236:         /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */
etisserant@236:         }else if ((*pTransmissionType == TRANS_RTR_SYNC )) {
etisserant@236:     	  if(d->PDO_status[numPdo].transmit_type_parameter & PDO_RTR_SYNC_READY){
etisserant@236:             /*Data ready, just send*/
etisserant@236:     	    canSend(d->canHandle,&d->PDO_status[numPdo].last_message);
etisserant@236:             return 0;
etisserant@236:     	  }else{
etisserant@236:             /* if SYNC did never occur, force emission with current data */
etisserant@236:             /* DS301 do not tell what to do in such a case...*/
etisserant@236:             MSG_ERR(0x1947, "Not ready RTR_SYNC TPDO send current data : ", m->cob_id.w);
etisserant@236:             status = state5;
etisserant@236:     	  }
etisserant@236:           break;
etisserant@235:         }else if(
etisserant@235:              (*pTransmissionType == TRANS_EVENT_PROFILE) ||
etisserant@235:              (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) {
etisserant@235: 	  /* Zap all timers and inhibit flag */
etisserant@235: 	  d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer);
etisserant@235: 	  d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer);
etisserant@235:   	  d->PDO_status[numPdo].transmit_type_parameter &= ~PDO_INHIBITED;
etisserant@235:   	  /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
etisserant@235:           PDOEventTimerAlarm(d, numPdo);
etisserant@235:           return 0;
etisserant@235:         }else {
nico@215:           /* The requested PDO is not to send on request. So, does
nico@208:             nothing. */
nico@208:           MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);
nico@208:           return 0xFF;
nico@208:         }
nico@208: 
etisserant@235:       case state5:/* build and send requested PDO */
etisserant@235:       {
etisserant@235:       	Message pdo;
etisserant@235:     	if( buildPDO(d, numPdo, &pdo))
etisserant@235:     	{
etisserant@236:           MSG_ERR(0x1948, " Couldn't build TPDO n°", numPdo);
etisserant@236:           return 0xFF;
etisserant@235:     	}
etisserant@235:     	canSend(d->canHandle,&pdo);
nico@208:         return 0;
etisserant@235:       }
nico@215:       }/* end switch status */
nico@215:     }/* end while */
nico@215:   }/* end if Requete */
nico@208: 
etisserant@0:   return 0;
etisserant@0: }
etisserant@0: 
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param NbBits
nico@208: ** @param SrcByteIndex
nico@208: ** @param SrcBitIndex
nico@208: ** @param SrcBigEndian
nico@208: ** @param DestByteIndex
nico@208: ** @param DestBitIndex
nico@208: ** @param DestBigEndian
nico@208: **/
greg@201: void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)
greg@201: {
nico@215:   /* This loop copy as many bits that it can each time, crossing*/
nico@215:   /* successively bytes*/
nico@208:   // boundaries from LSB to MSB.
nico@208:   while(NbBits > 0)
nico@208:     {
nico@215:       /* Bit missalignement between src and dest*/
nico@208:       INTEGER8 Vect = DestBitIndex - SrcBitIndex;
nico@208: 
nico@215:       /* We can now get src and align it to dest*/
nico@208:       UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
nico@208: 
nico@215:       /* Compute the nb of bit we will be able to copy*/
nico@208:       UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex :  8 - SrcBitIndex );
nico@208:       UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
nico@208: 
nico@215:       /* Create a mask that will serve in:*/
nico@208:       UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
nico@208: 
nico@215:       /* - Filtering src*/
nico@208:       UNS8 Filtered = Aligned & ~Mask;
nico@208: 
nico@215:       /* - and erase bits where we write, preserve where we don't*/
nico@208:       *DestByteIndex &= Mask;
nico@208: 
nico@215:       /* Then write.*/
nico@208:       *DestByteIndex |= Filtered ;
nico@208: 
nico@215:       /*Compute next time cursors for src*/
nico@215:       if((SrcBitIndex += BitsToCopy)>7)/* cross boundary ?*/
nico@208:         {
nico@215:           SrcBitIndex = 0;/* First bit*/
nico@215:           SrcByteIndex += (SrcBigEndian ? -1 : 1);/* Next byte*/
nico@215:         }
nico@215: 
nico@215: 
nico@215:       /*Compute next time cursors for dest*/
nico@208:       if((DestBitIndex += BitsToCopy)>7)
nico@208:         {
nico@215:           DestBitIndex = 0;/* First bit*/
nico@215:           DestByteIndex += (DestBigEndian ? -1 : 1);/* Next byte*/
nico@215:         }
nico@215: 
nico@215:       /*And decrement counter.*/
nico@208:       NbBits -= BitsToCopy;
nico@208:     }
greg@201: 
greg@201: }
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: **
nico@208: ** @return
nico@208: **/
etisserant@235: 
etisserant@235: UNS8 sendPDOevent( CO_Data* d)
etisserant@235: {
etisserant@235:   /* Calls _sendPDOevent specifying it is not a sync event */
etisserant@235:   return _sendPDOevent(d, 0);
etisserant@235: }
etisserant@235: 	
etisserant@235: 
etisserant@235: void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum)
etisserant@235: {
etisserant@235: 	/* This is needed to avoid deletion of re-attribuated timer */
etisserant@235: 	d->PDO_status[pdoNum].event_timer = TIMER_NONE;
etisserant@235: 	/* force emission of PDO by artificially changing last emitted*/
etisserant@235: 	d->PDO_status[pdoNum].last_message.cob_id.w = 0;
etisserant@235: 	_sendPDOevent( d, 0 ); /* not a Sync Event*/	
etisserant@235: }
etisserant@235: 
etisserant@235: void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum)
etisserant@235: {
etisserant@235: 	/* This is needed to avoid deletion of re-attribuated timer */
etisserant@235: 	d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;
etisserant@235: 	/* Remove inhibit flag */
etisserant@235: 	d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;
etisserant@235: 	_sendPDOevent( d, 0 ); /* not a Sync Event*/
etisserant@235: }
etisserant@235: 
etisserant@235: /*!
etisserant@235: **
etisserant@235: **
etisserant@235: ** @param d
etisserant@235: ** @param isSyncEvent
etisserant@235: **
etisserant@235: ** @return
etisserant@235: **/
etisserant@235: 
etisserant@235: UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent )
etisserant@235: { 
etisserant@235:   UNS8 	pdoNum = 0x00;       /* number of the actual processed pdo-nr. */
etisserant@235:   UNS8* pTransmissionType = NULL;  
etisserant@235:   UNS8 status = state3;
etisserant@235:   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
etisserant@235:   UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
etisserant@235:   UNS16 lastIndex = d->lastIndex->PDO_TRS;  
etisserant@235: 
etisserant@235:   /* study all PDO stored in the objects dictionary */	
etisserant@235:   if(offsetObjdict){
etisserant@235:    Message pdo = Message_Initializer;
etisserant@235:    while( offsetObjdict <= lastIndex) {  
etisserant@235:     switch( status ) {
etisserant@236:     case state3:
etisserant@236:       if (/*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5)*/
etisserant@236:           /* check if TPDO is not valid */ 
luis@298:           *(UNS32*)d->objdict[offsetObjdict].pSubindex[1].pObject & 0x80000000) {
etisserant@236: 	  MSG_WAR(0x3960, "Not a valid PDO ", 0x1800 + pdoNum);
etisserant@236: 	  /*Go next TPDO*/
etisserant@236: 	  status = state11;
etisserant@236: 	  break;
etisserant@235: 	}
etisserant@236:       /* get the PDO transmission type */
etisserant@235:       pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject;    
etisserant@236:       MSG_WAR(0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);
etisserant@236: 
etisserant@236:       /* check if transmission type is SYNCRONOUS */
etisserant@236:       /* The message is transmited every n SYNC with n=TransmissionType */      
etisserant@235:       if( isSyncEvent && 
etisserant@235:       	  (*pTransmissionType >= TRANS_SYNC_MIN) &&
etisserant@235:       	  (*pTransmissionType <= TRANS_SYNC_MAX) &&
etisserant@236:           (++d->PDO_status[pdoNum].transmit_type_parameter == *pTransmissionType) ) {
etisserant@236:         /*Reset count of SYNC*/	
etisserant@235: 	d->PDO_status[pdoNum].transmit_type_parameter = 0;
etisserant@236: 	MSG_WAR(0x3964, "  PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
leonid@252:  {
leonid@252:  Message msg_init = Message_Initializer;
leonid@252:  pdo = msg_init;
leonid@252:  }	
etisserant@235:         if(buildPDO(d, pdoNum, &pdo))
etisserant@235:         {
etisserant@236:             MSG_ERR(0x1906, " Couldn't build TPDO number : ", pdoNum);
etisserant@235: 	    status = state11;
etisserant@235: 	    break;
etisserant@235:         }
etisserant@235: 	status = state5;
etisserant@236:       /* If transmission RTR, with data sampled on SYNC */
etisserant@236:       }else if( isSyncEvent && 
etisserant@236:       	  (*pTransmissionType == TRANS_RTR_SYNC)) {
etisserant@236:           if(buildPDO(d, pdoNum, &d->PDO_status[pdoNum].last_message))
etisserant@236:           {
etisserant@236:              MSG_ERR(0x1966, " Couldn't build TPDO number : ", pdoNum);
etisserant@236:              d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_RTR_SYNC_READY; 
etisserant@236:           }else{
etisserant@236:              d->PDO_status[pdoNum].transmit_type_parameter |= PDO_RTR_SYNC_READY; 
etisserant@236:           }
etisserant@236: 	  status = state11;
etisserant@236: 	  break;
etisserant@235:       /* If transmission on Event and not inhibited, check for changes */
etisserant@236:       }else if((*pTransmissionType == TRANS_EVENT_PROFILE ||
etisserant@235:                *pTransmissionType == TRANS_EVENT_SPECIFIC )&&
etisserant@235:               !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) {
etisserant@236: 	MSG_WAR(0x3968, "  PDO is on EVENT. Trans type : ", *pTransmissionType);
leonid@252:  {
leonid@252:  Message msg_init = Message_Initializer;
leonid@252:  pdo = msg_init;
leonid@252:  }	
etisserant@235:         if(buildPDO(d, pdoNum, &pdo))
etisserant@235:         {
etisserant@236:             MSG_ERR(0x3907, " Couldn't build TPDO number : ", pdoNum);
etisserant@235: 	    status = state11;
etisserant@235: 	    break;
etisserant@235:         }
etisserant@235:         
etisserant@235: 	/*Compare new and old PDO*/
etisserant@235: 	if(d->PDO_status[pdoNum].last_message.cob_id.w == pdo.cob_id.w &&
etisserant@235: 	   d->PDO_status[pdoNum].last_message.len == pdo.len &&
etisserant@315: #ifdef UNS64
etisserant@235: 	   *(UNS64*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS64*)(&pdo.data[0])){
etisserant@315: #else /* don't ALLOW_64BIT_OPS*/
etisserant@315:        *(UNS32*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS32*)(&pdo.data[0]) &&
etisserant@315:        *(UNS32*)(&d->PDO_status[pdoNum].last_message.data[4]) == *(UNS32*)(&pdo.data[4])){
etisserant@315: #endif 
etisserant@235: 	   	/* No changes -> go to next pdo*/
etisserant@235: 		status = state11;
etisserant@235: 	}else{
luis@298: 		
luis@298: 		UNS16 EventTimerDuration;
luis@298:         UNS16 InhibitTimerDuration;
luis@298: 		
etisserant@236: 		MSG_WAR(0x306A, "Changes TPDO number : ", pdoNum);
etisserant@235: 		/* Changes detected -> transmit message */
luis@298:         EventTimerDuration = *(UNS16*)d->objdict[offsetObjdict].pSubindex[5].pObject;
luis@298:         InhibitTimerDuration = *(UNS16*)d->objdict[offsetObjdict].pSubindex[3].pObject;
etisserant@297:         
etisserant@235: 		status = state5;
etisserant@235: 		
etisserant@235: 		/* Start both event_timer and inhibit_timer*/
etisserant@297:         if(EventTimerDuration){
etisserant@297:            DelAlarm(d->PDO_status[pdoNum].event_timer);
etisserant@297:            d->PDO_status[pdoNum].event_timer = SetAlarm(d, pdoNum, &PDOEventTimerAlarm, MS_TO_TIMEVAL(EventTimerDuration), 0);
etisserant@297:         }
etisserant@235: 		
etisserant@297:         if(InhibitTimerDuration){
etisserant@297: 		   DelAlarm(d->PDO_status[pdoNum].inhibit_timer);
etisserant@297: 		   d->PDO_status[pdoNum].inhibit_timer = SetAlarm(d, pdoNum, &PDOInhibitTimerAlarm, US_TO_TIMEVAL(InhibitTimerDuration * 100), 0);
etisserant@297:            /* and inhibit TPDO */
etisserant@297:            d->PDO_status[pdoNum].transmit_type_parameter |= PDO_INHIBITED;
etisserant@297:         }
etisserant@235: 		
etisserant@235: 	}
etisserant@235:       }else{
etisserant@236: 	MSG_WAR(0x306C, "  PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", *pTransmissionType);
etisserant@235: 	status = state11;
etisserant@235:       }      
etisserant@235:         break;
etisserant@235:     case state5: /*Send the pdo*/
etisserant@235: 	/*store_as_last_message*/
etisserant@235: 	d->PDO_status[pdoNum].last_message = pdo;	
etisserant@236: 	MSG_WAR(0x396D, "sendPDO cobId :", pdo.cob_id.w);
etisserant@236: 	MSG_WAR(0x396E,  "     Nb octets  : ",  pdo.len);
etisserant@235:     	
etisserant@235:     	canSend(d->canHandle,&pdo);
etisserant@235: 	status = state11;
etisserant@235: 	break;     
etisserant@235:     case state11: /*Go to next TPDO*/     
etisserant@235: 	pdoNum++;
etisserant@235: 	offsetObjdict++;
etisserant@235: 	offsetObjdictMap++;
etisserant@236: 	MSG_WAR(0x3970, "next pdo index : ", pdoNum);
etisserant@235: 	status = state3;
etisserant@235: 	break;
etisserant@235:       
etisserant@235:     default:
etisserant@236:       MSG_ERR(0x1972,"Unknown state has been reached : %d",status);
etisserant@235:       return 0xFF;
etisserant@235:     }/* end switch case */
etisserant@235:     
etisserant@235:   }/* end while */
etisserant@235:   }
etisserant@235:   return 0;
etisserant@235: }
etisserant@235: 
etisserant@236: /*!
etisserant@236: **
etisserant@236: **
etisserant@236: ** @param d
etisserant@236: ** @param OD_entry
etisserant@236: ** @param bSubindex 
etisserant@236: ** @return always 0
etisserant@236: **/
etisserant@236: 
etisserant@236: UNS32 TPDO_Communication_Parameter_Callback(CO_Data* d, const indextable * OD_entry, UNS8 bSubindex)
etisserant@236: {
etisserant@236:   /* If PDO are actives */
etisserant@236:   if(d->CurrentCommunicationState.csPDO) switch(bSubindex)
etisserant@236:   {
etisserant@236:   	case 2: /* Changed transmition type */
etisserant@236:   	case 3: /* Changed inhibit time */
etisserant@236:   	case 5: /* Changed event time */
etisserant@236:   	{
etisserant@236:           const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS; 
etisserant@236:           UNS8 numPdo = OD_entry - TPDO_com;       /* number of the actual processed pdo-nr. */
etisserant@236:         
etisserant@236:           /* Zap all timers and inhibit flag */
etisserant@236:           d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer);
etisserant@236:           d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer);
etisserant@236:           d->PDO_status[numPdo].transmit_type_parameter = 0;
etisserant@236:           /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
etisserant@236:           PDOEventTimerAlarm(d, numPdo);
etisserant@236:           return 0;
etisserant@236:   	}
etisserant@236: 
etisserant@236:   	default: /* other subindex are ignored*/
etisserant@236:   	break;
etisserant@236:   }
etisserant@236:   return 0;
etisserant@236: }
etisserant@236: 
etisserant@235: void PDOInit(CO_Data* d)
etisserant@235: {
etisserant@236:   /* For each TPDO mapping parameters */
etisserant@236:   UNS16	pdoIndex = 0x1800; /* OD index of TDPO */
etisserant@236: 
etisserant@236:   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
etisserant@236:   UNS16 lastIndex = d->lastIndex->PDO_TRS;
etisserant@236:   if(offsetObjdict) while( offsetObjdict <= lastIndex) {
etisserant@236:     /* Assign callbacks to sensible TPDO mapping subindexes */
etisserant@236:     UNS32 errorCode;
etisserant@236:     ODCallback_t *CallbackList;
etisserant@236:     /* Find callback list */
etisserant@236:     scanIndexOD (d, pdoIndex, &errorCode, &CallbackList);
etisserant@236:     if(errorCode == OD_SUCCESSFUL && CallbackList)
etisserant@236:     {
etisserant@236:       /*Assign callbacks to corresponding subindex*/
etisserant@236:       /* Transmission type */
etisserant@236:       CallbackList[2] = &TPDO_Communication_Parameter_Callback;
etisserant@236:       /* Inhibit time */
etisserant@236:       CallbackList[3] = &TPDO_Communication_Parameter_Callback;
etisserant@236:       /* Event timer */
etisserant@236:       CallbackList[5] = &TPDO_Communication_Parameter_Callback;
etisserant@236:     }
etisserant@236:     pdoIndex++;
etisserant@236:     offsetObjdict++;
etisserant@236:   }  
etisserant@236: 
etisserant@236:   /* Trigger a non-sync event */
etisserant@236:   _sendPDOevent( d, 0 );
etisserant@235: }
etisserant@235: 
etisserant@235: void PDOStop(CO_Data* d)
etisserant@235: {
etisserant@236:   /* For each TPDO mapping parameters */
etisserant@235:   UNS8 	pdoNum = 0x00;       /* number of the actual processed pdo-nr. */
etisserant@235:   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
etisserant@235:   UNS16 lastIndex = d->lastIndex->PDO_TRS;
etisserant@235:   if(offsetObjdict) while( offsetObjdict <= lastIndex) {
etisserant@236:   	/* Delete TPDO timers */
etisserant@235: 	d->PDO_status[pdoNum].event_timer = DelAlarm(d->PDO_status[pdoNum].event_timer);
etisserant@235: 	d->PDO_status[pdoNum].inhibit_timer = DelAlarm(d->PDO_status[pdoNum].inhibit_timer);
etisserant@236: 	/* Reset transmit type parameter */
etisserant@235: 	d->PDO_status[pdoNum].transmit_type_parameter = 0;
etisserant@235: 	d->PDO_status[pdoNum].last_message.cob_id.w = 0;
etisserant@235: 	pdoNum++;
etisserant@235: 	offsetObjdict++;
etisserant@235:   }  
etisserant@235: }