src/pdo.c
changeset 215 f49e5a6b7804
parent 208 05d95c45b388
child 217 94c3f89bc3cc
equal deleted inserted replaced
214:003cc3c63855 215:f49e5a6b7804
    38 {
    38 {
    39   UNS8 i;
    39   UNS8 i;
    40   if( d->nodeState == Operational ) {
    40   if( d->nodeState == Operational ) {
    41     Message m;
    41     Message m;
    42 
    42 
    43     /*! Message copy for sending */
    43     /* Message copy for sending */
    44     m.cob_id.w = pdo.cobId & 0x7FF; /*! Because the cobId is 11 bytes
    44     m.cob_id.w = pdo.cobId & 0x7FF; /* Because the cobId is 11 bytes
    45                                       length */
    45                                       length */
    46     if ( req == NOT_A_REQUEST ) {
    46     if ( req == NOT_A_REQUEST ) {
    47       UNS8 i;
    47       UNS8 i;
    48       m.rtr = NOT_A_REQUEST;
    48       m.rtr = NOT_A_REQUEST;
    49       m.len = pdo.len;
    49       m.len = pdo.len;
    50       /*! memcpy(&m.data, &pdo.data, m.len); */
    50       /* memcpy(&m.data, &pdo.data, m.len); */
    51       /*! This Memcpy depends on packing structure. Avoid */
    51       /* This Memcpy depends on packing structure. Avoid */
    52       for (i = 0 ; i < pdo.len ; i++)
    52       for (i = 0 ; i < pdo.len ; i++)
    53         m.data[i] = pdo.data[i];
    53         m.data[i] = pdo.data[i];
    54     }
    54     }
    55     else {
    55     else {
    56       m.rtr = REQUEST;
    56       m.rtr = REQUEST;
    62     for (i = 0 ; i < m.len ; i++) {
    62     for (i = 0 ; i < m.len ; i++) {
    63       MSG_WAR(0x3903,"           data : ", m.data[i]);
    63       MSG_WAR(0x3903,"           data : ", m.data[i]);
    64     }
    64     }
    65 
    65 
    66     return canSend(d->canHandle,&m);
    66     return canSend(d->canHandle,&m);
    67   } /*! end if */
    67   } /* end if */
    68   return 0xFF;
    68   return 0xFF;
    69 }
    69 }
    70 
    70 
    71 /*!
    71 /*!
    72 ** PDO Manager
    72 ** PDO Manager
    82   UNS8 i;
    82   UNS8 i;
    83   s_PDO pdo;
    83   s_PDO pdo;
    84 
    84 
    85   MSG_WAR(0x3905, "PDOmGR",0);
    85   MSG_WAR(0x3905, "PDOmGR",0);
    86 
    86 
    87   /*! if PDO is waiting for transmission,
    87   /* if PDO is waiting for transmission,
    88     preparation of the message to send */
    88     preparation of the message to send */
    89   pdo.cobId = cobId;
    89   pdo.cobId = cobId;
    90   pdo.len =  d->process_var.count;
    90   pdo.len =  d->process_var.count;
    91   /*! memcpy(&(pdo.data), &(process_var.data), pdo.len); */
    91   /* memcpy(&(pdo.data), &(process_var.data), pdo.len); */
    92      /* Ce memcpy devrait tre portable */
    92      /* Ce memcpy devrait tre portable */
    93     for ( i = 0 ; i < pdo.len ; i++) 
    93     for ( i = 0 ; i < pdo.len ; i++) 
    94       pdo.data[i] = d->process_var.data[i];
    94       pdo.data[i] = d->process_var.data[i];
    95 
    95 
    96     res = sendPDO(d, pdo, NOT_A_REQUEST);
    96     res = sendPDO(d, pdo, NOT_A_REQUEST);
   126   offset = 0x00;
   126   offset = 0x00;
   127   ind = index - 0x1800;
   127   ind = index - 0x1800;
   128 
   128 
   129   MSG_WAR(0x3910,"Prepare PDO to send index :", index);
   129   MSG_WAR(0x3910,"Prepare PDO to send index :", index);
   130 
   130 
   131   /*! only operational state allows PDO transmission */
   131   /* only operational state allows PDO transmission */
   132   if( d->nodeState != Operational ) {
   132   if( d->nodeState != Operational ) {
   133     MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index);
   133     MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index);
   134     return 0xFF;
   134     return 0xFF;
   135   }
   135   }
   136   offsetObjdictPrm = d->firstIndex->PDO_TRS;
   136   offsetObjdictPrm = d->firstIndex->PDO_TRS;
   137   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
   137   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
   138 
   138 
   139   if (offsetObjdictPrm && offsetObjdict)
   139   if (offsetObjdictPrm && offsetObjdict)
   140     {
   140     {
   141       /*! get mapped objects number to transmit with this PDO */
   141       /* get mapped objects number to transmit with this PDO */
   142       pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject;
   142       pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject;
   143       MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount);
   143       MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount);
   144       MSG_WAR(0x3913, "        at index : ", 0x1A00 + ind);
   144       MSG_WAR(0x3913, "        at index : ", 0x1A00 + ind);
   145       while (subInd < *pMappingCount) { /*! Loop on mapped variables */
   145       while (subInd < *pMappingCount) { /* Loop on mapped variables */
   146         /*! get mapping parameters */
   146         /* get mapping parameters */
   147         pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject;
   147         pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject;
   148         MSG_WAR(0x3914, "Get the mapping      at index : ", (UNS16)0x1A00 + ind);
   148         MSG_WAR(0x3914, "Get the mapping      at index : ", (UNS16)0x1A00 + ind);
   149         MSG_WAR(0x3915, "                     subIndex : ", subInd + 1);
   149         MSG_WAR(0x3915, "                     subIndex : ", subInd + 1);
   150         MSG_WAR(0x3916, "                     value    : ", *(UNS32 *)pMappingParameter);
   150         MSG_WAR(0x3916, "                     value    : ", *(UNS32 *)pMappingParameter);
   151         /*! Get the mapped variable */
   151         /* Get the mapped variable */
   152         Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
   152         Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
   153         objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16),
   153         objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16),
   154                              (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF),
   154                              (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF),
   155                              (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
   155                              (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
   156 
   156 
   162         }
   162         }
   163 
   163 
   164         offset += Size;
   164         offset += Size;
   165         d->process_var.count = offset;
   165         d->process_var.count = offset;
   166         subInd++;
   166         subInd++;
   167       }/*! end Loop on mapped variables  */
   167       }/* end Loop on mapped variables  */
   168     }
   168     }
   169   return 0;
   169   return 0;
   170 }
   170 }
   171 #endif
   171 #endif
   172 
   172 
   184   UNS16          offset;
   184   UNS16          offset;
   185   UNS16          lastIndex;
   185   UNS16          lastIndex;
   186   UNS8           err;
   186   UNS8           err;
   187 
   187 
   188   MSG_WAR(0x3930, "sendPDOrequest ",0);
   188   MSG_WAR(0x3930, "sendPDOrequest ",0);
   189   /*! Sending the request only if the cobid have been found on the PDO
   189   /* Sending the request only if the cobid have been found on the PDO
   190      receive */
   190      receive */
   191   /*! part dictionary */
   191   /* part dictionary */
   192   offset = d->firstIndex->PDO_RCV;
   192   offset = d->firstIndex->PDO_RCV;
   193   lastIndex = d->lastIndex->PDO_RCV;
   193   lastIndex = d->lastIndex->PDO_RCV;
   194   if (offset)
   194   if (offset)
   195     while (offset <= lastIndex) {
   195     while (offset <= lastIndex) {
   196       /*! get the CobId*/
   196       /* get the CobId*/
   197       pwCobId = d->objdict[offset].pSubindex[1].pObject;
   197       pwCobId = d->objdict[offset].pSubindex[1].pObject;
   198 
   198 
   199       if ( *pwCobId  == cobId ) {
   199       if ( *pwCobId  == cobId ) {
   200         s_PDO pdo;
   200         s_PDO pdo;
   201         pdo.cobId = *pwCobId;
   201         pdo.cobId = *pwCobId;
   219 ** @return
   219 ** @return
   220 **/
   220 **/
   221 UNS8 proceedPDO(CO_Data* d, Message *m)
   221 UNS8 proceedPDO(CO_Data* d, Message *m)
   222 {
   222 {
   223   UNS8   numPdo;
   223   UNS8   numPdo;
   224   UNS8   numMap;  /*! Number of the mapped varable */
   224   UNS8   numMap;  /* Number of the mapped varable */
   225   UNS8 i;
   225   UNS8 i;
   226   UNS8 *     pMappingCount = NULL;    /*! count of mapped objects... */
   226   UNS8 *     pMappingCount = NULL;    /* count of mapped objects... */
   227   /*! pointer to the var which is mapped to a pdo... */
   227   /* pointer to the var which is mapped to a pdo... */
   228   /*!  void *     pMappedAppObject = NULL;   */
   228   /*  void *     pMappedAppObject = NULL;   */
   229   /*! pointer fo the var which holds the mapping parameter of an
   229   /* pointer fo the var which holds the mapping parameter of an
   230      mapping entry */
   230      mapping entry */
   231   UNS32 *    pMappingParameter = NULL;
   231   UNS32 *    pMappingParameter = NULL;
   232   UNS8  *    pTransmissionType = NULL; /*! pointer to the transmission
   232   UNS8  *    pTransmissionType = NULL; /* pointer to the transmission
   233                                          type */
   233                                          type */
   234   UNS32 *    pwCobId = NULL;
   234   UNS32 *    pwCobId = NULL;
   235   UNS8       Size;
   235   UNS8       Size;
   236   UNS8       dataType;
   236   UNS8       dataType;
   237   UNS8       offset;
   237   UNS8       offset;
   243 
   243 
   244   MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));
   244   MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));
   245   offset = 0x00;
   245   offset = 0x00;
   246   numPdo = 0;
   246   numPdo = 0;
   247   numMap = 0;
   247   numMap = 0;
   248   if((*m).rtr == NOT_A_REQUEST ) { /*! The PDO received is not a
   248   if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a
   249                                      request. */
   249                                      request. */
   250     offsetObjdict = d->firstIndex->PDO_RCV;
   250     offsetObjdict = d->firstIndex->PDO_RCV;
   251     lastIndex = d->lastIndex->PDO_RCV;
   251     lastIndex = d->lastIndex->PDO_RCV;
   252 
   252 
   253     /*! study of all the PDO stored in the dictionary */
   253     /* study of all the PDO stored in the dictionary */
   254     if(offsetObjdict)
   254     if(offsetObjdict)
   255       while (offsetObjdict <= lastIndex) {
   255       while (offsetObjdict <= lastIndex) {
   256 
   256 
   257         switch( status ) {
   257         switch( status ) {
   258 
   258 
   259         case state1:/*! data are stored in process_var array */
   259         case state1:/* data are stored in process_var array */
   260           /*! memcpy(&(process_var.data), &m->data, (*m).len); */
   260           /* memcpy(&(process_var.data), &m->data, (*m).len); */
   261           /*! Ce memcpy devrait etre portable */
   261           /* Ce memcpy devrait etre portable */
   262           for ( i = 0 ; i < m->len ; i++)
   262           for ( i = 0 ; i < m->len ; i++)
   263             d->process_var.data[i] = m->data[i];
   263             d->process_var.data[i] = m->data[i];
   264           d->process_var.count = (*m).len;
   264           d->process_var.count = (*m).len;
   265 
   265 
   266           status = state2;
   266           status = state2;
   267           break;
   267           break;
   268 
   268 
   269         case state2:
   269         case state2:
   270           /*! get CobId of the dictionary correspondant to the received
   270           /* get CobId of the dictionary correspondant to the received
   271              PDO */
   271              PDO */
   272           pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;
   272           pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;
   273           /*! check the CobId coherance */
   273           /* check the CobId coherance */
   274           /*!pwCobId is the cobId read in the dictionary at the state 3
   274           /*pwCobId is the cobId read in the dictionary at the state 3
   275             */
   275             */
   276           if ( *pwCobId == (*m).cob_id.w ){
   276           if ( *pwCobId == (*m).cob_id.w ){
   277             /*! The cobId is recognized */
   277             /* The cobId is recognized */
   278             status = state4;
   278             status = state4;
   279             MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
   279             MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
   280             break;
   280             break;
   281           }
   281           }
   282           else {
   282           else {
   283             /*! cobId received does not match with those write in the
   283             /* cobId received does not match with those write in the
   284               dictionnary */
   284               dictionnary */
   285             numPdo++;
   285             numPdo++;
   286             offsetObjdict++;
   286             offsetObjdict++;
   287             status = state2;
   287             status = state2;
   288             break;
   288             break;
   289           }
   289           }
   290 
   290 
   291             case state4:/*! Get Mapped Objects Number */
   291             case state4:/* Get Mapped Objects Number */
   292                /*! The cobId of the message received has been found in the
   292                /* The cobId of the message received has been found in the
   293                  dictionnary. */
   293                  dictionnary. */
   294                offsetObjdict = d->firstIndex->PDO_RCV_MAP;
   294                offsetObjdict = d->firstIndex->PDO_RCV_MAP;
   295              lastIndex = d->lastIndex->PDO_RCV_MAP;
   295              lastIndex = d->lastIndex->PDO_RCV_MAP;
   296              pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
   296              pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
   297              numMap = 0;
   297              numMap = 0;
   301                pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
   301                pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
   302                if (pMappingParameter == NULL) {
   302                if (pMappingParameter == NULL) {
   303                  MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
   303                  MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
   304                  return 0xFF;
   304                  return 0xFF;
   305                }
   305                }
   306                /*! Get the addresse of the mapped variable. */
   306                /* Get the addresse of the mapped variable. */
   307                /*! detail of *pMappingParameter : */
   307                /* detail of *pMappingParameter : */
   308                /*! The 16 hight bits contains the index, the medium 8 bits
   308                /* The 16 hight bits contains the index, the medium 8 bits
   309                  contains the subindex, */
   309                  contains the subindex, */
   310                /*! and the lower 8 bits contains the size of the mapped
   310                /* and the lower 8 bits contains the size of the mapped
   311                  variable. */
   311                  variable. */
   312 
   312 
   313                Size = (UNS8)(*pMappingParameter);
   313                Size = (UNS8)(*pMappingParameter);
   314 
   314 
   315                /*! copy bit per bit in little endian */
   315                /* copy bit per bit in little endian */
   316                CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
   316                CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
   317 
   317 
   318                ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 =>
   318                ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 =>
   319                                                    2, ... */
   319                                                    2, ... */
   320 
   320 
   330                }
   330                }
   331 
   331 
   332                MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w);
   332                MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w);
   333                MSG_WAR(0x3943, "         Mapped at index : ", (*pMappingParameter) >> 16);
   333                MSG_WAR(0x3943, "         Mapped at index : ", (*pMappingParameter) >> 16);
   334                MSG_WAR(0x3944, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
   334                MSG_WAR(0x3944, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
   335                /*! MSG_WAR(0x3945, "                data : ",*((UNS32*)pMappedAppObject)); */
   335                /* MSG_WAR(0x3945, "                data : ",*((UNS32*)pMappedAppObject)); */
   336                offset += Size;
   336                offset += Size;
   337                numMap++;
   337                numMap++;
   338              } /*! end loop while on mapped variables */
   338              } /* end loop while on mapped variables */
   339 
   339 
   340              offset=0x00;
   340              offset=0x00;
   341              numMap = 0;
   341              numMap = 0;
   342              return 0;
   342              return 0;
   343 
   343 
   344         }/*! end switch status*/
   344         }/* end switch status*/
   345       }/*! end while*/
   345       }/* end while*/
   346   }/*! end if Donnees */
   346   }/* end if Donnees */
   347   else if ((*m).rtr == REQUEST ){
   347   else if ((*m).rtr == REQUEST ){
   348     MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);
   348     MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);
   349     status = state1;
   349     status = state1;
   350     offsetObjdict = d->firstIndex->PDO_TRS;
   350     offsetObjdict = d->firstIndex->PDO_TRS;
   351     lastIndex = d->lastIndex->PDO_TRS;
   351     lastIndex = d->lastIndex->PDO_TRS;
   352     if(offsetObjdict) while( offsetObjdict  <= lastIndex ){
   352     if(offsetObjdict) while( offsetObjdict  <= lastIndex ){
   353       /*! study of all PDO stored in the objects dictionary */
   353       /* study of all PDO stored in the objects dictionary */
   354 
   354 
   355       switch( status ){
   355       switch( status ){
   356 
   356 
   357       case state1:/*! check the CobId */
   357       case state1:/* check the CobId */
   358         /*! get CobId of the dictionary which match to the received PDO
   358         /* get CobId of the dictionary which match to the received PDO
   359          */
   359          */
   360         pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject;
   360         pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject;
   361         if ( *pwCobId == (*m).cob_id.w ) {
   361         if ( *pwCobId == (*m).cob_id.w ) {
   362           status = state4;
   362           status = state4;
   363           break;
   363           break;
   368         }
   368         }
   369         status = state1;
   369         status = state1;
   370         break;
   370         break;
   371 
   371 
   372 
   372 
   373       case state4:/*! check transmission type (after request?) */
   373       case state4:/* check transmission type (after request?) */
   374         pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;
   374         pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;
   375         if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) {
   375         if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) {
   376           status = state5;
   376           status = state5;
   377           break;
   377           break;
   378         }
   378         }
   379         else {
   379         else {
   380           /*! The requested PDO is not to send on request. So, does
   380           /* The requested PDO is not to send on request. So, does
   381             nothing. */
   381             nothing. */
   382           MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);
   382           MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);
   383           return 0xFF;
   383           return 0xFF;
   384         }
   384         }
   385 
   385 
   386       case state5:/*! get mapped objects number */
   386       case state5:/* get mapped objects number */
   387         offsetObjdict = d->firstIndex->PDO_TRS_MAP;
   387         offsetObjdict = d->firstIndex->PDO_TRS_MAP;
   388         lastIndex = d->lastIndex->PDO_TRS_MAP;
   388         lastIndex = d->lastIndex->PDO_TRS_MAP;
   389         pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
   389         pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
   390         numMap = 0;
   390         numMap = 0;
   391         while (numMap < *pMappingCount) {
   391         while (numMap < *pMappingCount) {
   392           pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
   392           pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
   393           /*! Get the mapped variable */
   393           /* Get the mapped variable */
   394           Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
   394           Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
   395           objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16),
   395           objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16),
   396                                 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF),
   396                                 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF),
   397                                 (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
   397                                 (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
   398           if (objDict != OD_SUCCESSFUL) {
   398           if (objDict != OD_SUCCESSFUL) {
   403           }
   403           }
   404           offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3);
   404           offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3);
   405           d->process_var.count = offset;
   405           d->process_var.count = offset;
   406           numMap++;
   406           numMap++;
   407 
   407 
   408         } /*! end while */
   408         } /* end while */
   409         PDOmGR( d, *pwCobId ); /*! Transmit the PDO */
   409         PDOmGR( d, *pwCobId ); /* Transmit the PDO */
   410         return 0;
   410         return 0;
   411 
   411 
   412       }/*! end switch status */
   412       }/* end switch status */
   413     }/*! end while */
   413     }/* end while */
   414   }/*! end if Requete */
   414   }/* end if Requete */
   415 
   415 
   416   return 0;
   416   return 0;
   417 }
   417 }
   418 
   418 
   419 /*!
   419 /*!
   427 ** @param DestBitIndex
   427 ** @param DestBitIndex
   428 ** @param DestBigEndian
   428 ** @param DestBigEndian
   429 **/
   429 **/
   430 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)
   430 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)
   431 {
   431 {
   432   /*! This loop copy as many bits that it can each time, crossing*/
   432   /* This loop copy as many bits that it can each time, crossing*/
   433   /*! successively bytes*/
   433   /* successively bytes*/
   434   // boundaries from LSB to MSB.
   434   // boundaries from LSB to MSB.
   435   while(NbBits > 0)
   435   while(NbBits > 0)
   436     {
   436     {
   437       /*! Bit missalignement between src and dest*/
   437       /* Bit missalignement between src and dest*/
   438       INTEGER8 Vect = DestBitIndex - SrcBitIndex;
   438       INTEGER8 Vect = DestBitIndex - SrcBitIndex;
   439 
   439 
   440       /*! We can now get src and align it to dest*/
   440       /* We can now get src and align it to dest*/
   441       UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
   441       UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
   442 
   442 
   443       /*! Compute the nb of bit we will be able to copy*/
   443       /* Compute the nb of bit we will be able to copy*/
   444       UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex :  8 - SrcBitIndex );
   444       UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex :  8 - SrcBitIndex );
   445       UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
   445       UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
   446 
   446 
   447       /*! Create a mask that will serve in:*/
   447       /* Create a mask that will serve in:*/
   448       UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
   448       UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
   449 
   449 
   450       /*! - Filtering src*/
   450       /* - Filtering src*/
   451       UNS8 Filtered = Aligned & ~Mask;
   451       UNS8 Filtered = Aligned & ~Mask;
   452 
   452 
   453       /*! - and erase bits where we write, preserve where we don't*/
   453       /* - and erase bits where we write, preserve where we don't*/
   454       *DestByteIndex &= Mask;
   454       *DestByteIndex &= Mask;
   455 
   455 
   456       /*! Then write.*/
   456       /* Then write.*/
   457       *DestByteIndex |= Filtered ;
   457       *DestByteIndex |= Filtered ;
   458 
   458 
   459       /*!Compute next time cursors for src*/
   459       /*Compute next time cursors for src*/
   460       if((SrcBitIndex += BitsToCopy)>7)/*! cross boundary ?*/
   460       if((SrcBitIndex += BitsToCopy)>7)/* cross boundary ?*/
   461         {
   461         {
   462           SrcBitIndex = 0;/*! First bit*/
   462           SrcBitIndex = 0;/* First bit*/
   463           SrcByteIndex += (SrcBigEndian ? -1 : 1);/*! Next byte*/
   463           SrcByteIndex += (SrcBigEndian ? -1 : 1);/* Next byte*/
   464         }
   464         }
   465 
   465 
   466 
   466 
   467       /*!Compute next time cursors for dest*/
   467       /*Compute next time cursors for dest*/
   468       if((DestBitIndex += BitsToCopy)>7)
   468       if((DestBitIndex += BitsToCopy)>7)
   469         {
   469         {
   470           DestBitIndex = 0;/*! First bit*/
   470           DestBitIndex = 0;/* First bit*/
   471           DestByteIndex += (DestBigEndian ? -1 : 1);/*! Next byte*/
   471           DestByteIndex += (DestBigEndian ? -1 : 1);/* Next byte*/
   472         }
   472         }
   473 
   473 
   474       /*!And decrement counter.*/
   474       /*And decrement counter.*/
   475       NbBits -= BitsToCopy;
   475       NbBits -= BitsToCopy;
   476     }
   476     }
   477 
   477 
   478 }
   478 }
   479 
   479 
   491 ** @param variable
   491 ** @param variable
   492 **
   492 **
   493 ** @return
   493 ** @return
   494 **/
   494 **/
   495 UNS8 sendPDOevent( CO_Data* d, void * variable )
   495 UNS8 sendPDOevent( CO_Data* d, void * variable )
   496 { /*! DO NOT USE MSG_ERR because the macro may send a PDO -> infinite
   496 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite
   497     loop if it fails.*/
   497     loop if it fails.*/
   498   UNS32           objDict = 0;
   498   UNS32           objDict = 0;
   499   UNS8            ind, sub_ind;
   499   UNS8            ind, sub_ind;
   500   UNS8            status;
   500   UNS8            status;
   501   UNS8            offset;
   501   UNS8            offset;
   502   UNS8 *     pMappingCount = NULL;
   502   UNS8 *     pMappingCount = NULL;
   503   UNS32 *    pMappingParameter = NULL;
   503   UNS32 *    pMappingParameter = NULL;
   504   void *     pMappedAppObject = NULL;
   504   void *     pMappedAppObject = NULL;
   505   UNS8 *     pTransmissionType = NULL; /*! pointer to the transmission
   505   UNS8 *     pTransmissionType = NULL; /* pointer to the transmission
   506                                          type */
   506                                          type */
   507   UNS32 *    pwCobId = NULL;
   507   UNS32 *    pwCobId = NULL;
   508   UNS8 *     pSize;
   508   UNS8 *     pSize;
   509   UNS8       size;
   509   UNS8       size;
   510   UNS8       dataType;
   510   UNS8       dataType;
   517   offset  = 0x00;
   517   offset  = 0x00;
   518   pSize   = &size;
   518   pSize   = &size;
   519   status  = state1;
   519   status  = state1;
   520 
   520 
   521 
   521 
   522   /*! look for the index and subindex where the variable is mapped */
   522   /* look for the index and subindex where the variable is mapped */
   523   /*! Then, send the pdo which contains the variable. */
   523   /* Then, send the pdo which contains the variable. */
   524 
   524 
   525   MSG_WAR (0x3960, "sendPDOevent", 0);
   525   MSG_WAR (0x3960, "sendPDOevent", 0);
   526   offsetObjdictPrm = d->firstIndex->PDO_TRS;
   526   offsetObjdictPrm = d->firstIndex->PDO_TRS;
   527 
   527 
   528   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
   528   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
   529   lastIndex = d->lastIndex->PDO_TRS_MAP;
   529   lastIndex = d->lastIndex->PDO_TRS_MAP;
   530 
   530 
   531   if (offsetObjdictPrm && offsetObjdict)
   531   if (offsetObjdictPrm && offsetObjdict)
   532     /*! Loop on PDO Transmit */
   532     /* Loop on PDO Transmit */
   533     while(offsetObjdict <= lastIndex){
   533     while(offsetObjdict <= lastIndex){
   534       /*! Check the transmission mode */
   534       /* Check the transmission mode */
   535       pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject;
   535       pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject;
   536       if (*pTransmissionType != TRANS_EVENT) {
   536       if (*pTransmissionType != TRANS_EVENT) {
   537         ind++;
   537         ind++;
   538         offsetObjdict++;
   538         offsetObjdict++;
   539         offsetObjdictPrm++;
   539         offsetObjdictPrm++;
   540         continue;
   540         continue;
   541       }
   541       }
   542       pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject;
   542       pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject;
   543       numMap = 1; /*! mapped variable */
   543       numMap = 1; /* mapped variable */
   544       while (numMap <= *pMappingCount) {
   544       while (numMap <= *pMappingCount) {
   545         pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject;
   545         pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject;
   546         /*! Get the variable */
   546         /* Get the variable */
   547         objDict = getODentry( d,
   547         objDict = getODentry( d,
   548                               (UNS16)((*pMappingParameter) >> 16),
   548                               (UNS16)((*pMappingParameter) >> 16),
   549                               (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF),
   549                               (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF),
   550                               (void * *)&pMappedAppObject, pSize, &dataType, 0 );
   550                               (void * *)&pMappedAppObject, pSize, &dataType, 0 );
   551         if( objDict != OD_SUCCESSFUL ) {
   551         if( objDict != OD_SUCCESSFUL ) {
   560           MSG_WAR(0x3963, "Variable to send found at index : ",
   560           MSG_WAR(0x3963, "Variable to send found at index : ",
   561                   (*pMappingParameter) >> 16);
   561                   (*pMappingParameter) >> 16);
   562           MSG_WAR(0x3964, "                       subIndex : ",
   562           MSG_WAR(0x3964, "                       subIndex : ",
   563                   ((*pMappingParameter) >> 8 ) & 0x000000FF);
   563                   ((*pMappingParameter) >> 8 ) & 0x000000FF);
   564           buildPDO(d, 0x1800 + ind);
   564           buildPDO(d, 0x1800 + ind);
   565           /*! Get the cobId */
   565           /* Get the cobId */
   566           pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject;
   566           pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject;
   567           PDOmGR( d, *pwCobId ); /*! Send the PDO */
   567           PDOmGR( d, *pwCobId ); /* Send the PDO */
   568           return 0;
   568           return 0;
   569         }
   569         }
   570         numMap++;
   570         numMap++;
   571       } /*! End loop on mapped variable */
   571       } /* End loop on mapped variable */
   572       ind++;
   572       ind++;
   573       offsetObjdict++;
   573       offsetObjdict++;
   574       offsetObjdictPrm++;
   574       offsetObjdictPrm++;
   575     } /*! End loop while on PDO */
   575     } /* End loop while on PDO */
   576 
   576 
   577   MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0);
   577   MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0);
   578   return 0xFF;
   578   return 0xFF;
   579 
   579 
   580 }
   580 }