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: UNS8 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ etisserant@235: UNS8 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); etisserant@235: etisserant@235: MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter); etisserant@235: MSG_WAR(0x3050, " at index : ", TPDO_map->index); etisserant@235: MSG_WAR(0x3051, " sub-index : ", prp_j + 1); etisserant@235: etisserant@235: if( getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ) != OD_SUCCESSFUL ){ etisserant@235: MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter)); etisserant@235: return 0xFF; etisserant@235: } etisserant@235: /* copy bit per bit in little endian*/ etisserant@235: CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&pdo->data[offset>>3], offset%8, 0); etisserant@235: etisserant@235: offset += Size ; 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 */ etisserant@0: UNS8 i; 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 dataType; 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); nico@208: nico@215: /* copy bit per bit in little endian */ etisserant@235: CopyBits(Size, (UNS8*)&m->data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0); nico@208: nico@208: ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => nico@208: 2, ... */ nico@208: nico@208: objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16), nico@208: (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF), nico@208: tmp, &ByteSize, 0 ); nico@208: nico@208: if(objDict != OD_SUCCESSFUL) { nico@208: MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1); nico@208: MSG_WAR(0x2939, " Mapped at index : ", (*pMappingParameter) >> 16); nico@208: MSG_WAR(0x2940, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); nico@208: return 0xFF; nico@208: } nico@208: nico@208: MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w); nico@208: MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16); nico@208: MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); nico@215: /* MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */ nico@208: offset += Size; nico@208: numMap++; etisserant@235: /*TODO : check that offset is not not greater that message size (in bit) */ 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 */ etisserant@236: *(UNS32*)d->objdict[offsetObjdict].pSubindex[0].pObject & 0x8000) { 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@235: *(UNS64*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS64*)(&pdo.data[0])){ etisserant@235: /* No changes -> go to next pdo*/ etisserant@235: status = state11; etisserant@235: }else{ etisserant@236: MSG_WAR(0x306A, "Changes TPDO number : ", pdoNum); etisserant@235: /* Changes detected -> transmit message */ etisserant@235: status = state5; etisserant@235: etisserant@235: /* Start both event_timer and inhibit_timer*/ etisserant@235: DelAlarm(d->PDO_status[pdoNum].event_timer); etisserant@235: d->PDO_status[pdoNum].event_timer = SetAlarm(d, pdoNum, &PDOEventTimerAlarm, MS_TO_TIMEVAL(*(UNS16*)d->objdict[offsetObjdict].pSubindex[5].pObject), 0); etisserant@235: etisserant@235: DelAlarm(d->PDO_status[pdoNum].inhibit_timer); etisserant@235: d->PDO_status[pdoNum].inhibit_timer = SetAlarm(d, pdoNum, &PDOInhibitTimerAlarm, US_TO_TIMEVAL(*(UNS16*)d->objdict[offsetObjdict].pSubindex[3].pObject * 100), 0); etisserant@235: etisserant@235: /* and inhibit TPDO */ etisserant@235: d->PDO_status[pdoNum].transmit_type_parameter |= PDO_INHIBITED; 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: UNS8 pTransmissionType = *(UNS8*) OD_entry->pSubindex[2].pObject; 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: }