From c7faf3fefdd94dee3a84ff2c524ed081a0241796 Mon Sep 17 00:00:00 2001 From: etisserant Date: Thu, 20 Mar 2008 14:03:25 +0000 Subject: [PATCH] re-indented pdo.c --- src/pdo.c | 1095 ++++++++++++++++++++++++++++++----------------------- 1 file changed, 617 insertions(+), 478 deletions(-) diff --git a/src/pdo.c b/src/pdo.c index 9ceea31..e4c0316 100644 --- a/src/pdo.c +++ b/src/pdo.c @@ -46,56 +46,69 @@ ** @return **/ -UNS8 buildPDO(CO_Data* d, UNS8 numPdo, Message *pdo) +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; + 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 @@ UNS8 buildPDO(CO_Data* d, UNS8 numPdo, Message *pdo) ** ** @return **/ -UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex ) +UNS8 +sendPDOrequest (CO_Data * d, UNS16 RPDOIndex) { - UNS16 * pwCobId; + 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 @@ UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex ) ** ** @return **/ -UNS8 proceedPDO(CO_Data* d, Message *m) +UNS8 +proceedPDO (CO_Data * d, Message * m) { - UNS8 numPdo; - UNS8 numMap; /* Number of the mapped varable */ - UNS8 * pMappingCount = NULL; /* count of mapped objects... */ + 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; + if ((*m).rtr == NOT_A_REQUEST) + { /* The PDO received is not a + request. */ + offsetObjdict = d->firstIndex->PDO_RCV; + lastIndex = d->lastIndex->PDO_RCV; - 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; - } + /* study of all the PDO stored in the dictionary */ + if (offsetObjdict) + while (offsetObjdict <= lastIndex) + { - 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 */ + 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 @@ UNS8 proceedPDO(CO_Data* d, Message *m) ** @param DestBitIndex ** @param DestBigEndian **/ -void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 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*/ + /* 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; + /* 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 ); + /* 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))); + /* Create a mask that will serve in: */ + UNS8 Mask = + ((0xff << (DestBitIndex + BitsToCopy)) | + (0xff >> (8 - DestBitIndex))); - /* - Filtering src*/ + /* - 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 ; + /* Then write. */ + *DestByteIndex |= Filtered; - /*Compute next time cursors for src*/ - if((SrcBitIndex += BitsToCopy)>7)/* cross boundary ?*/ + /*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 @@ void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndi ** @return **/ -UNS8 sendPDOevent( CO_Data* d) +UNS8 +sendPDOevent (CO_Data * d) { /* Calls _sendPDOevent specifying it is not a sync event */ - return _sendPDOevent(d, 0); + return _sendPDOevent (d, 0); } - -void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum) + +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*/ + /* 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) +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*/ + /* 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 @@ void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum) ** @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)) - { - 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)) + 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(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 @@ UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent ) ** @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 ); + _sendPDOevent (d, 0); } -void PDOStop(CO_Data* d) +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++; + } } -- 2.39.2