CanFestival: src/pdo.c Source File

00001 /*
-00002   This file is part of CanFestival, a library implementing CanOpen
-00003   Stack.
-00004 
-00005   Copyright (C): Edouard TISSERANT and Francis DUPIN
-00006 
-00007   See COPYING file for copyrights details.
-00008 
-00009   This library is free software; you can redistribute it and/or
-00010   modify it under the terms of the GNU Lesser General Public
-00011   License as published by the Free Software Foundation; either
-00012   version 2.1 of the License, or (at your option) any later version.
-00013 
-00014   This library is distributed in the hope that it will be useful,
-00015   but WITHOUT ANY WARRANTY; without even the implied warranty of
-00016   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-00017   Lesser General Public License for more details.
-00018 
-00019   You should have received a copy of the GNU Lesser General Public
-00020   License along with this library; if not, write to the Free Software
-00021   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
-00022   USA
-00023 */
-00024 #include "pdo.h"
-00025 #include "objacces.h"
-00026 #include "canfestival.h"
-00027 
-00048 UNS8 buildPDO(CO_Data* d, UNS8 numPdo, Message *pdo)
-00049 {
-00050         const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo; 
-00051         const indextable* TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;
-00052         
-00053         UNS8 prp_j = 0x00;
-00054         UNS8 offset = 0x00;
-00055         const UNS8* pMappingCount = (UNS8*) TPDO_map->pSubindex[0].pObject;
-00056 
-00057         pdo->cob_id.w = *(UNS32*)TPDO_com->pSubindex[1].pObject & 0x7FF;
-00058         pdo->rtr = NOT_A_REQUEST;
-00059 
-00060         MSG_WAR(0x3009, "  PDO CobId is : ", *(UNS32*)TPDO_com->pSubindex[1].pObject);
-00061         MSG_WAR(0x300D, "  Number of objects mapped : ",*pMappingCount );
-00062         
-00063         do{
-00064                 UNS8 dataType; /* Unused */
-00065                 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; /* temporary space to hold bits */
-00066 
-00067                 /* pointer fo the var which holds the mapping parameter of an mapping entry  */
-00068                 UNS32* pMappingParameter = (UNS32*) TPDO_map->pSubindex[prp_j + 1].pObject;
-00069                 UNS16 index = (UNS16)((*pMappingParameter) >> 16);
-00070                 UNS8 Size = (UNS8)(*pMappingParameter); /* Size in bits */
-00071                 UNS8 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
-00072                 UNS8 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
-00073                 
-00074                 MSG_WAR(0x300F, "  got mapping parameter : ", *pMappingParameter);
-00075                 MSG_WAR(0x3050, "    at index : ", TPDO_map->index);
-00076                 MSG_WAR(0x3051, "    sub-index : ", prp_j + 1);
-00077                 
-00078                 if( getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ) != OD_SUCCESSFUL ){
-00079                         MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
-00080                         return 0xFF;
-00081                 }
-00082                 /* copy bit per bit in little endian*/
-00083                 CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&pdo->data[offset>>3], offset%8, 0);
-00084 
-00085                 offset += Size ;
-00086                 prp_j++;
-00087         }while( prp_j < *pMappingCount );
-00088 
-00089         pdo->len = 1 + ((offset - 1) >> 3);
-00090 
-00091         MSG_WAR(0x3015, "  End scan mapped variable", 0);
-00092 
-00093         return 0;
-00094 }
-00095 
-00104 UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex )
-00105 {
-00106   UNS32 * pwCobId;
-00107   UNS16 offset = d->firstIndex->PDO_RCV;
-00108   UNS16 lastIndex = d->lastIndex->PDO_RCV;
-00109 
-00110   /* Sending the request only if the cobid have been found on the PDO
-00111      receive */
-00112   /* part dictionary */
-00113   
-00114   MSG_WAR(0x3930, "sendPDOrequest RPDO Index : ",RPDOIndex);
-00115   
-00116   if (offset && RPDOIndex >= 0x1400){
-00117     offset += RPDOIndex - 0x1400;  
-00118     if (offset <= lastIndex) {
-00119       /* get the CobId*/
-00120       pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject;
-00121 
-00122       MSG_WAR(0x3930, "sendPDOrequest cobId is : ",*pwCobId);
-00123 
-00124       Message pdo = {*pwCobId, REQUEST, 0};
-00125       return canSend(d->canHandle,&pdo);
-00126     }
-00127   }
-00128   MSG_ERR(0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex);
-00129   return 0xFF;
-00130 }
-00131 
-00132 
-00141 UNS8 proceedPDO(CO_Data* d, Message *m)
-00142 {
-00143   UNS8   numPdo;
-00144   UNS8   numMap;  /* Number of the mapped varable */
-00145   UNS8 i;
-00146   UNS8 *     pMappingCount = NULL;    /* count of mapped objects... */
-00147   /* pointer to the var which is mapped to a pdo... */
-00148   /*  void *     pMappedAppObject = NULL;   */
-00149   /* pointer fo the var which holds the mapping parameter of an
-00150      mapping entry */
-00151   UNS32 *    pMappingParameter = NULL;
-00152   UNS8  *    pTransmissionType = NULL; /* pointer to the transmission
-00153                                          type */
-00154   UNS32 *    pwCobId = NULL;
-00155   UNS8       Size;
-00156   UNS8       dataType;
-00157   UNS8       offset;
-00158   UNS8       status;
-00159   UNS32      objDict;
-00160   UNS16      offsetObjdict;
-00161   UNS16      lastIndex;
-00162   
-00163   status = state2;
-00164 
-00165   MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));
-00166   offset = 0x00;
-00167   numPdo = 0;
-00168   numMap = 0;
-00169   if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a
-00170                                      request. */
-00171 
-00172     offsetObjdict = d->firstIndex->PDO_RCV;
-00173     lastIndex = d->lastIndex->PDO_RCV;
-00174 
-00175     /* study of all the PDO stored in the dictionary */
-00176     if(offsetObjdict)
-00177       while (offsetObjdict <= lastIndex) {
-00178 
-00179         switch( status ) {
-00180 
-00181         case state2:
-00182           /* get CobId of the dictionary correspondant to the received
-00183              PDO */
-00184           pwCobId = (UNS32*) d->objdict[offsetObjdict].pSubindex[1].pObject;
-00185           /* check the CobId coherance */
-00186           /*pwCobId is the cobId read in the dictionary at the state 3
-00187             */
-00188           if ( *pwCobId == (*m).cob_id.w ){
-00189             /* The cobId is recognized */
-00190             status = state4;
-00191             MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
-00192             break;
-00193           }
-00194           else {
-00195             /* cobId received does not match with those write in the
-00196               dictionnary */
-00197             numPdo++;
-00198             offsetObjdict++;
-00199             status = state2;
-00200             break;
-00201           }
-00202 
-00203             case state4:/* Get Mapped Objects Number */
-00204                /* The cobId of the message received has been found in the
-00205                  dictionnary. */
-00206                offsetObjdict = d->firstIndex->PDO_RCV_MAP;
-00207              lastIndex = d->lastIndex->PDO_RCV_MAP;
-00208              pMappingCount = (UNS8*) (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
-00209              numMap = 0;
-00210              while (numMap < *pMappingCount) {
-00211                UNS8 tmp[]= {0,0,0,0,0,0,0,0};
-00212                UNS8 ByteSize;
-00213                pMappingParameter = (UNS32*) (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
-00214                if (pMappingParameter == NULL) {
-00215                  MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
-00216                  return 0xFF;
-00217                }
-00218                /* Get the addresse of the mapped variable. */
-00219                /* detail of *pMappingParameter : */
-00220                /* The 16 hight bits contains the index, the medium 8 bits
-00221                  contains the subindex, */
-00222                /* and the lower 8 bits contains the size of the mapped
-00223                  variable. */
-00224 
-00225                Size = (UNS8)(*pMappingParameter);
-00226 
-00227                /* copy bit per bit in little endian */
-00228                CopyBits(Size, (UNS8*)&m->data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
-00229 
-00230                ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 =>
-00231                                                    2, ... */
-00232 
-00233                objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16),
-00234                                     (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),
-00235                                  tmp, &ByteSize, 0 );
-00236 
-00237                if(objDict != OD_SUCCESSFUL) {
-00238                  MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);
-00239                  MSG_WAR(0x2939, "         Mapped at index : ", (*pMappingParameter) >> 16);
-00240                  MSG_WAR(0x2940, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
-00241                  return 0xFF;
-00242                }
-00243 
-00244                MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w);
-00245                MSG_WAR(0x3943, "         Mapped at index : ", (*pMappingParameter) >> 16);
-00246                MSG_WAR(0x3944, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
-00247                /* MSG_WAR(0x3945, "                data : ",*((UNS32*)pMappedAppObject)); */
-00248                offset += Size;
-00249                numMap++;
-00250                /*TODO :  check that offset is not not greater that message size (in bit) */
-00251              } /* end loop while on mapped variables */
-00252 
-00253              offset=0x00;
-00254              numMap = 0;
-00255              return 0;
-00256 
-00257         }/* end switch status*/
-00258       }/* end while*/
-00259   }/* end if Donnees */
-00260   else if ((*m).rtr == REQUEST ){
-00261     MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);
-00262     status = state1;
-00263     offsetObjdict = d->firstIndex->PDO_TRS;
-00264     lastIndex = d->lastIndex->PDO_TRS;
-00265     if(offsetObjdict) while( offsetObjdict  <= lastIndex ){
-00266       /* study of all PDO stored in the objects dictionary */
-00267 
-00268       switch( status ){
-00269 
-00270       case state1:/* check the CobId */
-00271         /* get CobId of the dictionary which match to the received PDO
-00272          */
-00273         pwCobId = (UNS32*) (d->objdict + offsetObjdict)->pSubindex[1].pObject;
-00274         if ( *pwCobId == (*m).cob_id.w ) {
-00275           status = state4;
-00276           break;
-00277         }
-00278         else {
-00279           numPdo++;
-00280           offsetObjdict++;
-00281         }
-00282         status = state1;
-00283         break;
-00284 
-00285 
-00286       case state4:/* check transmission type */
-00287         pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject;
-00288         /* If PDO is to be sampled and send on RTR, do it*/
-00289         if ( (*pTransmissionType == TRANS_RTR)) {
-00290           status = state5;
-00291           break;
-00292         /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */
-00293         }else if ((*pTransmissionType == TRANS_RTR_SYNC )) {
-00294           if(d->PDO_status[numPdo].transmit_type_parameter & PDO_RTR_SYNC_READY){
-00295             /*Data ready, just send*/
-00296             canSend(d->canHandle,&d->PDO_status[numPdo].last_message);
-00297             return 0;
-00298           }else{
-00299             /* if SYNC did never occur, force emission with current data */
-00300             /* DS301 do not tell what to do in such a case...*/
-00301             MSG_ERR(0x1947, "Not ready RTR_SYNC TPDO send current data : ", m->cob_id.w);
-00302             status = state5;
-00303           }
-00304           break;
-00305         }else if(
-00306              (*pTransmissionType == TRANS_EVENT_PROFILE) ||
-00307              (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) {
-00308           /* Zap all timers and inhibit flag */
-00309           d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer);
-00310           d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer);
-00311           d->PDO_status[numPdo].transmit_type_parameter &= ~PDO_INHIBITED;
-00312           /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
-00313           PDOEventTimerAlarm(d, numPdo);
-00314           return 0;
-00315         }else {
-00316           /* The requested PDO is not to send on request. So, does
-00317             nothing. */
-00318           MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);
-00319           return 0xFF;
-00320         }
-00321 
-00322       case state5:/* build and send requested PDO */
-00323       {
-00324         Message pdo;
-00325         if( buildPDO(d, numPdo, &pdo))
-00326         {
-00327           MSG_ERR(0x1948, " Couldn't build TPDO n°", numPdo);
-00328           return 0xFF;
-00329         }
-00330         canSend(d->canHandle,&pdo);
-00331         return 0;
-00332       }
-00333       }/* end switch status */
-00334     }/* end while */
-00335   }/* end if Requete */
-00336 
-00337   return 0;
-00338 }
-00339 
-00351 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)
-00352 {
-00353   /* This loop copy as many bits that it can each time, crossing*/
-00354   /* successively bytes*/
-00355   // boundaries from LSB to MSB.
-00356   while(NbBits > 0)
-00357     {
-00358       /* Bit missalignement between src and dest*/
-00359       INTEGER8 Vect = DestBitIndex - SrcBitIndex;
-00360 
-00361       /* We can now get src and align it to dest*/
-00362       UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
-00363 
-00364       /* Compute the nb of bit we will be able to copy*/
-00365       UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex :  8 - SrcBitIndex );
-00366       UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
-00367 
-00368       /* Create a mask that will serve in:*/
-00369       UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
-00370 
-00371       /* - Filtering src*/
-00372       UNS8 Filtered = Aligned & ~Mask;
-00373 
-00374       /* - and erase bits where we write, preserve where we don't*/
-00375       *DestByteIndex &= Mask;
-00376 
-00377       /* Then write.*/
-00378       *DestByteIndex |= Filtered ;
-00379 
-00380       /*Compute next time cursors for src*/
-00381       if((SrcBitIndex += BitsToCopy)>7)/* cross boundary ?*/
-00382         {
-00383           SrcBitIndex = 0;/* First bit*/
-00384           SrcByteIndex += (SrcBigEndian ? -1 : 1);/* Next byte*/
-00385         }
-00386 
-00387 
-00388       /*Compute next time cursors for dest*/
-00389       if((DestBitIndex += BitsToCopy)>7)
-00390         {
-00391           DestBitIndex = 0;/* First bit*/
-00392           DestByteIndex += (DestBigEndian ? -1 : 1);/* Next byte*/
-00393         }
-00394 
-00395       /*And decrement counter.*/
-00396       NbBits -= BitsToCopy;
-00397     }
-00398 
-00399 }
-00408 UNS8 sendPDOevent( CO_Data* d)
-00409 {
-00410   /* Calls _sendPDOevent specifying it is not a sync event */
-00411   return _sendPDOevent(d, 0);
-00412 }
-00413         
-00414 
-00415 void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum)
-00416 {
-00417         /* This is needed to avoid deletion of re-attribuated timer */
-00418         d->PDO_status[pdoNum].event_timer = TIMER_NONE;
-00419         /* force emission of PDO by artificially changing last emitted*/
-00420         d->PDO_status[pdoNum].last_message.cob_id.w = 0;
-00421         _sendPDOevent( d, 0 ); /* not a Sync Event*/    
-00422 }
-00423 
-00424 void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum)
-00425 {
-00426         /* This is needed to avoid deletion of re-attribuated timer */
-00427         d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;
-00428         /* Remove inhibit flag */
-00429         d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;
-00430         _sendPDOevent( d, 0 ); /* not a Sync Event*/
-00431 }
-00432 
-00442 UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent )
-00443 { 
-00444   UNS8  pdoNum = 0x00;       /* number of the actual processed pdo-nr. */
-00445   UNS8* pTransmissionType = NULL;  
-00446   UNS8 status = state3;
-00447   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
-00448   UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
-00449   UNS16 lastIndex = d->lastIndex->PDO_TRS;  
-00450 
-00451   /* study all PDO stored in the objects dictionary */  
-00452   if(offsetObjdict){
-00453    Message pdo = Message_Initializer;
-00454    while( offsetObjdict <= lastIndex) {  
-00455     switch( status ) {
-00456     case state3:
-00457       if (/*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5)*/
-00458           /* check if TPDO is not valid */ 
-00459           *(UNS32*)d->objdict[offsetObjdict].pSubindex[0].pObject & 0x8000) {
-00460           MSG_WAR(0x3960, "Not a valid PDO ", 0x1800 + pdoNum);
-00461           /*Go next TPDO*/
-00462           status = state11;
-00463           break;
-00464         }
-00465       /* get the PDO transmission type */
-00466       pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject;    
-00467       MSG_WAR(0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);
-00468 
-00469       /* check if transmission type is SYNCRONOUS */
-00470       /* The message is transmited every n SYNC with n=TransmissionType */      
-00471       if( isSyncEvent && 
-00472           (*pTransmissionType >= TRANS_SYNC_MIN) &&
-00473           (*pTransmissionType <= TRANS_SYNC_MAX) &&
-00474           (++d->PDO_status[pdoNum].transmit_type_parameter == *pTransmissionType) ) {
-00475         /*Reset count of SYNC*/ 
-00476         d->PDO_status[pdoNum].transmit_type_parameter = 0;
-00477         MSG_WAR(0x3964, "  PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
-00478         pdo = (Message)Message_Initializer;
-00479         if(buildPDO(d, pdoNum, &pdo))
-00480         {
-00481             MSG_ERR(0x1906, " Couldn't build TPDO number : ", pdoNum);
-00482             status = state11;
-00483             break;
-00484         }
-00485         status = state5;
-00486       /* If transmission RTR, with data sampled on SYNC */
-00487       }else if( isSyncEvent && 
-00488           (*pTransmissionType == TRANS_RTR_SYNC)) {
-00489           if(buildPDO(d, pdoNum, &d->PDO_status[pdoNum].last_message))
-00490           {
-00491              MSG_ERR(0x1966, " Couldn't build TPDO number : ", pdoNum);
-00492              d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_RTR_SYNC_READY; 
-00493           }else{
-00494              d->PDO_status[pdoNum].transmit_type_parameter |= PDO_RTR_SYNC_READY; 
-00495           }
-00496           status = state11;
-00497           break;
-00498       /* If transmission on Event and not inhibited, check for changes */
-00499       }else if((*pTransmissionType == TRANS_EVENT_PROFILE ||
-00500                *pTransmissionType == TRANS_EVENT_SPECIFIC )&&
-00501               !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) {
-00502         MSG_WAR(0x3968, "  PDO is on EVENT. Trans type : ", *pTransmissionType);
-00503         pdo = (Message)Message_Initializer;
-00504         if(buildPDO(d, pdoNum, &pdo))
-00505         {
-00506             MSG_ERR(0x3907, " Couldn't build TPDO number : ", pdoNum);
-00507             status = state11;
-00508             break;
-00509         }
-00510         
-00511         /*Compare new and old PDO*/
-00512         if(d->PDO_status[pdoNum].last_message.cob_id.w == pdo.cob_id.w &&
-00513            d->PDO_status[pdoNum].last_message.len == pdo.len &&
-00514            *(UNS64*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS64*)(&pdo.data[0])){
-00515                 /* No changes -> go to next pdo*/
-00516                 status = state11;
-00517         }else{
-00518                 MSG_WAR(0x306A, "Changes TPDO number : ", pdoNum);
-00519                 /* Changes detected -> transmit message */
-00520                 status = state5;
-00521                 
-00522                 /* Start both event_timer and inhibit_timer*/
-00523                 DelAlarm(d->PDO_status[pdoNum].event_timer);
-00524                 d->PDO_status[pdoNum].event_timer = SetAlarm(d, pdoNum, &PDOEventTimerAlarm, MS_TO_TIMEVAL(*(UNS16*)d->objdict[offsetObjdict].pSubindex[5].pObject), 0);
-00525                 
-00526                 DelAlarm(d->PDO_status[pdoNum].inhibit_timer);
-00527                 d->PDO_status[pdoNum].inhibit_timer = SetAlarm(d, pdoNum, &PDOInhibitTimerAlarm, US_TO_TIMEVAL(*(UNS16*)d->objdict[offsetObjdict].pSubindex[3].pObject * 100), 0);
-00528                 
-00529                 /* and inhibit TPDO */
-00530                 d->PDO_status[pdoNum].transmit_type_parameter |= PDO_INHIBITED;
-00531         }
-00532       }else{
-00533         MSG_WAR(0x306C, "  PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", *pTransmissionType);
-00534         status = state11;
-00535       }      
-00536         break;
-00537     case state5: /*Send the pdo*/
-00538         /*store_as_last_message*/
-00539         d->PDO_status[pdoNum].last_message = pdo;       
-00540         MSG_WAR(0x396D, "sendPDO cobId :", pdo.cob_id.w);
-00541         MSG_WAR(0x396E,  "     Nb octets  : ",  pdo.len);
-00542         
-00543         canSend(d->canHandle,&pdo);
-00544         status = state11;
-00545         break;     
-00546     case state11: /*Go to next TPDO*/     
-00547         pdoNum++;
-00548         offsetObjdict++;
-00549         offsetObjdictMap++;
-00550         MSG_WAR(0x3970, "next pdo index : ", pdoNum);
-00551         status = state3;
-00552         break;
-00553       
-00554     default:
-00555       MSG_ERR(0x1972,"Unknown state has been reached : %d",status);
-00556       return 0xFF;
-00557     }/* end switch case */
-00558     
-00559   }/* end while */
-00560   }
-00561   return 0;
-00562 }
-00563 
-00573 UNS32 TPDO_Communication_Parameter_Callback(CO_Data* d, const indextable * OD_entry, UNS8 bSubindex)
-00574 {
-00575   /* If PDO are actives */
-00576   if(d->CurrentCommunicationState.csPDO) switch(bSubindex)
-00577   {
-00578         case 2: /* Changed transmition type */
-00579         case 3: /* Changed inhibit time */
-00580         case 5: /* Changed event time */
-00581         {
-00582           UNS8 pTransmissionType = *(UNS8*) OD_entry->pSubindex[2].pObject;
-00583           const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS; 
-00584           UNS8 numPdo = OD_entry - TPDO_com;       /* number of the actual processed pdo-nr. */
-00585         
-00586           /* Zap all timers and inhibit flag */
-00587           d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer);
-00588           d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer);
-00589           d->PDO_status[numPdo].transmit_type_parameter = 0;
-00590           /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
-00591           PDOEventTimerAlarm(d, numPdo);
-00592           return 0;
-00593         }
-00594 
-00595         default: /* other subindex are ignored*/
-00596         break;
-00597   }
-00598   return 0;
-00599 }
-00600 
-00601 void PDOInit(CO_Data* d)
-00602 {
-00603   /* For each TPDO mapping parameters */
-00604   UNS16 pdoIndex = 0x1800; /* OD index of TDPO */
-00605 
-00606   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
-00607   UNS16 lastIndex = d->lastIndex->PDO_TRS;
-00608   if(offsetObjdict) while( offsetObjdict <= lastIndex) {
-00609     /* Assign callbacks to sensible TPDO mapping subindexes */
-00610     UNS32 errorCode;
-00611     ODCallback_t *CallbackList;
-00612     /* Find callback list */
-00613     scanIndexOD (d, pdoIndex, &errorCode, &CallbackList);
-00614     if(errorCode == OD_SUCCESSFUL && CallbackList)
-00615     {
-00616       /*Assign callbacks to corresponding subindex*/
-00617       /* Transmission type */
-00618       CallbackList[2] = &TPDO_Communication_Parameter_Callback;
-00619       /* Inhibit time */
-00620       CallbackList[3] = &TPDO_Communication_Parameter_Callback;
-00621       /* Event timer */
-00622       CallbackList[5] = &TPDO_Communication_Parameter_Callback;
-00623     }
-00624     pdoIndex++;
-00625     offsetObjdict++;
-00626   }  
-00627 
-00628   /* Trigger a non-sync event */
-00629   _sendPDOevent( d, 0 );
-00630 }
-00631 
-00632 void PDOStop(CO_Data* d)
-00633 {
-00634   /* For each TPDO mapping parameters */
-00635   UNS8  pdoNum = 0x00;       /* number of the actual processed pdo-nr. */
-00636   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
-00637   UNS16 lastIndex = d->lastIndex->PDO_TRS;
-00638   if(offsetObjdict) while( offsetObjdict <= lastIndex) {
-00639         /* Delete TPDO timers */
-00640         d->PDO_status[pdoNum].event_timer = DelAlarm(d->PDO_status[pdoNum].event_timer);
-00641         d->PDO_status[pdoNum].inhibit_timer = DelAlarm(d->PDO_status[pdoNum].inhibit_timer);
-00642         /* Reset transmit type parameter */
-00643         d->PDO_status[pdoNum].transmit_type_parameter = 0;
-00644         d->PDO_status[pdoNum].last_message.cob_id.w = 0;
-00645         pdoNum++;
-00646         offsetObjdict++;
-00647   }  
-00648 }
-