diff -r f812bf6b7237 -r 905677ed00f3 src/pdo.c --- a/src/pdo.c Fri Jun 29 19:51:39 2007 +0200 +++ b/src/pdo.c Mon Jul 02 18:22:58 2007 +0200 @@ -54,7 +54,7 @@ UNS8 offset = 0x00; const UNS8* pMappingCount = (UNS8*) TPDO_map->pSubindex[0].pObject; - pdo->cob_id.w = *(UNS32*)TPDO_com->pSubindex[1].pObject; + pdo->cob_id.w = *(UNS32*)TPDO_com->pSubindex[1].pObject & 0x7FF; pdo->rtr = NOT_A_REQUEST; MSG_WAR(0x3009, " PDO CobId is : ", *(UNS32*)TPDO_com->pSubindex[1].pObject); @@ -101,31 +101,31 @@ ** ** @return **/ -UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId ) +UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex ) { UNS32 * pwCobId; - UNS16 offset; - UNS16 lastIndex; - UNS8 err; - - MSG_WAR(0x3930, "sendPDOrequest ",0); + 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 */ - offset = d->firstIndex->PDO_RCV; - lastIndex = d->lastIndex->PDO_RCV; - if (offset) - while (offset <= lastIndex) { + + MSG_WAR(0x3930, "sendPDOrequest RPDO Index : ",RPDOIndex); + + if (offset && RPDOIndex >= 0x1400){ + offset += RPDOIndex - 0x1400; + if (offset <= lastIndex) { /* get the CobId*/ pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; - if ( *pwCobId == cobId ) { - Message pdo = {*pwCobId, REQUEST, 0}; - return canSend(d->canHandle,&pdo); - } - offset++; + MSG_WAR(0x3930, "sendPDOrequest cobId is : ",*pwCobId); + + Message pdo = {*pwCobId, REQUEST, 0}; + return canSend(d->canHandle,&pdo); } - MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId); + } + MSG_ERR(0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex); return 0xFF; } @@ -283,12 +283,25 @@ break; - case state4:/* check transmission type (after request?) */ + case state4:/* check transmission type */ pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject; - if ( (*pTransmissionType == TRANS_RTR) || - (*pTransmissionType == TRANS_RTR_SYNC )) { + /* 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.w); + status = state5; + } + break; }else if( (*pTransmissionType == TRANS_EVENT_PROFILE) || (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) { @@ -311,8 +324,8 @@ Message pdo; if( buildPDO(d, numPdo, &pdo)) { - MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter)); - //return 0xFF; /*No real reason to stop...*/ + MSG_ERR(0x1948, " Couldn't build TPDO n°", numPdo); + return 0xFF; } canSend(d->canHandle,&pdo); return 0; @@ -401,8 +414,6 @@ void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum) { - printf("EV PDOEventTimerAlarm : %d\n", 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*/ @@ -412,8 +423,6 @@ void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum) { - printf("EV PDOInhibitTimerAlarm : %d\n", pdoNum); - /* This is needed to avoid deletion of re-attribuated timer */ d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE; /* Remove inhibit flag */ @@ -441,44 +450,60 @@ /* study all PDO stored in the objects dictionary */ if(offsetObjdict){ - Message clean = Message_Initializer; Message pdo = Message_Initializer; while( offsetObjdict <= lastIndex) { switch( status ) { - case state3: /* get the PDO transmission type */ - if (d->objdict[offsetObjdict].bSubCount <= 2) { - MSG_ERR(0x1004, "Subindex 2 not found at index ", 0x1800 + pdoNum); - return 0xFF; + 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[0].pObject & 0x8000) { + 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(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum); - - /* check if transmission type is after (this) SYNC */ - /* The message may not be transmited every SYNC but every n SYNC */ + 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) ) { + (++d->PDO_status[pdoNum].transmit_type_parameter == *pTransmissionType) ) { + /*Reset count of SYNC*/ d->PDO_status[pdoNum].transmit_type_parameter = 0; - MSG_WAR(0x3007, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType); - pdo = clean; + MSG_WAR(0x3964, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType); + pdo = (Message)Message_Initializer; if(buildPDO(d, pdoNum, &pdo)) { - MSG_ERR(0x3006, " Couldn't build TPDO number : ", pdoNum); + 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 || + }else if((*pTransmissionType == TRANS_EVENT_PROFILE || *pTransmissionType == TRANS_EVENT_SPECIFIC )&& !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) { - MSG_WAR(0x3008, " PDO is on EVENT. Trans type : ", *pTransmissionType); - pdo = pdo = clean; + MSG_WAR(0x3968, " PDO is on EVENT. Trans type : ", *pTransmissionType); + pdo = (Message)Message_Initializer; if(buildPDO(d, pdoNum, &pdo)) { - MSG_ERR(0x3007, " Couldn't build TPDO number : ", pdoNum); + MSG_ERR(0x3907, " Couldn't build TPDO number : ", pdoNum); status = state11; break; } @@ -490,8 +515,7 @@ /* No changes -> go to next pdo*/ status = state11; }else{ - MSG_WAR(0x3008, "Changes TPDO number : ", pdoNum); - printf("EV Changes TPDO number : %d\n", pdoNum); + MSG_WAR(0x306A, "Changes TPDO number : ", pdoNum); /* Changes detected -> transmit message */ status = state5; @@ -506,19 +530,15 @@ d->PDO_status[pdoNum].transmit_type_parameter |= PDO_INHIBITED; } }else{ - MSG_WAR(0x3009, " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", *pTransmissionType); + 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(0x3901, "sendPDO cobId :", pdo.cob_id.w); - MSG_WAR(0x3902, " Nb octets : ", pdo.len); - {int i; - for (i = 0 ; i < pdo.len ; i++) { - MSG_WAR(0x3903," data : ", pdo.data[i]); - }} + MSG_WAR(0x396D, "sendPDO cobId :", pdo.cob_id.w); + MSG_WAR(0x396E, " Nb octets : ", pdo.len); canSend(d->canHandle,&pdo); status = state11; @@ -527,12 +547,12 @@ pdoNum++; offsetObjdict++; offsetObjdictMap++; - MSG_WAR(0x3017, "next pdo index : ", pdoNum); + MSG_WAR(0x3970, "next pdo index : ", pdoNum); status = state3; break; default: - MSG_ERR(0x1019,"Unknown state has been reached : %d",status); + MSG_ERR(0x1972,"Unknown state has been reached : %d",status); return 0xFF; }/* end switch case */ @@ -541,31 +561,85 @@ return 0; } +/*! +** +** +** @param d +** @param OD_entry +** @param bSubindex +** @return always 0 +**/ + +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 */ + { + UNS8 pTransmissionType = *(UNS8*) OD_entry->pSubindex[2].pObject; + 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].transmit_type_parameter = 0; + /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */ + PDOEventTimerAlarm(d, numPdo); + return 0; + } + + default: /* other subindex are ignored*/ + break; + } + return 0; +} + void PDOInit(CO_Data* d) { - - /* TODO: implement callbacks on 140xh - * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx - * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx - * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx - * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx - * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx - * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx - * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx - * XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx - */ - - _sendPDOevent(d, 0 ); + /* For each TPDO mapping parameters */ + 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++; + } + + /* Trigger a non-sync event */ + _sendPDOevent( d, 0 ); } void PDOStop(CO_Data* d) { + /* For each TPDO mapping parameters */ 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.w = 0; pdoNum++;