nico@207: nico@207: nico@215: CanFestival: src/pdo.c Source File nico@207: nico@207: nico@207: nico@207: nico@207:
nico@207:
nico@207:
nico@207:
nico@215:
etisserant@240: src
nico@215:

pdo.c

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