00001 /*
 00002   This file is part of CanFestival, a library implementing CanOpen
 00003   Stack.
@@ -46,512 +46,555 @@
 00025 #include "objacces.h"
 00026 #include "canfestival.h"
 00027 
-00037 UNS8 sendPDO(CO_Data* d, s_PDO pdo, UNS8 req)
-00038 {
-00039   UNS8 i;
-00040   if( d->nodeState == Operational ) {
-00041     Message m;
-00042 
-00043     /* Message copy for sending */
-00044     m.cob_id.w = pdo.cobId & 0x7FF; /* Because the cobId is 11 bytes
-00045                                       length */
-00046     if ( req == NOT_A_REQUEST ) {
-00047       UNS8 i;
-00048       m.rtr = NOT_A_REQUEST;
-00049       m.len = pdo.len;
-00050       /* memcpy(&m.data, &pdo.data, m.len); */
-00051       /* This Memcpy depends on packing structure. Avoid */
-00052       for (i = 0 ; i < pdo.len ; i++)
-00053         m.data[i] = pdo.data[i];
-00054     }
-00055     else {
-00056       m.rtr = REQUEST;
-00057       m.len = 0;
-00058     }
+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(0x3901, "sendPDO cobId :", m.cob_id.w);
-00061     MSG_WAR(0x3902,  "     Nb octets  : ",  m.len);
-00062     for (i = 0 ; i < m.len ; i++) {
-00063       MSG_WAR(0x3903,"           data : ", m.data[i]);
-00064     }
-00065 
-00066     return canSend(d->canHandle,&m);
-00067   } /* end if */
-00068   return 0xFF;
-00069 }
-00070 
-00079 UNS8 PDOmGR(CO_Data* d, UNS32 cobId)
-00080 {
-00081   UNS8 res;
-00082   UNS8 i;
-00083   s_PDO pdo;
+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   MSG_WAR(0x3905, "PDOmGR",0);
-00086 
-00087   /* if PDO is waiting for transmission,
-00088     preparation of the message to send */
-00089   pdo.cobId = cobId;
-00090   pdo.len =  d->process_var.count;
-00091   /* memcpy(&(pdo.data), &(process_var.data), pdo.len); */
-00092      /* Ce memcpy devrait tre portable */
-00093     for ( i = 0 ; i < pdo.len ; i++) 
-00094       pdo.data[i] = d->process_var.data[i];
+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 
-00096     res = sendPDO(d, pdo, NOT_A_REQUEST);
-00097 
-00098     return res;
-00099 }
-00100 
-00101 #if 0
-00102 /*********************************************************************/
-00103 /* TODO : implement bit mapping                                                  */
-00104 /*********************************************************************/
-00105 
-00106 UNS8 buildPDO(CO_Data* d, UNS16 index)
-00107 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */       
-00108   UNS16 ind;
-00109   UNS8      subInd;
-00110 
-00111   UNS8 *     pMappingCount = NULL;      /* count of mapped objects... */
-00112   /* pointer to the var which is mapped to a pdo */
-00113 /*  void *     pMappedAppObject = NULL;  */
-00114   /* pointer fo the var which holds the mapping parameter of an mapping entry  */ 
-00115  
-00116   UNS32 *    pMappingParameter = NULL;
-00117 
-00118   UNS8      Size;
-00119   UNS8      dataType;
-00120   UNS8      offset;
-00121   UNS16     offsetObjdict;
-00122   UNS16     offsetObjdictPrm;
-00123   UNS32     objDict;
-00124 
-00125   subInd=(UNS8)0x00;
-00126   offset = 0x00;
-00127   ind = index - 0x1800;
-00128 
-00129   MSG_WAR(0x3910,"Prepare PDO to send index :", index);
-00130 
-00131   /* only operational state allows PDO transmission */
-00132   if( d->nodeState != Operational ) {
-00133     MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index);
-00134     return 0xFF;
-00135   }
-00136   offsetObjdictPrm = d->firstIndex->PDO_TRS;
-00137   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
-00138 
-00139   if (offsetObjdictPrm && offsetObjdict)
-00140     {
-00141       /* get mapped objects number to transmit with this PDO */
-00142       pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject;
-00143       MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount);
-00144       MSG_WAR(0x3913, "        at index : ", 0x1A00 + ind);
-00145       while (subInd < *pMappingCount) { /* Loop on mapped variables */
-00146         /* get mapping parameters */
-00147         pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject;
-00148         MSG_WAR(0x3914, "Get the mapping      at index : ", (UNS16)0x1A00 + ind);
-00149         MSG_WAR(0x3915, "                     subIndex : ", subInd + 1);
-00150         MSG_WAR(0x3916, "                     value    : ", *(UNS32 *)pMappingParameter);
-00151         /* Get the mapped variable */
-00152         Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
-00153         objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16),
-00154                              (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF),
-00155                              (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
-00156 
-00157         if (objDict != OD_SUCCESSFUL) {
-00158           MSG_WAR(0x2919, "error accessing to the mapped var : ", subInd + 1);
-00159           MSG_WAR(0x2920, "         Mapped at index : ", (*pMappingParameter) >> 16);
-00160           MSG_WAR(0x2921, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
-00161           return 0xFF;
-00162         }
-00163 
-00164         offset += Size;
-00165         d->process_var.count = offset;
-00166         subInd++;
-00167       }/* end Loop on mapped variables  */
-00168     }
-00169   return 0;
-00170 }
-00171 #endif
-00172 
-00181 UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId )
-00182 {
-00183   UNS32 * pwCobId;
-00184   UNS16          offset;
-00185   UNS16          lastIndex;
-00186   UNS8           err;
-00187 
-00188   MSG_WAR(0x3930, "sendPDOrequest ",0);
-00189   /* Sending the request only if the cobid have been found on the PDO
-00190      receive */
-00191   /* part dictionary */
-00192   offset = d->firstIndex->PDO_RCV;
-00193   lastIndex = d->lastIndex->PDO_RCV;
-00194   if (offset)
-00195     while (offset <= lastIndex) {
-00196       /* get the CobId*/
-00197       pwCobId = d->objdict[offset].pSubindex[1].pObject;
-00198 
-00199       if ( *pwCobId  == cobId ) {
-00200         s_PDO pdo;
-00201         pdo.cobId = *pwCobId;
-00202         pdo.len = 0;
-00203         err  = sendPDO(d, pdo, REQUEST);
-00204         return err;
-00205       }
-00206       offset++;
-00207     }
-00208   MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId);
-00209   return 0xFF;
-00210 }
-00211 
-00212 
-00221 UNS8 proceedPDO(CO_Data* d, Message *m)
-00222 {
-00223   UNS8   numPdo;
-00224   UNS8   numMap;  /* Number of the mapped varable */
-00225   UNS8 i;
-00226   UNS8 *     pMappingCount = NULL;    /* count of mapped objects... */
-00227   /* pointer to the var which is mapped to a pdo... */
-00228   /*  void *     pMappedAppObject = NULL;   */
-00229   /* pointer fo the var which holds the mapping parameter of an
-00230      mapping entry */
-00231   UNS32 *    pMappingParameter = NULL;
-00232   UNS8  *    pTransmissionType = NULL; /* pointer to the transmission
-00233                                          type */
-00234   UNS32 *    pwCobId = NULL;
-00235   UNS8       Size;
-00236   UNS8       dataType;
-00237   UNS8       offset;
-00238   UNS8       status;
-00239   UNS32      objDict;
-00240   UNS16      offsetObjdict;
-00241   UNS16      lastIndex;
-00242   status = state1;
+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(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));
-00245   offset = 0x00;
-00246   numPdo = 0;
-00247   numMap = 0;
-00248   if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a
-00249                                      request. */
-00250     offsetObjdict = d->firstIndex->PDO_RCV;
-00251     lastIndex = d->lastIndex->PDO_RCV;
+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     /* study of all the PDO stored in the dictionary */
-00254     if(offsetObjdict)
-00255       while (offsetObjdict <= lastIndex) {
+00253              offset=0x00;
+00254              numMap = 0;
+00255              return 0;
 00256 
-00257         switch( status ) {
-00258 
-00259         case state1:/* data are stored in process_var array */
-00260           /* memcpy(&(process_var.data), &m->data, (*m).len); */
-00261           /* Ce memcpy devrait etre portable */
-00262           for ( i = 0 ; i < m->len ; i++)
-00263             d->process_var.data[i] = m->data[i];
-00264           d->process_var.count = (*m).len;
-00265 
-00266           status = state2;
-00267           break;
-00268 
-00269         case state2:
-00270           /* get CobId of the dictionary correspondant to the received
-00271              PDO */
-00272           pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;
-00273           /* check the CobId coherance */
-00274           /*pwCobId is the cobId read in the dictionary at the state 3
-00275             */
-00276           if ( *pwCobId == (*m).cob_id.w ){
-00277             /* The cobId is recognized */
-00278             status = state4;
-00279             MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
-00280             break;
-00281           }
-00282           else {
-00283             /* cobId received does not match with those write in the
-00284               dictionnary */
-00285             numPdo++;
-00286             offsetObjdict++;
-00287             status = state2;
-00288             break;
-00289           }
-00290 
-00291             case state4:/* Get Mapped Objects Number */
-00292                /* The cobId of the message received has been found in the
-00293                  dictionnary. */
-00294                offsetObjdict = d->firstIndex->PDO_RCV_MAP;
-00295              lastIndex = d->lastIndex->PDO_RCV_MAP;
-00296              pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
-00297              numMap = 0;
-00298              while (numMap < *pMappingCount) {
-00299                UNS8 tmp[]= {0,0,0,0,0,0,0,0};
-00300                UNS8 ByteSize;
-00301                pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
-00302                if (pMappingParameter == NULL) {
-00303                  MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
-00304                  return 0xFF;
-00305                }
-00306                /* Get the addresse of the mapped variable. */
-00307                /* detail of *pMappingParameter : */
-00308                /* The 16 hight bits contains the index, the medium 8 bits
-00309                  contains the subindex, */
-00310                /* and the lower 8 bits contains the size of the mapped
-00311                  variable. */
-00312 
-00313                Size = (UNS8)(*pMappingParameter);
-00314 
-00315                /* copy bit per bit in little endian */
-00316                CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
-00317 
-00318                ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 =>
-00319                                                    2, ... */
-00320 
-00321                objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16),
-00322                                     (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),
-00323                                  tmp, &ByteSize, 0 );
-00324 
-00325                if(objDict != OD_SUCCESSFUL) {
-00326                  MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);
-00327                  MSG_WAR(0x2939, "         Mapped at index : ", (*pMappingParameter) >> 16);
-00328                  MSG_WAR(0x2940, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
-00329                  return 0xFF;
-00330                }
-00331 
-00332                MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w);
-00333                MSG_WAR(0x3943, "         Mapped at index : ", (*pMappingParameter) >> 16);
-00334                MSG_WAR(0x3944, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
-00335                /* MSG_WAR(0x3945, "                data : ",*((UNS32*)pMappedAppObject)); */
-00336                offset += Size;
-00337                numMap++;
-00338              } /* end loop while on mapped variables */
+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 
-00340              offset=0x00;
-00341              numMap = 0;
-00342              return 0;
-00343 
-00344         }/* end switch status*/
-00345       }/* end while*/
-00346   }/* end if Donnees */
-00347   else if ((*m).rtr == REQUEST ){
-00348     MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);
-00349     status = state1;
-00350     offsetObjdict = d->firstIndex->PDO_TRS;
-00351     lastIndex = d->lastIndex->PDO_TRS;
-00352     if(offsetObjdict) while( offsetObjdict  <= lastIndex ){
-00353       /* study of all PDO stored in the objects dictionary */
-00354 
-00355       switch( status ){
-00356 
-00357       case state1:/* check the CobId */
-00358         /* get CobId of the dictionary which match to the received PDO
-00359          */
-00360         pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject;
-00361         if ( *pwCobId == (*m).cob_id.w ) {
-00362           status = state4;
-00363           break;
-00364         }
-00365         else {
-00366           numPdo++;
-00367           offsetObjdict++;
-00368         }
-00369         status = state1;
-00370         break;
-00371 
-00372 
-00373       case state4:/* check transmission type (after request?) */
-00374         pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;
-00375         if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) {
-00376           status = state5;
-00377           break;
-00378         }
-00379         else {
-00380           /* The requested PDO is not to send on request. So, does
-00381             nothing. */
-00382           MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);
-00383           return 0xFF;
-00384         }
-00385 
-00386       case state5:/* get mapped objects number */
-00387         offsetObjdict = d->firstIndex->PDO_TRS_MAP;
-00388         lastIndex = d->lastIndex->PDO_TRS_MAP;
-00389         pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
-00390         numMap = 0;
-00391         while (numMap < *pMappingCount) {
-00392           pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
-00393           /* Get the mapped variable */
-00394           Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
-00395           objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16),
-00396                                 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF),
-00397                                 (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
-00398           if (objDict != OD_SUCCESSFUL) {
-00399             MSG_ERR(0x1948, "error accessing to the mapped var : ", numMap + 1);
-00400             MSG_WAR(0x2949, "         Mapped at index : ", (*pMappingParameter) >> 16);
-00401             MSG_WAR(0x2950, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
-00402             return 0xFF;
-00403           }
-00404           offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3);
-00405           d->process_var.count = offset;
-00406           numMap++;
-00407 
-00408         } /* end while */
-00409         PDOmGR( d, *pwCobId ); /* Transmit the PDO */
-00410         return 0;
-00411 
-00412       }/* end switch status */
-00413     }/* end while */
-00414   }/* end if Requete */
-00415 
-00416   return 0;
-00417 }
-00418 
-00430 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)
-00431 {
-00432   /* This loop copy as many bits that it can each time, crossing*/
-00433   /* successively bytes*/
-00434   // boundaries from LSB to MSB.
-00435   while(NbBits > 0)
-00436     {
-00437       /* Bit missalignement between src and dest*/
-00438       INTEGER8 Vect = DestBitIndex - SrcBitIndex;
-00439 
-00440       /* We can now get src and align it to dest*/
-00441       UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
-00442 
-00443       /* Compute the nb of bit we will be able to copy*/
-00444       UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex :  8 - SrcBitIndex );
-00445       UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
-00446 
-00447       /* Create a mask that will serve in:*/
-00448       UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
-00449 
-00450       /* - Filtering src*/
-00451       UNS8 Filtered = Aligned & ~Mask;
-00452 
-00453       /* - and erase bits where we write, preserve where we don't*/
-00454       *DestByteIndex &= Mask;
-00455 
-00456       /* Then write.*/
-00457       *DestByteIndex |= Filtered ;
-00458 
-00459       /*Compute next time cursors for src*/
-00460       if((SrcBitIndex += BitsToCopy)>7)/* cross boundary ?*/
-00461         {
-00462           SrcBitIndex = 0;/* First bit*/
-00463           SrcByteIndex += (SrcBigEndian ? -1 : 1);/* Next byte*/
+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 
-00466 
-00467       /*Compute next time cursors for dest*/
-00468       if((DestBitIndex += BitsToCopy)>7)
-00469         {
-00470           DestBitIndex = 0;/* First bit*/
-00471           DestByteIndex += (DestBigEndian ? -1 : 1);/* Next byte*/
-00472         }
-00473 
-00474       /*And decrement counter.*/
-00475       NbBits -= BitsToCopy;
-00476     }
-00477 
-00478 }
-00479 
-00480 #if 0
-00481 
-00482 /*********************************************************************/
-00483 /* TODO : reimplement this using CallBacks
-00484  */
-00485 /*********************************************************************/
-00486 
-00495 UNS8 sendPDOevent( CO_Data* d, void * variable )
-00496 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite
-00497     loop if it fails.*/
-00498   UNS32           objDict = 0;
-00499   UNS8            ind, sub_ind;
-00500   UNS8            status;
-00501   UNS8            offset;
-00502   UNS8 *     pMappingCount = NULL;
-00503   UNS32 *    pMappingParameter = NULL;
-00504   void *     pMappedAppObject = NULL;
-00505   UNS8 *     pTransmissionType = NULL; /* pointer to the transmission
-00506                                          type */
-00507   UNS32 *    pwCobId = NULL;
-00508   UNS8 *     pSize;
-00509   UNS8       size;
-00510   UNS8       dataType;
-00511   UNS16      offsetObjdict;
-00512   UNS16      offsetObjdictPrm;
-00513   UNS16      lastIndex;
-00514   UNS8       numMap;
-00515   ind     = 0x00;
-00516   sub_ind = 1;
-00517   offset  = 0x00;
-00518   pSize   = &size;
-00519   status  = state1;
-00520 
-00521 
-00522   /* look for the index and subindex where the variable is mapped */
-00523   /* Then, send the pdo which contains the variable. */
-00524 
-00525   MSG_WAR (0x3960, "sendPDOevent", 0);
-00526   offsetObjdictPrm = d->firstIndex->PDO_TRS;
-00527 
-00528   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
-00529   lastIndex = d->lastIndex->PDO_TRS_MAP;
-00530 
-00531   if (offsetObjdictPrm && offsetObjdict)
-00532     /* Loop on PDO Transmit */
-00533     while(offsetObjdict <= lastIndex){
-00534       /* Check the transmission mode */
-00535       pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject;
-00536       if (*pTransmissionType != TRANS_EVENT) {
-00537         ind++;
-00538         offsetObjdict++;
-00539         offsetObjdictPrm++;
-00540         continue;
-00541       }
-00542       pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject;
-00543       numMap = 1; /* mapped variable */
-00544       while (numMap <= *pMappingCount) {
-00545         pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject;
-00546         /* Get the variable */
-00547         objDict = getODentry( d,
-00548                               (UNS16)((*pMappingParameter) >> 16),
-00549                               (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF),
-00550                               (void * *)&pMappedAppObject, pSize, &dataType, 0 );
-00551         if( objDict != OD_SUCCESSFUL ) {
-00552           MSG_WAR(0x2961, "Error in dict. at index : ",
-00553                   (*pMappingParameter) >> (UNS8)16);
-00554 
-00555           MSG_WAR(0x2962, "               subindex : ",
-00556                   ((*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
-00557           return 0xFF;
-00558         }
-00559         if (pMappedAppObject == variable) { // Variable found !
-00560           MSG_WAR(0x3963, "Variable to send found at index : ",
-00561                   (*pMappingParameter) >> 16);
-00562           MSG_WAR(0x3964, "                       subIndex : ",
-00563                   ((*pMappingParameter) >> 8 ) & 0x000000FF);
-00564           buildPDO(d, 0x1800 + ind);
-00565           /* Get the cobId */
-00566           pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject;
-00567           PDOmGR( d, *pwCobId ); /* Send the PDO */
-00568           return 0;
-00569         }
-00570         numMap++;
-00571       } /* End loop on mapped variable */
-00572       ind++;
-00573       offsetObjdict++;
-00574       offsetObjdictPrm++;
-00575     } /* End loop while on PDO */
-00576 
-00577   MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0);
-00578   return 0xFF;
-00579 
-00580 }
-00581 #endif
-00582 
-00583 
-00584 
-