--- a/src/pdo.c Thu Mar 20 14:22:11 2008 +0100
+++ b/src/pdo.c Thu Mar 20 15:03:25 2008 +0100
@@ -46,56 +46,69 @@
** @return
**/
-UNS8 buildPDO(CO_Data* d, UNS8 numPdo, Message *pdo)
-{
- const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo;
- const indextable* TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;
-
- UNS8 prp_j = 0x00;
- UNS8 offset = 0x00;
- const UNS8* pMappingCount = (UNS8*) TPDO_map->pSubindex[0].pObject;
-
- pdo->cob_id = *(UNS16*)TPDO_com->pSubindex[1].pObject & UNS16_LE(0x7FF);
- pdo->rtr = NOT_A_REQUEST;
-
- MSG_WAR(0x3009, " PDO CobId is : ", *(UNS32*)TPDO_com->pSubindex[1].pObject);
- MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount );
-
- do{
- UNS8 dataType; /* Unused */
- UNS8 tmp[]= {0,0,0,0,0,0,0,0}; /* temporary space to hold bits */
-
- /* pointer fo the var which holds the mapping parameter of an mapping entry */
- UNS32* pMappingParameter = (UNS32*) TPDO_map->pSubindex[prp_j + 1].pObject;
- UNS16 index = (UNS16)((*pMappingParameter) >> 16);
- UNS8 Size = (UNS8)(*pMappingParameter & (UNS32)0x000000FF); /* Size in bits */
-
- /* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */
- if(Size && ((offset + Size) <= 64)) {
- UNS8 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
- UNS8 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
-
- MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter);
- MSG_WAR(0x3050, " at index : ", TPDO_map->index);
- MSG_WAR(0x3051, " sub-index : ", prp_j + 1);
-
- if( getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ) != OD_SUCCESSFUL ){
- MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
- return 0xFF;
- }
- /* copy bit per bit in little endian*/
- CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&pdo->data[offset>>3], offset%8, 0);
-
- offset += Size ;
- }
- prp_j++;
- }while( prp_j < *pMappingCount );
-
- pdo->len = 1 + ((offset - 1) >> 3);
-
- MSG_WAR(0x3015, " End scan mapped variable", 0);
-
- return 0;
+UNS8
+buildPDO (CO_Data * d, UNS8 numPdo, Message * pdo)
+{
+ const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo;
+ const indextable *TPDO_map =
+ d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;
+
+ UNS8 prp_j = 0x00;
+ UNS8 offset = 0x00;
+ const UNS8 *pMappingCount = (UNS8 *) TPDO_map->pSubindex[0].pObject;
+
+ pdo->cob_id = *(UNS16 *) TPDO_com->pSubindex[1].pObject & UNS16_LE (0x7FF);
+ pdo->rtr = NOT_A_REQUEST;
+
+ MSG_WAR (0x3009, " PDO CobId is : ",
+ *(UNS32 *) TPDO_com->pSubindex[1].pObject);
+ MSG_WAR (0x300D, " Number of objects mapped : ", *pMappingCount);
+
+ do
+ {
+ UNS8 dataType; /* Unused */
+ UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 }; /* temporary space to hold bits */
+
+ /* pointer fo the var which holds the mapping parameter of an mapping entry */
+ UNS32 *pMappingParameter =
+ (UNS32 *) TPDO_map->pSubindex[prp_j + 1].pObject;
+ UNS16 index = (UNS16) ((*pMappingParameter) >> 16);
+ UNS8 Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF); /* Size in bits */
+
+ /* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */
+ if (Size && ((offset + Size) <= 64))
+ {
+ UNS8 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
+ UNS8 subIndex =
+ (UNS8) (((*pMappingParameter) >> (UNS8) 8) & (UNS32) 0x000000FF);
+
+ MSG_WAR (0x300F, " got mapping parameter : ", *pMappingParameter);
+ MSG_WAR (0x3050, " at index : ", TPDO_map->index);
+ MSG_WAR (0x3051, " sub-index : ", prp_j + 1);
+
+ if (getODentry (d, index, subIndex, tmp, &ByteSize, &dataType, 0) !=
+ OD_SUCCESSFUL)
+ {
+ MSG_ERR (0x1013,
+ " Couldn't find mapped variable at index-subindex-size : ",
+ (UNS16) (*pMappingParameter));
+ return 0xFF;
+ }
+ /* copy bit per bit in little endian */
+ CopyBits (Size, ((UNS8 *) tmp), 0, 0,
+ (UNS8 *) & pdo->data[offset >> 3], offset % 8, 0);
+
+ offset += Size;
+ }
+ prp_j++;
+ }
+ while (prp_j < *pMappingCount);
+
+ pdo->len = 1 + ((offset - 1) >> 3);
+
+ MSG_WAR (0x3015, " End scan mapped variable", 0);
+
+ return 0;
}
/*!
@@ -106,35 +119,38 @@
**
** @return
**/
-UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex )
-{
- UNS16 * pwCobId;
+UNS8
+sendPDOrequest (CO_Data * d, UNS16 RPDOIndex)
+{
+ UNS16 *pwCobId;
UNS16 offset = d->firstIndex->PDO_RCV;
UNS16 lastIndex = d->lastIndex->PDO_RCV;
/* Sending the request only if the cobid have been found on the PDO
receive */
/* part dictionary */
-
- MSG_WAR(0x3930, "sendPDOrequest RPDO Index : ",RPDOIndex);
-
- if (offset && RPDOIndex >= 0x1400){
- offset += RPDOIndex - 0x1400;
- if (offset <= lastIndex) {
- /* get the CobId*/
- pwCobId = (UNS16*) d->objdict[offset].pSubindex[1].pObject;
-
- MSG_WAR(0x3930, "sendPDOrequest cobId is : ",*pwCobId);
- {
- Message pdo;
- pdo.cob_id = *pwCobId;
- pdo.rtr = REQUEST;
- pdo.len = 0;
- return canSend(d->canHandle,&pdo);
- }
+
+ MSG_WAR (0x3930, "sendPDOrequest RPDO Index : ", RPDOIndex);
+
+ if (offset && RPDOIndex >= 0x1400)
+ {
+ offset += RPDOIndex - 0x1400;
+ if (offset <= lastIndex)
+ {
+ /* get the CobId */
+ pwCobId = (UNS16 *) d->objdict[offset].pSubindex[1].pObject;
+
+ MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId);
+ {
+ Message pdo;
+ pdo.cob_id = *pwCobId;
+ pdo.rtr = REQUEST;
+ pdo.len = 0;
+ return canSend (d->canHandle, &pdo);
+ }
+ }
}
- }
- MSG_ERR(0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex);
+ MSG_ERR (0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex);
return 0xFF;
}
@@ -147,201 +163,254 @@
**
** @return
**/
-UNS8 proceedPDO(CO_Data* d, Message *m)
-{
- UNS8 numPdo;
- UNS8 numMap; /* Number of the mapped varable */
- UNS8 * pMappingCount = NULL; /* count of mapped objects... */
+UNS8
+proceedPDO (CO_Data * d, Message * m)
+{
+ UNS8 numPdo;
+ UNS8 numMap; /* Number of the mapped varable */
+ UNS8 *pMappingCount = NULL; /* count of mapped objects... */
/* pointer to the var which is mapped to a pdo... */
/* void * pMappedAppObject = NULL; */
/* pointer fo the var which holds the mapping parameter of an
mapping entry */
- UNS32 * pMappingParameter = NULL;
- UNS8 * pTransmissionType = NULL; /* pointer to the transmission
- type */
- UNS16 * pwCobId = NULL;
- UNS8 Size;
- UNS8 offset;
- UNS8 status;
- UNS32 objDict;
- UNS16 offsetObjdict;
- UNS16 lastIndex;
-
+ UNS32 *pMappingParameter = NULL;
+ UNS8 *pTransmissionType = NULL; /* pointer to the transmission
+ type */
+ UNS16 *pwCobId = NULL;
+ UNS8 Size;
+ UNS8 offset;
+ UNS8 status;
+ UNS32 objDict;
+ UNS16 offsetObjdict;
+ UNS16 lastIndex;
+
status = state2;
- MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id & UNS16_LE(0x7ff)));
+ MSG_WAR (0x3935, "proceedPDO, cobID : ", ((*m).cob_id & UNS16_LE (0x7ff)));
offset = 0x00;
numPdo = 0;
numMap = 0;
- if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a
- request. */
-
- offsetObjdict = d->firstIndex->PDO_RCV;
- lastIndex = d->lastIndex->PDO_RCV;
-
- /* study of all the PDO stored in the dictionary */
- if(offsetObjdict)
- while (offsetObjdict <= lastIndex) {
-
- switch( status ) {
-
- case state2:
- /* get CobId of the dictionary correspondant to the received
- PDO */
- pwCobId = (UNS16*) d->objdict[offsetObjdict].pSubindex[1].pObject;
- /* check the CobId coherance */
- /*pwCobId is the cobId read in the dictionary at the state 3
- */
- if ( *pwCobId == (*m).cob_id ){
- /* The cobId is recognized */
- status = state4;
- MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
- break;
- }
- else {
- /* cobId received does not match with those write in the
- dictionnary */
- numPdo++;
- offsetObjdict++;
- status = state2;
- break;
- }
-
- case state4:/* Get Mapped Objects Number */
- /* The cobId of the message received has been found in the
- dictionnary. */
- offsetObjdict = d->firstIndex->PDO_RCV_MAP;
- lastIndex = d->lastIndex->PDO_RCV_MAP;
- pMappingCount = (UNS8*) (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
- numMap = 0;
- while (numMap < *pMappingCount) {
- UNS8 tmp[]= {0,0,0,0,0,0,0,0};
- UNS8 ByteSize;
- pMappingParameter = (UNS32*) (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
- if (pMappingParameter == NULL) {
- MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
- return 0xFF;
- }
- /* Get the addresse of the mapped variable. */
- /* detail of *pMappingParameter : */
- /* The 16 hight bits contains the index, the medium 8 bits
- contains the subindex, */
- /* and the lower 8 bits contains the size of the mapped
- variable. */
-
- Size = (UNS8)(*pMappingParameter & (UNS32)0x000000FF);
-
- /* set variable only if Size != 0 and Size is lower than remaining bits in the PDO */
- if(Size && ((offset + Size) <= (m->len << 3))) {
- /* copy bit per bit in little endian */
- CopyBits(Size, (UNS8*)&m->data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
-
- ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 =>
- 2, ... */
-
- objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16),
- (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),
- tmp, &ByteSize, 0 );
-
- if(objDict != OD_SUCCESSFUL) {
- MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);
- MSG_WAR(0x2939, " Mapped at index : ", (*pMappingParameter) >> 16);
- MSG_WAR(0x2940, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
- return 0xFF;
- }
-
- MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id);
- MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16);
- MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
- /* MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */
- offset += Size;
- }
- numMap++;
- } /* end loop while on mapped variables */
-
- offset=0x00;
- numMap = 0;
- return 0;
-
- }/* end switch status*/
- }/* end while*/
- }/* end if Donnees */
- else if ((*m).rtr == REQUEST ){
- MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id);
- status = state1;
- offsetObjdict = d->firstIndex->PDO_TRS;
- lastIndex = d->lastIndex->PDO_TRS;
- if(offsetObjdict) while( offsetObjdict <= lastIndex ){
- /* study of all PDO stored in the objects dictionary */
-
- switch( status ){
-
- case state1:/* check the CobId */
- /* get CobId of the dictionary which match to the received PDO
- */
- pwCobId = (UNS16*) (d->objdict + offsetObjdict)->pSubindex[1].pObject;
- if ( *pwCobId == (*m).cob_id ) {
- status = state4;
- break;
- }
- else {
- numPdo++;
- offsetObjdict++;
- }
- status = state1;
- break;
-
-
- case state4:/* check transmission type */
- pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject;
- /* If PDO is to be sampled and send on RTR, do it*/
- if ( (*pTransmissionType == TRANS_RTR)) {
- status = state5;
- break;
- /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */
- }else if ((*pTransmissionType == TRANS_RTR_SYNC )) {
- if(d->PDO_status[numPdo].transmit_type_parameter & PDO_RTR_SYNC_READY){
- /*Data ready, just send*/
- canSend(d->canHandle,&d->PDO_status[numPdo].last_message);
- return 0;
- }else{
- /* if SYNC did never occur, force emission with current data */
- /* DS301 do not tell what to do in such a case...*/
- MSG_ERR(0x1947, "Not ready RTR_SYNC TPDO send current data : ", m->cob_id);
- status = state5;
- }
- break;
- }else if(
- (*pTransmissionType == TRANS_EVENT_PROFILE) ||
- (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) {
- /* Zap all timers and inhibit flag */
- d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer);
- d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer);
- d->PDO_status[numPdo].transmit_type_parameter &= ~PDO_INHIBITED;
- /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
- PDOEventTimerAlarm(d, numPdo);
- return 0;
- }else {
- /* The requested PDO is not to send on request. So, does
- nothing. */
- MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id);
- return 0xFF;
- }
-
- case state5:/* build and send requested PDO */
- {
- Message pdo;
- if( buildPDO(d, numPdo, &pdo))
- {
- MSG_ERR(0x1948, " Couldn't build TPDO n°", numPdo);
- return 0xFF;
- }
- canSend(d->canHandle,&pdo);
- return 0;
- }
- }/* end switch status */
- }/* end while */
- }/* end if Requete */
+ if ((*m).rtr == NOT_A_REQUEST)
+ { /* The PDO received is not a
+ request. */
+
+ offsetObjdict = d->firstIndex->PDO_RCV;
+ lastIndex = d->lastIndex->PDO_RCV;
+
+ /* study of all the PDO stored in the dictionary */
+ if (offsetObjdict)
+ while (offsetObjdict <= lastIndex)
+ {
+
+ switch (status)
+ {
+
+ case state2:
+ /* get CobId of the dictionary correspondant to the received
+ PDO */
+ pwCobId =
+ (UNS16 *) d->objdict[offsetObjdict].pSubindex[1].pObject;
+ /* check the CobId coherance */
+ /*pwCobId is the cobId read in the dictionary at the state 3
+ */
+ if (*pwCobId == (*m).cob_id)
+ {
+ /* The cobId is recognized */
+ status = state4;
+ MSG_WAR (0x3936, "cobId found at index ",
+ 0x1400 + numPdo);
+ break;
+ }
+ else
+ {
+ /* cobId received does not match with those write in the
+ dictionnary */
+ numPdo++;
+ offsetObjdict++;
+ status = state2;
+ break;
+ }
+
+ case state4: /* Get Mapped Objects Number */
+ /* The cobId of the message received has been found in the
+ dictionnary. */
+ offsetObjdict = d->firstIndex->PDO_RCV_MAP;
+ lastIndex = d->lastIndex->PDO_RCV_MAP;
+ pMappingCount =
+ (UNS8 *) (d->objdict + offsetObjdict +
+ numPdo)->pSubindex[0].pObject;
+ numMap = 0;
+ while (numMap < *pMappingCount)
+ {
+ UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+ UNS8 ByteSize;
+ pMappingParameter =
+ (UNS32 *) (d->objdict + offsetObjdict +
+ numPdo)->pSubindex[numMap + 1].pObject;
+ if (pMappingParameter == NULL)
+ {
+ MSG_ERR (0x1937, "Couldn't get mapping parameter : ",
+ numMap + 1);
+ return 0xFF;
+ }
+ /* Get the addresse of the mapped variable. */
+ /* detail of *pMappingParameter : */
+ /* The 16 hight bits contains the index, the medium 8 bits
+ contains the subindex, */
+ /* and the lower 8 bits contains the size of the mapped
+ variable. */
+
+ Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF);
+
+ /* set variable only if Size != 0 and Size is lower than remaining bits in the PDO */
+ if (Size && ((offset + Size) <= (m->len << 3)))
+ {
+ /* copy bit per bit in little endian */
+ CopyBits (Size, (UNS8 *) & m->data[offset >> 3],
+ offset % 8, 0, ((UNS8 *) tmp), 0, 0);
+
+ ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 =>
+ 2, ... */
+
+ objDict =
+ setODentry (d, (UNS16) ((*pMappingParameter) >> 16),
+ (UNS8) (((*pMappingParameter) >> 8) &
+ 0xFF), tmp, &ByteSize, 0);
+
+ if (objDict != OD_SUCCESSFUL)
+ {
+ MSG_ERR (0x1938,
+ "error accessing to the mapped var : ",
+ numMap + 1);
+ MSG_WAR (0x2939, " Mapped at index : ",
+ (*pMappingParameter) >> 16);
+ MSG_WAR (0x2940, " subindex : ",
+ ((*pMappingParameter) >> 8) & 0xFF);
+ return 0xFF;
+ }
+
+ MSG_WAR (0x3942,
+ "Variable updated with value received by PDO cobid : ",
+ m->cob_id);
+ MSG_WAR (0x3943, " Mapped at index : ",
+ (*pMappingParameter) >> 16);
+ MSG_WAR (0x3944, " subindex : ",
+ ((*pMappingParameter) >> 8) & 0xFF);
+ /* MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */
+ offset += Size;
+ }
+ numMap++;
+ } /* end loop while on mapped variables */
+
+ offset = 0x00;
+ numMap = 0;
+ return 0;
+
+ } /* end switch status */
+ } /* end while */
+ } /* end if Donnees */
+ else if ((*m).rtr == REQUEST)
+ {
+ MSG_WAR (0x3946, "Receive a PDO request cobId : ", m->cob_id);
+ status = state1;
+ offsetObjdict = d->firstIndex->PDO_TRS;
+ lastIndex = d->lastIndex->PDO_TRS;
+ if (offsetObjdict)
+ while (offsetObjdict <= lastIndex)
+ {
+ /* study of all PDO stored in the objects dictionary */
+
+ switch (status)
+ {
+
+ case state1: /* check the CobId */
+ /* get CobId of the dictionary which match to the received PDO
+ */
+ pwCobId =
+ (UNS16 *) (d->objdict +
+ offsetObjdict)->pSubindex[1].pObject;
+ if (*pwCobId == (*m).cob_id)
+ {
+ status = state4;
+ break;
+ }
+ else
+ {
+ numPdo++;
+ offsetObjdict++;
+ }
+ status = state1;
+ break;
+
+
+ case state4: /* check transmission type */
+ pTransmissionType =
+ (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
+ /* If PDO is to be sampled and send on RTR, do it */
+ if ((*pTransmissionType == TRANS_RTR))
+ {
+ status = state5;
+ break;
+ /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */
+ }
+ else if ((*pTransmissionType == TRANS_RTR_SYNC))
+ {
+ if (d->PDO_status[numPdo].
+ transmit_type_parameter & PDO_RTR_SYNC_READY)
+ {
+ /*Data ready, just send */
+ canSend (d->canHandle,
+ &d->PDO_status[numPdo].last_message);
+ return 0;
+ }
+ else
+ {
+ /* if SYNC did never occur, force emission with current data */
+ /* DS301 do not tell what to do in such a case... */
+ MSG_ERR (0x1947,
+ "Not ready RTR_SYNC TPDO send current data : ",
+ m->cob_id);
+ status = state5;
+ }
+ break;
+ }
+ else if ((*pTransmissionType == TRANS_EVENT_PROFILE) ||
+ (*pTransmissionType == TRANS_EVENT_SPECIFIC))
+ {
+ /* Zap all timers and inhibit flag */
+ d->PDO_status[numPdo].event_timer =
+ DelAlarm (d->PDO_status[numPdo].event_timer);
+ d->PDO_status[numPdo].inhibit_timer =
+ DelAlarm (d->PDO_status[numPdo].inhibit_timer);
+ d->PDO_status[numPdo].transmit_type_parameter &=
+ ~PDO_INHIBITED;
+ /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
+ PDOEventTimerAlarm (d, numPdo);
+ return 0;
+ }
+ else
+ {
+ /* The requested PDO is not to send on request. So, does
+ nothing. */
+ MSG_WAR (0x2947, "PDO is not to send on request : ",
+ m->cob_id);
+ return 0xFF;
+ }
+
+ case state5: /* build and send requested PDO */
+ {
+ Message pdo;
+ if (buildPDO (d, numPdo, &pdo))
+ {
+ MSG_ERR (0x1948, " Couldn't build TPDO n°", numPdo);
+ return 0xFF;
+ }
+ canSend (d->canHandle, &pdo);
+ return 0;
+ }
+ } /* end switch status */
+ } /* end while */
+ } /* end if Requete */
return 0;
}
@@ -357,55 +426,62 @@
** @param DestBitIndex
** @param DestBigEndian
**/
-void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)
-{
- /* This loop copy as many bits that it can each time, crossing*/
- /* successively bytes*/
+void
+CopyBits (UNS8 NbBits, UNS8 * SrcByteIndex, UNS8 SrcBitIndex,
+ UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex,
+ UNS8 DestBigEndian)
+{
+ /* This loop copy as many bits that it can each time, crossing */
+ /* successively bytes */
// boundaries from LSB to MSB.
- while(NbBits > 0)
+ while (NbBits > 0)
{
- /* Bit missalignement between src and dest*/
+ /* Bit missalignement between src and dest */
INTEGER8 Vect = DestBitIndex - SrcBitIndex;
- /* We can now get src and align it to dest*/
- UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
-
- /* Compute the nb of bit we will be able to copy*/
- UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex : 8 - SrcBitIndex );
+ /* We can now get src and align it to dest */
+ UNS8 Aligned =
+ Vect > 0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
+
+ /* Compute the nb of bit we will be able to copy */
+ UNS8 BoudaryLimit = (Vect > 0 ? 8 - DestBitIndex : 8 - SrcBitIndex);
UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
- /* Create a mask that will serve in:*/
- UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
-
- /* - Filtering src*/
+ /* Create a mask that will serve in: */
+ UNS8 Mask =
+ ((0xff << (DestBitIndex + BitsToCopy)) |
+ (0xff >> (8 - DestBitIndex)));
+
+ /* - Filtering src */
UNS8 Filtered = Aligned & ~Mask;
- /* - and erase bits where we write, preserve where we don't*/
+ /* - and erase bits where we write, preserve where we don't */
*DestByteIndex &= Mask;
- /* Then write.*/
- *DestByteIndex |= Filtered ;
-
- /*Compute next time cursors for src*/
- if((SrcBitIndex += BitsToCopy)>7)/* cross boundary ?*/
+ /* Then write. */
+ *DestByteIndex |= Filtered;
+
+ /*Compute next time cursors for src */
+ if ((SrcBitIndex += BitsToCopy) > 7) /* cross boundary ? */
{
- SrcBitIndex = 0;/* First bit*/
- SrcByteIndex += (SrcBigEndian ? -1 : 1);/* Next byte*/
+ SrcBitIndex = 0; /* First bit */
+ SrcByteIndex += (SrcBigEndian ? -1 : 1); /* Next byte */
}
- /*Compute next time cursors for dest*/
- if((DestBitIndex += BitsToCopy)>7)
+ /*Compute next time cursors for dest */
+ if ((DestBitIndex += BitsToCopy) > 7)
{
- DestBitIndex = 0;/* First bit*/
- DestByteIndex += (DestBigEndian ? -1 : 1);/* Next byte*/
+ DestBitIndex = 0; /* First bit */
+ DestByteIndex += (DestBigEndian ? -1 : 1); /* Next byte */
}
- /*And decrement counter.*/
+ /*And decrement counter. */
NbBits -= BitsToCopy;
}
}
+
/*!
**
**
@@ -414,29 +490,32 @@
** @return
**/
-UNS8 sendPDOevent( CO_Data* d)
+UNS8
+sendPDOevent (CO_Data * d)
{
/* Calls _sendPDOevent specifying it is not a sync event */
- return _sendPDOevent(d, 0);
-}
-
-
-void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum)
-{
- /* This is needed to avoid deletion of re-attribuated timer */
- d->PDO_status[pdoNum].event_timer = TIMER_NONE;
- /* force emission of PDO by artificially changing last emitted*/
- d->PDO_status[pdoNum].last_message.cob_id = 0;
- _sendPDOevent( d, 0 ); /* not a Sync Event*/
-}
-
-void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum)
-{
- /* This is needed to avoid deletion of re-attribuated timer */
- d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;
- /* Remove inhibit flag */
- d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;
- _sendPDOevent( d, 0 ); /* not a Sync Event*/
+ return _sendPDOevent (d, 0);
+}
+
+
+void
+PDOEventTimerAlarm (CO_Data * d, UNS32 pdoNum)
+{
+ /* This is needed to avoid deletion of re-attribuated timer */
+ d->PDO_status[pdoNum].event_timer = TIMER_NONE;
+ /* force emission of PDO by artificially changing last emitted */
+ d->PDO_status[pdoNum].last_message.cob_id = 0;
+ _sendPDOevent (d, 0); /* not a Sync Event */
+}
+
+void
+PDOInhibitTimerAlarm (CO_Data * d, UNS32 pdoNum)
+{
+ /* This is needed to avoid deletion of re-attribuated timer */
+ d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;
+ /* Remove inhibit flag */
+ d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;
+ _sendPDOevent (d, 0); /* not a Sync Event */
}
/*!
@@ -448,147 +527,193 @@
** @return
**/
-UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent )
-{
- UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */
- UNS8* pTransmissionType = NULL;
+UNS8
+_sendPDOevent (CO_Data * d, UNS8 isSyncEvent)
+{
+ UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */
+ UNS8 *pTransmissionType = NULL;
UNS8 status = state3;
UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
- UNS16 lastIndex = d->lastIndex->PDO_TRS;
-
- /* study all PDO stored in the objects dictionary */
- if(offsetObjdict){
- Message pdo = Message_Initializer;
- while( offsetObjdict <= lastIndex) {
- switch( status ) {
- case state3:
- if (/*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5)*/
- /* check if TPDO is not valid */
- *(UNS32*)d->objdict[offsetObjdict].pSubindex[1].pObject & 0x80000000) {
- MSG_WAR(0x3960, "Not a valid PDO ", 0x1800 + pdoNum);
- /*Go next TPDO*/
- status = state11;
- break;
- }
- /* get the PDO transmission type */
- pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject;
- MSG_WAR(0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);
-
- /* check if transmission type is SYNCRONOUS */
- /* The message is transmited every n SYNC with n=TransmissionType */
- if( isSyncEvent &&
- (*pTransmissionType >= TRANS_SYNC_MIN) &&
- (*pTransmissionType <= TRANS_SYNC_MAX) &&
- (++d->PDO_status[pdoNum].transmit_type_parameter == *pTransmissionType) ) {
- /*Reset count of SYNC*/
- d->PDO_status[pdoNum].transmit_type_parameter = 0;
- MSG_WAR(0x3964, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
- {
- Message msg_init = Message_Initializer;
- pdo = msg_init;
- }
- if(buildPDO(d, pdoNum, &pdo))
+ UNS16 lastIndex = d->lastIndex->PDO_TRS;
+
+ /* study all PDO stored in the objects dictionary */
+ if (offsetObjdict)
+ {
+ Message pdo = Message_Initializer;
+ while (offsetObjdict <= lastIndex)
{
- MSG_ERR(0x1906, " Couldn't build TPDO number : ", pdoNum);
- status = state11;
- break;
- }
- status = state5;
- /* If transmission RTR, with data sampled on SYNC */
- }else if( isSyncEvent &&
- (*pTransmissionType == TRANS_RTR_SYNC)) {
- if(buildPDO(d, pdoNum, &d->PDO_status[pdoNum].last_message))
- {
- MSG_ERR(0x1966, " Couldn't build TPDO number : ", pdoNum);
- d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_RTR_SYNC_READY;
- }else{
- d->PDO_status[pdoNum].transmit_type_parameter |= PDO_RTR_SYNC_READY;
- }
- status = state11;
- break;
- /* If transmission on Event and not inhibited, check for changes */
- }else if((*pTransmissionType == TRANS_EVENT_PROFILE ||
- *pTransmissionType == TRANS_EVENT_SPECIFIC )&&
- !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) {
- MSG_WAR(0x3968, " PDO is on EVENT. Trans type : ", *pTransmissionType);
- {
- Message msg_init = Message_Initializer;
- pdo = msg_init;
- }
- if(buildPDO(d, pdoNum, &pdo))
- {
- MSG_ERR(0x3907, " Couldn't build TPDO number : ", pdoNum);
- status = state11;
- break;
- }
-
- /*Compare new and old PDO*/
- if(d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id &&
- d->PDO_status[pdoNum].last_message.len == pdo.len &&
+ switch (status)
+ {
+ case state3:
+ if ( /*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5) */
+ /* check if TPDO is not valid */
+ *(UNS32 *) d->objdict[offsetObjdict].pSubindex[1].
+ pObject & 0x80000000)
+ {
+ MSG_WAR (0x3960, "Not a valid PDO ", 0x1800 + pdoNum);
+ /*Go next TPDO */
+ status = state11;
+ break;
+ }
+ /* get the PDO transmission type */
+ pTransmissionType =
+ (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
+ MSG_WAR (0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);
+
+ /* check if transmission type is SYNCRONOUS */
+ /* The message is transmited every n SYNC with n=TransmissionType */
+ if (isSyncEvent &&
+ (*pTransmissionType >= TRANS_SYNC_MIN) &&
+ (*pTransmissionType <= TRANS_SYNC_MAX) &&
+ (++d->PDO_status[pdoNum].transmit_type_parameter ==
+ *pTransmissionType))
+ {
+ /*Reset count of SYNC */
+ d->PDO_status[pdoNum].transmit_type_parameter = 0;
+ MSG_WAR (0x3964, " PDO is on SYNCHRO. Trans type : ",
+ *pTransmissionType);
+ {
+ Message msg_init = Message_Initializer;
+ pdo = msg_init;
+ }
+ if (buildPDO (d, pdoNum, &pdo))
+ {
+ MSG_ERR (0x1906, " Couldn't build TPDO number : ",
+ pdoNum);
+ status = state11;
+ break;
+ }
+ status = state5;
+ /* If transmission RTR, with data sampled on SYNC */
+ }
+ else if (isSyncEvent && (*pTransmissionType == TRANS_RTR_SYNC))
+ {
+ if (buildPDO
+ (d, pdoNum, &d->PDO_status[pdoNum].last_message))
+ {
+ MSG_ERR (0x1966, " Couldn't build TPDO number : ",
+ pdoNum);
+ d->PDO_status[pdoNum].transmit_type_parameter &=
+ ~PDO_RTR_SYNC_READY;
+ }
+ else
+ {
+ d->PDO_status[pdoNum].transmit_type_parameter |=
+ PDO_RTR_SYNC_READY;
+ }
+ status = state11;
+ break;
+ /* If transmission on Event and not inhibited, check for changes */
+ }
+ else if ((*pTransmissionType == TRANS_EVENT_PROFILE ||
+ *pTransmissionType == TRANS_EVENT_SPECIFIC) &&
+ !(d->PDO_status[pdoNum].
+ transmit_type_parameter & PDO_INHIBITED))
+ {
+ MSG_WAR (0x3968, " PDO is on EVENT. Trans type : ",
+ *pTransmissionType);
+ {
+ Message msg_init = Message_Initializer;
+ pdo = msg_init;
+ }
+ if (buildPDO (d, pdoNum, &pdo))
+ {
+ MSG_ERR (0x3907, " Couldn't build TPDO number : ",
+ pdoNum);
+ status = state11;
+ break;
+ }
+
+ /*Compare new and old PDO */
+ if (d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id
+ && d->PDO_status[pdoNum].last_message.len == pdo.len &&
#ifdef UNS64
- *(UNS64*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS64*)(&pdo.data[0])){
-#else /* don't ALLOW_64BIT_OPS*/
- *(UNS32*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS32*)(&pdo.data[0]) &&
- *(UNS32*)(&d->PDO_status[pdoNum].last_message.data[4]) == *(UNS32*)(&pdo.data[4])){
-#endif
- /* No changes -> go to next pdo*/
- status = state11;
- }else{
-
- UNS16 EventTimerDuration;
- UNS16 InhibitTimerDuration;
-
- MSG_WAR(0x306A, "Changes TPDO number : ", pdoNum);
- /* Changes detected -> transmit message */
- EventTimerDuration = *(UNS16*)d->objdict[offsetObjdict].pSubindex[5].pObject;
- InhibitTimerDuration = *(UNS16*)d->objdict[offsetObjdict].pSubindex[3].pObject;
-
- status = state5;
-
- /* Start both event_timer and inhibit_timer*/
- if(EventTimerDuration){
- DelAlarm(d->PDO_status[pdoNum].event_timer);
- d->PDO_status[pdoNum].event_timer = SetAlarm(d, pdoNum, &PDOEventTimerAlarm, MS_TO_TIMEVAL(EventTimerDuration), 0);
- }
-
- if(InhibitTimerDuration){
- DelAlarm(d->PDO_status[pdoNum].inhibit_timer);
- d->PDO_status[pdoNum].inhibit_timer = SetAlarm(d, pdoNum, &PDOInhibitTimerAlarm, US_TO_TIMEVAL(InhibitTimerDuration * 100), 0);
- /* and inhibit TPDO */
- d->PDO_status[pdoNum].transmit_type_parameter |= PDO_INHIBITED;
- }
-
- }
- }else{
- MSG_WAR(0x306C, " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", *pTransmissionType);
- status = state11;
- }
- break;
- case state5: /*Send the pdo*/
- /*store_as_last_message*/
- d->PDO_status[pdoNum].last_message = pdo;
- MSG_WAR(0x396D, "sendPDO cobId :", pdo.cob_id);
- MSG_WAR(0x396E, " Nb octets : ", pdo.len);
-
- canSend(d->canHandle,&pdo);
- status = state11;
- break;
- case state11: /*Go to next TPDO*/
- pdoNum++;
- offsetObjdict++;
- offsetObjdictMap++;
- MSG_WAR(0x3970, "next pdo index : ", pdoNum);
- status = state3;
- break;
-
- default:
- MSG_ERR(0x1972,"Unknown state has been reached : %d",status);
- return 0xFF;
- }/* end switch case */
-
- }/* end while */
- }
+ *(UNS64 *) (&d->PDO_status[pdoNum].last_message.
+ data[0]) == *(UNS64 *) (&pdo.data[0])
+#else /* don't ALLOW_64BIT_OPS */
+ *(UNS32 *) (&d->PDO_status[pdoNum].last_message.
+ data[0]) == *(UNS32 *) (&pdo.data[0])
+ && *(UNS32 *) (&d->PDO_status[pdoNum].last_message.
+ data[4]) == *(UNS32 *) (&pdo.data[4])
+#endif
+ )
+ {
+ /* No changes -> go to next pdo */
+ status = state11;
+ }
+ else
+ {
+
+ UNS16 EventTimerDuration;
+ UNS16 InhibitTimerDuration;
+
+ MSG_WAR (0x306A, "Changes TPDO number : ", pdoNum);
+ /* Changes detected -> transmit message */
+ EventTimerDuration =
+ *(UNS16 *) d->objdict[offsetObjdict].pSubindex[5].
+ pObject;
+ InhibitTimerDuration =
+ *(UNS16 *) d->objdict[offsetObjdict].pSubindex[3].
+ pObject;
+
+ status = state5;
+
+ /* Start both event_timer and inhibit_timer */
+ if (EventTimerDuration)
+ {
+ DelAlarm (d->PDO_status[pdoNum].event_timer);
+ d->PDO_status[pdoNum].event_timer =
+ SetAlarm (d, pdoNum, &PDOEventTimerAlarm,
+ MS_TO_TIMEVAL (EventTimerDuration), 0);
+ }
+
+ if (InhibitTimerDuration)
+ {
+ DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
+ d->PDO_status[pdoNum].inhibit_timer =
+ SetAlarm (d, pdoNum, &PDOInhibitTimerAlarm,
+ US_TO_TIMEVAL (InhibitTimerDuration *
+ 100), 0);
+ /* and inhibit TPDO */
+ d->PDO_status[pdoNum].transmit_type_parameter |=
+ PDO_INHIBITED;
+ }
+
+ }
+ }
+ else
+ {
+ MSG_WAR (0x306C,
+ " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ",
+ *pTransmissionType);
+ status = state11;
+ }
+ break;
+ case state5: /*Send the pdo */
+ /*store_as_last_message */
+ d->PDO_status[pdoNum].last_message = pdo;
+ MSG_WAR (0x396D, "sendPDO cobId :", pdo.cob_id);
+ MSG_WAR (0x396E, " Nb octets : ", pdo.len);
+
+ canSend (d->canHandle, &pdo);
+ status = state11;
+ break;
+ case state11: /*Go to next TPDO */
+ pdoNum++;
+ offsetObjdict++;
+ offsetObjdictMap++;
+ MSG_WAR (0x3970, "next pdo index : ", pdoNum);
+ status = state3;
+ break;
+
+ default:
+ MSG_ERR (0x1972, "Unknown state has been reached : %d", status);
+ return 0xFF;
+ } /* end switch case */
+
+ } /* end while */
+ }
return 0;
}
@@ -601,78 +726,92 @@
** @return always 0
**/
-UNS32 TPDO_Communication_Parameter_Callback(CO_Data* d, const indextable * OD_entry, UNS8 bSubindex)
+UNS32
+TPDO_Communication_Parameter_Callback (CO_Data * d,
+ const indextable * OD_entry,
+ UNS8 bSubindex)
{
/* If PDO are actives */
- if(d->CurrentCommunicationState.csPDO) switch(bSubindex)
- {
- case 2: /* Changed transmition type */
- case 3: /* Changed inhibit time */
- case 5: /* Changed event time */
- {
- const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS;
- UNS8 numPdo = OD_entry - TPDO_com; /* number of the actual processed pdo-nr. */
-
+ if (d->CurrentCommunicationState.csPDO)
+ switch (bSubindex)
+ {
+ case 2: /* Changed transmition type */
+ case 3: /* Changed inhibit time */
+ case 5: /* Changed event time */
+ {
+ const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS;
+ UNS8 numPdo = OD_entry - TPDO_com; /* number of the actual processed pdo-nr. */
+
/* Zap all timers and inhibit flag */
- d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer);
- d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer);
+ d->PDO_status[numPdo].event_timer =
+ DelAlarm (d->PDO_status[numPdo].event_timer);
+ d->PDO_status[numPdo].inhibit_timer =
+ DelAlarm (d->PDO_status[numPdo].inhibit_timer);
d->PDO_status[numPdo].transmit_type_parameter = 0;
/* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
- PDOEventTimerAlarm(d, numPdo);
+ PDOEventTimerAlarm (d, numPdo);
return 0;
- }
-
- default: /* other subindex are ignored*/
- break;
- }
+ }
+
+ default: /* other subindex are ignored */
+ break;
+ }
return 0;
}
-void PDOInit(CO_Data* d)
+void
+PDOInit (CO_Data * d)
{
/* For each TPDO mapping parameters */
- UNS16 pdoIndex = 0x1800; /* OD index of TDPO */
+ UNS16 pdoIndex = 0x1800; /* OD index of TDPO */
UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
UNS16 lastIndex = d->lastIndex->PDO_TRS;
- if(offsetObjdict) while( offsetObjdict <= lastIndex) {
- /* Assign callbacks to sensible TPDO mapping subindexes */
- UNS32 errorCode;
- ODCallback_t *CallbackList;
- /* Find callback list */
- scanIndexOD (d, pdoIndex, &errorCode, &CallbackList);
- if(errorCode == OD_SUCCESSFUL && CallbackList)
- {
- /*Assign callbacks to corresponding subindex*/
- /* Transmission type */
- CallbackList[2] = &TPDO_Communication_Parameter_Callback;
- /* Inhibit time */
- CallbackList[3] = &TPDO_Communication_Parameter_Callback;
- /* Event timer */
- CallbackList[5] = &TPDO_Communication_Parameter_Callback;
- }
- pdoIndex++;
- offsetObjdict++;
- }
+ if (offsetObjdict)
+ while (offsetObjdict <= lastIndex)
+ {
+ /* Assign callbacks to sensible TPDO mapping subindexes */
+ UNS32 errorCode;
+ ODCallback_t *CallbackList;
+ /* Find callback list */
+ scanIndexOD (d, pdoIndex, &errorCode, &CallbackList);
+ if (errorCode == OD_SUCCESSFUL && CallbackList)
+ {
+ /*Assign callbacks to corresponding subindex */
+ /* Transmission type */
+ CallbackList[2] = &TPDO_Communication_Parameter_Callback;
+ /* Inhibit time */
+ CallbackList[3] = &TPDO_Communication_Parameter_Callback;
+ /* Event timer */
+ CallbackList[5] = &TPDO_Communication_Parameter_Callback;
+ }
+ pdoIndex++;
+ offsetObjdict++;
+ }
/* Trigger a non-sync event */
- _sendPDOevent( d, 0 );
-}
-
-void PDOStop(CO_Data* d)
+ _sendPDOevent (d, 0);
+}
+
+void
+PDOStop (CO_Data * d)
{
/* For each TPDO mapping parameters */
- UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */
+ UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */
UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
UNS16 lastIndex = d->lastIndex->PDO_TRS;
- if(offsetObjdict) while( offsetObjdict <= lastIndex) {
- /* Delete TPDO timers */
- d->PDO_status[pdoNum].event_timer = DelAlarm(d->PDO_status[pdoNum].event_timer);
- d->PDO_status[pdoNum].inhibit_timer = DelAlarm(d->PDO_status[pdoNum].inhibit_timer);
- /* Reset transmit type parameter */
- d->PDO_status[pdoNum].transmit_type_parameter = 0;
- d->PDO_status[pdoNum].last_message.cob_id = 0;
- pdoNum++;
- offsetObjdict++;
- }
-}
+ if (offsetObjdict)
+ while (offsetObjdict <= lastIndex)
+ {
+ /* Delete TPDO timers */
+ d->PDO_status[pdoNum].event_timer =
+ DelAlarm (d->PDO_status[pdoNum].event_timer);
+ d->PDO_status[pdoNum].inhibit_timer =
+ DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
+ /* Reset transmit type parameter */
+ d->PDO_status[pdoNum].transmit_type_parameter = 0;
+ d->PDO_status[pdoNum].last_message.cob_id = 0;
+ pdoNum++;
+ offsetObjdict++;
+ }
+}