pdo.c

Go to the documentation of this 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 }

Generated on Mon Jul 2 19:10:16 2007 for CanFestival by  doxygen 1.5.1