src/pdo.c
changeset 425 4bc4706528ae
parent 380 fe47568b8281
child 426 8011552b1319
equal deleted inserted replaced
424:5bbb2817a820 425:4bc4706528ae
    44 ** @param TPDO_map TPDO mapping parameters OD entry
    44 ** @param TPDO_map TPDO mapping parameters OD entry
    45 **
    45 **
    46 ** @return
    46 ** @return
    47 **/
    47 **/
    48 
    48 
    49 UNS8 buildPDO(CO_Data* d, UNS8 numPdo, Message *pdo)
    49 UNS8
    50 {
    50 buildPDO (CO_Data * d, UNS8 numPdo, Message * pdo)
    51 	const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo; 
    51 {
    52 	const indextable* TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;
    52   const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo;
    53 	
    53   const indextable *TPDO_map =
    54 	UNS8 prp_j = 0x00;
    54     d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;
    55 	UNS8 offset = 0x00;
    55 
    56 	const UNS8* pMappingCount = (UNS8*) TPDO_map->pSubindex[0].pObject;
    56   UNS8 prp_j = 0x00;
    57 
    57   UNS8 offset = 0x00;
    58 	pdo->cob_id = *(UNS16*)TPDO_com->pSubindex[1].pObject & UNS16_LE(0x7FF);
    58   const UNS8 *pMappingCount = (UNS8 *) TPDO_map->pSubindex[0].pObject;
    59 	pdo->rtr = NOT_A_REQUEST;
    59 
    60 
    60   pdo->cob_id = *(UNS16 *) TPDO_com->pSubindex[1].pObject & UNS16_LE (0x7FF);
    61 	MSG_WAR(0x3009, "  PDO CobId is : ", *(UNS32*)TPDO_com->pSubindex[1].pObject);
    61   pdo->rtr = NOT_A_REQUEST;
    62 	MSG_WAR(0x300D, "  Number of objects mapped : ",*pMappingCount );
    62 
    63 	
    63   MSG_WAR (0x3009, "  PDO CobId is : ",
    64 	do{
    64            *(UNS32 *) TPDO_com->pSubindex[1].pObject);
    65 		UNS8 dataType; /* Unused */
    65   MSG_WAR (0x300D, "  Number of objects mapped : ", *pMappingCount);
    66 		UNS8 tmp[]= {0,0,0,0,0,0,0,0}; /* temporary space to hold bits */
    66 
    67 
    67   do
    68 		/* pointer fo the var which holds the mapping parameter of an mapping entry  */
    68     {
    69 		UNS32* pMappingParameter = (UNS32*) TPDO_map->pSubindex[prp_j + 1].pObject;
    69       UNS8 dataType;            /* Unused */
    70 		UNS16 index = (UNS16)((*pMappingParameter) >> 16);
    70       UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 };  /* temporary space to hold bits */
    71 		UNS8 Size = (UNS8)(*pMappingParameter & (UNS32)0x000000FF); /* Size in bits */
    71 
    72 		
    72       /* pointer fo the var which holds the mapping parameter of an mapping entry  */
    73 		/* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */
    73       UNS32 *pMappingParameter =
    74 		if(Size && ((offset + Size) <= 64)) {
    74         (UNS32 *) TPDO_map->pSubindex[prp_j + 1].pObject;
    75 			UNS8 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
    75       UNS16 index = (UNS16) ((*pMappingParameter) >> 16);
    76 			UNS8 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
    76       UNS8 Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF);     /* Size in bits */
    77 			
    77 
    78 			MSG_WAR(0x300F, "  got mapping parameter : ", *pMappingParameter);
    78       /* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */
    79 			MSG_WAR(0x3050, "    at index : ", TPDO_map->index);
    79       if (Size && ((offset + Size) <= 64))
    80 			MSG_WAR(0x3051, "    sub-index : ", prp_j + 1);
    80         {
    81 			
    81           UNS8 ByteSize = 1 + ((Size - 1) >> 3);        /*1->8 => 1 ; 9->16 => 2, ... */
    82 			if( getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ) != OD_SUCCESSFUL ){
    82           UNS8 subIndex =
    83 				MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
    83             (UNS8) (((*pMappingParameter) >> (UNS8) 8) & (UNS32) 0x000000FF);
    84 				return 0xFF;
    84 
    85 			}
    85           MSG_WAR (0x300F, "  got mapping parameter : ", *pMappingParameter);
    86 			/* copy bit per bit in little endian*/
    86           MSG_WAR (0x3050, "    at index : ", TPDO_map->index);
    87 			CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&pdo->data[offset>>3], offset%8, 0);
    87           MSG_WAR (0x3051, "    sub-index : ", prp_j + 1);
    88 
    88 
    89 			offset += Size ;
    89           if (getODentry (d, index, subIndex, tmp, &ByteSize, &dataType, 0) !=
    90 		}
    90               OD_SUCCESSFUL)
    91 		prp_j++;
    91             {
    92 	}while( prp_j < *pMappingCount );
    92               MSG_ERR (0x1013,
    93 
    93                        " Couldn't find mapped variable at index-subindex-size : ",
    94 	pdo->len = 1 + ((offset - 1) >> 3);
    94                        (UNS16) (*pMappingParameter));
    95 
    95               return 0xFF;
    96 	MSG_WAR(0x3015, "  End scan mapped variable", 0);
    96             }
    97 
    97           /* copy bit per bit in little endian */
    98 	return 0;
    98           CopyBits (Size, ((UNS8 *) tmp), 0, 0,
       
    99                     (UNS8 *) & pdo->data[offset >> 3], offset % 8, 0);
       
   100 
       
   101           offset += Size;
       
   102         }
       
   103       prp_j++;
       
   104     }
       
   105   while (prp_j < *pMappingCount);
       
   106 
       
   107   pdo->len = 1 + ((offset - 1) >> 3);
       
   108 
       
   109   MSG_WAR (0x3015, "  End scan mapped variable", 0);
       
   110 
       
   111   return 0;
    99 }
   112 }
   100 
   113 
   101 /*!
   114 /*!
   102 **
   115 **
   103 **
   116 **
   104 ** @param d
   117 ** @param d
   105 ** @param cobId
   118 ** @param cobId
   106 **
   119 **
   107 ** @return
   120 ** @return
   108 **/
   121 **/
   109 UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex )
   122 UNS8
   110 {
   123 sendPDOrequest (CO_Data * d, UNS16 RPDOIndex)
   111   UNS16 * pwCobId;
   124 {
       
   125   UNS16 *pwCobId;
   112   UNS16 offset = d->firstIndex->PDO_RCV;
   126   UNS16 offset = d->firstIndex->PDO_RCV;
   113   UNS16 lastIndex = d->lastIndex->PDO_RCV;
   127   UNS16 lastIndex = d->lastIndex->PDO_RCV;
   114 
   128 
   115   /* Sending the request only if the cobid have been found on the PDO
   129   /* Sending the request only if the cobid have been found on the PDO
   116      receive */
   130      receive */
   117   /* part dictionary */
   131   /* part dictionary */
   118   
   132 
   119   MSG_WAR(0x3930, "sendPDOrequest RPDO Index : ",RPDOIndex);
   133   MSG_WAR (0x3930, "sendPDOrequest RPDO Index : ", RPDOIndex);
   120   
   134 
   121   if (offset && RPDOIndex >= 0x1400){
   135   if (offset && RPDOIndex >= 0x1400)
   122     offset += RPDOIndex - 0x1400;  
   136     {
   123     if (offset <= lastIndex) {
   137       offset += RPDOIndex - 0x1400;
   124       /* get the CobId*/
   138       if (offset <= lastIndex)
   125       pwCobId = (UNS16*) d->objdict[offset].pSubindex[1].pObject;
   139         {
   126 
   140           /* get the CobId */
   127       MSG_WAR(0x3930, "sendPDOrequest cobId is : ",*pwCobId);
   141           pwCobId = (UNS16 *) d->objdict[offset].pSubindex[1].pObject;
   128       {
   142 
   129           Message pdo;
   143           MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId);
   130           pdo.cob_id = *pwCobId;
   144           {
   131           pdo.rtr = REQUEST;
   145             Message pdo;
   132           pdo.len = 0;      
   146             pdo.cob_id = *pwCobId;
   133           return canSend(d->canHandle,&pdo);
   147             pdo.rtr = REQUEST;
   134       }
   148             pdo.len = 0;
       
   149             return canSend (d->canHandle, &pdo);
       
   150           }
       
   151         }
   135     }
   152     }
   136   }
   153   MSG_ERR (0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex);
   137   MSG_ERR(0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex);
       
   138   return 0xFF;
   154   return 0xFF;
   139 }
   155 }
   140 
   156 
   141 
   157 
   142 /*!
   158 /*!
   145 ** @param d
   161 ** @param d
   146 ** @param m
   162 ** @param m
   147 **
   163 **
   148 ** @return
   164 ** @return
   149 **/
   165 **/
   150 UNS8 proceedPDO(CO_Data* d, Message *m)
   166 UNS8
   151 {
   167 proceedPDO (CO_Data * d, Message * m)
   152   UNS8   numPdo;
   168 {
   153   UNS8   numMap;  /* Number of the mapped varable */
   169   UNS8 numPdo;
   154   UNS8 *     pMappingCount = NULL;    /* count of mapped objects... */
   170   UNS8 numMap;                  /* Number of the mapped varable */
       
   171   UNS8 *pMappingCount = NULL;   /* count of mapped objects... */
   155   /* pointer to the var which is mapped to a pdo... */
   172   /* pointer to the var which is mapped to a pdo... */
   156   /*  void *     pMappedAppObject = NULL;   */
   173   /*  void *     pMappedAppObject = NULL;   */
   157   /* pointer fo the var which holds the mapping parameter of an
   174   /* pointer fo the var which holds the mapping parameter of an
   158      mapping entry */
   175      mapping entry */
   159   UNS32 *    pMappingParameter = NULL;
   176   UNS32 *pMappingParameter = NULL;
   160   UNS8  *    pTransmissionType = NULL; /* pointer to the transmission
   177   UNS8 *pTransmissionType = NULL;       /* pointer to the transmission
   161                                          type */
   178                                            type */
   162   UNS16 *    pwCobId = NULL;
   179   UNS16 *pwCobId = NULL;
   163   UNS8       Size;
   180   UNS8 Size;
   164   UNS8       offset;
   181   UNS8 offset;
   165   UNS8       status;
   182   UNS8 status;
   166   UNS32      objDict;
   183   UNS32 objDict;
   167   UNS16      offsetObjdict;
   184   UNS16 offsetObjdict;
   168   UNS16      lastIndex;
   185   UNS16 lastIndex;
   169   
   186 
   170   status = state2;
   187   status = state2;
   171 
   188 
   172   MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id & UNS16_LE(0x7ff)));
   189   MSG_WAR (0x3935, "proceedPDO, cobID : ", ((*m).cob_id & UNS16_LE (0x7ff)));
   173   offset = 0x00;
   190   offset = 0x00;
   174   numPdo = 0;
   191   numPdo = 0;
   175   numMap = 0;
   192   numMap = 0;
   176   if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a
   193   if ((*m).rtr == NOT_A_REQUEST)
   177                                      request. */
   194     {                           /* The PDO received is not a
   178 
   195                                    request. */
   179     offsetObjdict = d->firstIndex->PDO_RCV;
   196 
   180     lastIndex = d->lastIndex->PDO_RCV;
   197       offsetObjdict = d->firstIndex->PDO_RCV;
   181 
   198       lastIndex = d->lastIndex->PDO_RCV;
   182     /* study of all the PDO stored in the dictionary */
   199 
   183     if(offsetObjdict)
   200       /* study of all the PDO stored in the dictionary */
   184       while (offsetObjdict <= lastIndex) {
   201       if (offsetObjdict)
   185 
   202         while (offsetObjdict <= lastIndex)
   186         switch( status ) {
   203           {
   187 
   204 
   188         case state2:
   205             switch (status)
   189           /* get CobId of the dictionary correspondant to the received
   206               {
   190              PDO */
   207 
   191           pwCobId = (UNS16*) d->objdict[offsetObjdict].pSubindex[1].pObject;
   208               case state2:
   192           /* check the CobId coherance */
   209                 /* get CobId of the dictionary correspondant to the received
   193           /*pwCobId is the cobId read in the dictionary at the state 3
   210                    PDO */
   194             */
   211                 pwCobId =
   195           if ( *pwCobId == (*m).cob_id ){
   212                   (UNS16 *) d->objdict[offsetObjdict].pSubindex[1].pObject;
   196             /* The cobId is recognized */
   213                 /* check the CobId coherance */
   197             status = state4;
   214                 /*pwCobId is the cobId read in the dictionary at the state 3
   198             MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
   215                  */
   199             break;
   216                 if (*pwCobId == (*m).cob_id)
   200           }
   217                   {
   201           else {
   218                     /* The cobId is recognized */
   202             /* cobId received does not match with those write in the
   219                     status = state4;
   203               dictionnary */
   220                     MSG_WAR (0x3936, "cobId found at index ",
   204             numPdo++;
   221                              0x1400 + numPdo);
   205             offsetObjdict++;
   222                     break;
   206             status = state2;
   223                   }
   207             break;
   224                 else
   208           }
   225                   {
   209 
   226                     /* cobId received does not match with those write in the
   210             case state4:/* Get Mapped Objects Number */
   227                        dictionnary */
   211                /* The cobId of the message received has been found in the
   228                     numPdo++;
   212                  dictionnary. */
   229                     offsetObjdict++;
   213                offsetObjdict = d->firstIndex->PDO_RCV_MAP;
   230                     status = state2;
   214              lastIndex = d->lastIndex->PDO_RCV_MAP;
   231                     break;
   215              pMappingCount = (UNS8*) (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
   232                   }
   216              numMap = 0;
   233 
   217              while (numMap < *pMappingCount) {
   234               case state4:     /* Get Mapped Objects Number */
   218                UNS8 tmp[]= {0,0,0,0,0,0,0,0};
   235                 /* The cobId of the message received has been found in the
   219                UNS8 ByteSize;
   236                    dictionnary. */
   220                pMappingParameter = (UNS32*) (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
   237                 offsetObjdict = d->firstIndex->PDO_RCV_MAP;
   221                if (pMappingParameter == NULL) {
   238                 lastIndex = d->lastIndex->PDO_RCV_MAP;
   222                  MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
   239                 pMappingCount =
   223                  return 0xFF;
   240                   (UNS8 *) (d->objdict + offsetObjdict +
   224                }
   241                             numPdo)->pSubindex[0].pObject;
   225                /* Get the addresse of the mapped variable. */
   242                 numMap = 0;
   226                /* detail of *pMappingParameter : */
   243                 while (numMap < *pMappingCount)
   227                /* The 16 hight bits contains the index, the medium 8 bits
   244                   {
   228                  contains the subindex, */
   245                     UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
   229                /* and the lower 8 bits contains the size of the mapped
   246                     UNS8 ByteSize;
   230                  variable. */
   247                     pMappingParameter =
   231 
   248                       (UNS32 *) (d->objdict + offsetObjdict +
   232                Size = (UNS8)(*pMappingParameter & (UNS32)0x000000FF);
   249                                  numPdo)->pSubindex[numMap + 1].pObject;
   233 					
   250                     if (pMappingParameter == NULL)
   234 					/* set variable only if Size != 0 and Size is lower than remaining bits in the PDO */
   251                       {
   235                if(Size && ((offset + Size) <= (m->len << 3))) {
   252                         MSG_ERR (0x1937, "Couldn't get mapping parameter : ",
   236                  /* copy bit per bit in little endian */
   253                                  numMap + 1);
   237                  CopyBits(Size, (UNS8*)&m->data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
   254                         return 0xFF;
   238 
   255                       }
   239                  ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 =>
   256                     /* Get the addresse of the mapped variable. */
   240                                                      2, ... */
   257                     /* detail of *pMappingParameter : */
   241 
   258                     /* The 16 hight bits contains the index, the medium 8 bits
   242                  objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16),
   259                        contains the subindex, */
   243                                       (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),
   260                     /* and the lower 8 bits contains the size of the mapped
   244                                    tmp, &ByteSize, 0 );
   261                        variable. */
   245 
   262 
   246                  if(objDict != OD_SUCCESSFUL) {
   263                     Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF);
   247                    MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);
   264 
   248                    MSG_WAR(0x2939, "         Mapped at index : ", (*pMappingParameter) >> 16);
   265                     /* set variable only if Size != 0 and Size is lower than remaining bits in the PDO */
   249                    MSG_WAR(0x2940, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
   266                     if (Size && ((offset + Size) <= (m->len << 3)))
   250                    return 0xFF;
   267                       {
   251                  }
   268                         /* copy bit per bit in little endian */
   252 
   269                         CopyBits (Size, (UNS8 *) & m->data[offset >> 3],
   253                  MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id);
   270                                   offset % 8, 0, ((UNS8 *) tmp), 0, 0);
   254                  MSG_WAR(0x3943, "         Mapped at index : ", (*pMappingParameter) >> 16);
   271 
   255                  MSG_WAR(0x3944, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
   272                         ByteSize = 1 + ((Size - 1) >> 3);       /*1->8 => 1 ; 9->16 =>
   256                  /* MSG_WAR(0x3945, "                data : ",*((UNS32*)pMappedAppObject)); */
   273                                                                    2, ... */
   257                  offset += Size;
   274 
   258 					}
   275                         objDict =
   259                numMap++;
   276                           setODentry (d, (UNS16) ((*pMappingParameter) >> 16),
   260              } /* end loop while on mapped variables */
   277                                       (UNS8) (((*pMappingParameter) >> 8) &
   261 
   278                                               0xFF), tmp, &ByteSize, 0);
   262              offset=0x00;
   279 
   263              numMap = 0;
   280                         if (objDict != OD_SUCCESSFUL)
   264              return 0;
   281                           {
   265 
   282                             MSG_ERR (0x1938,
   266         }/* end switch status*/
   283                                      "error accessing to the mapped var : ",
   267       }/* end while*/
   284                                      numMap + 1);
   268   }/* end if Donnees */
   285                             MSG_WAR (0x2939, "         Mapped at index : ",
   269   else if ((*m).rtr == REQUEST ){
   286                                      (*pMappingParameter) >> 16);
   270     MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id);
   287                             MSG_WAR (0x2940, "                subindex : ",
   271     status = state1;
   288                                      ((*pMappingParameter) >> 8) & 0xFF);
   272     offsetObjdict = d->firstIndex->PDO_TRS;
   289                             return 0xFF;
   273     lastIndex = d->lastIndex->PDO_TRS;
   290                           }
   274     if(offsetObjdict) while( offsetObjdict  <= lastIndex ){
   291 
   275       /* study of all PDO stored in the objects dictionary */
   292                         MSG_WAR (0x3942,
   276 
   293                                  "Variable updated with value received by PDO cobid : ",
   277       switch( status ){
   294                                  m->cob_id);
   278 
   295                         MSG_WAR (0x3943, "         Mapped at index : ",
   279       case state1:/* check the CobId */
   296                                  (*pMappingParameter) >> 16);
   280         /* get CobId of the dictionary which match to the received PDO
   297                         MSG_WAR (0x3944, "                subindex : ",
   281          */
   298                                  ((*pMappingParameter) >> 8) & 0xFF);
   282         pwCobId = (UNS16*) (d->objdict + offsetObjdict)->pSubindex[1].pObject;
   299                         /* MSG_WAR(0x3945, "                data : ",*((UNS32*)pMappedAppObject)); */
   283         if ( *pwCobId == (*m).cob_id ) {
   300                         offset += Size;
   284           status = state4;
   301                       }
   285           break;
   302                     numMap++;
   286         }
   303                   }             /* end loop while on mapped variables */
   287         else {
   304 
   288           numPdo++;
   305                 offset = 0x00;
   289           offsetObjdict++;
   306                 numMap = 0;
   290         }
   307                 return 0;
   291         status = state1;
   308 
   292         break;
   309               }                 /* end switch status */
   293 
   310           }                     /* end while */
   294 
   311     }                           /* end if Donnees */
   295       case state4:/* check transmission type */
   312   else if ((*m).rtr == REQUEST)
   296         pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject;
   313     {
   297         /* If PDO is to be sampled and send on RTR, do it*/
   314       MSG_WAR (0x3946, "Receive a PDO request cobId : ", m->cob_id);
   298         if ( (*pTransmissionType == TRANS_RTR)) {
   315       status = state1;
   299           status = state5;
   316       offsetObjdict = d->firstIndex->PDO_TRS;
   300           break;
   317       lastIndex = d->lastIndex->PDO_TRS;
   301         /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */
   318       if (offsetObjdict)
   302         }else if ((*pTransmissionType == TRANS_RTR_SYNC )) {
   319         while (offsetObjdict <= lastIndex)
   303     	  if(d->PDO_status[numPdo].transmit_type_parameter & PDO_RTR_SYNC_READY){
   320           {
   304             /*Data ready, just send*/
   321             /* study of all PDO stored in the objects dictionary */
   305     	    canSend(d->canHandle,&d->PDO_status[numPdo].last_message);
   322 
   306             return 0;
   323             switch (status)
   307     	  }else{
   324               {
   308             /* if SYNC did never occur, force emission with current data */
   325 
   309             /* DS301 do not tell what to do in such a case...*/
   326               case state1:     /* check the CobId */
   310             MSG_ERR(0x1947, "Not ready RTR_SYNC TPDO send current data : ", m->cob_id);
   327                 /* get CobId of the dictionary which match to the received PDO
   311             status = state5;
   328                  */
   312     	  }
   329                 pwCobId =
   313           break;
   330                   (UNS16 *) (d->objdict +
   314         }else if(
   331                              offsetObjdict)->pSubindex[1].pObject;
   315              (*pTransmissionType == TRANS_EVENT_PROFILE) ||
   332                 if (*pwCobId == (*m).cob_id)
   316              (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) {
   333                   {
   317 	  /* Zap all timers and inhibit flag */
   334                     status = state4;
   318 	  d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer);
   335                     break;
   319 	  d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer);
   336                   }
   320   	  d->PDO_status[numPdo].transmit_type_parameter &= ~PDO_INHIBITED;
   337                 else
   321   	  /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
   338                   {
   322           PDOEventTimerAlarm(d, numPdo);
   339                     numPdo++;
   323           return 0;
   340                     offsetObjdict++;
   324         }else {
   341                   }
   325           /* The requested PDO is not to send on request. So, does
   342                 status = state1;
   326             nothing. */
   343                 break;
   327           MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id);
   344 
   328           return 0xFF;
   345 
   329         }
   346               case state4:     /* check transmission type */
   330 
   347                 pTransmissionType =
   331       case state5:/* build and send requested PDO */
   348                   (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
   332       {
   349                 /* If PDO is to be sampled and send on RTR, do it */
   333       	Message pdo;
   350                 if ((*pTransmissionType == TRANS_RTR))
   334     	if( buildPDO(d, numPdo, &pdo))
   351                   {
   335     	{
   352                     status = state5;
   336           MSG_ERR(0x1948, " Couldn't build TPDO n°", numPdo);
   353                     break;
   337           return 0xFF;
   354                     /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */
   338     	}
   355                   }
   339     	canSend(d->canHandle,&pdo);
   356                 else if ((*pTransmissionType == TRANS_RTR_SYNC))
   340         return 0;
   357                   {
   341       }
   358                     if (d->PDO_status[numPdo].
   342       }/* end switch status */
   359                         transmit_type_parameter & PDO_RTR_SYNC_READY)
   343     }/* end while */
   360                       {
   344   }/* end if Requete */
   361                         /*Data ready, just send */
       
   362                         canSend (d->canHandle,
       
   363                                  &d->PDO_status[numPdo].last_message);
       
   364                         return 0;
       
   365                       }
       
   366                     else
       
   367                       {
       
   368                         /* if SYNC did never occur, force emission with current data */
       
   369                         /* DS301 do not tell what to do in such a case... */
       
   370                         MSG_ERR (0x1947,
       
   371                                  "Not ready RTR_SYNC TPDO send current data : ",
       
   372                                  m->cob_id);
       
   373                         status = state5;
       
   374                       }
       
   375                     break;
       
   376                   }
       
   377                 else if ((*pTransmissionType == TRANS_EVENT_PROFILE) ||
       
   378                          (*pTransmissionType == TRANS_EVENT_SPECIFIC))
       
   379                   {
       
   380                     /* Zap all timers and inhibit flag */
       
   381                     d->PDO_status[numPdo].event_timer =
       
   382                       DelAlarm (d->PDO_status[numPdo].event_timer);
       
   383                     d->PDO_status[numPdo].inhibit_timer =
       
   384                       DelAlarm (d->PDO_status[numPdo].inhibit_timer);
       
   385                     d->PDO_status[numPdo].transmit_type_parameter &=
       
   386                       ~PDO_INHIBITED;
       
   387                     /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
       
   388                     PDOEventTimerAlarm (d, numPdo);
       
   389                     return 0;
       
   390                   }
       
   391                 else
       
   392                   {
       
   393                     /* The requested PDO is not to send on request. So, does
       
   394                        nothing. */
       
   395                     MSG_WAR (0x2947, "PDO is not to send on request : ",
       
   396                              m->cob_id);
       
   397                     return 0xFF;
       
   398                   }
       
   399 
       
   400               case state5:     /* build and send requested PDO */
       
   401                 {
       
   402                   Message pdo;
       
   403                   if (buildPDO (d, numPdo, &pdo))
       
   404                     {
       
   405                       MSG_ERR (0x1948, " Couldn't build TPDO n°", numPdo);
       
   406                       return 0xFF;
       
   407                     }
       
   408                   canSend (d->canHandle, &pdo);
       
   409                   return 0;
       
   410                 }
       
   411               }                 /* end switch status */
       
   412           }                     /* end while */
       
   413     }                           /* end if Requete */
   345 
   414 
   346   return 0;
   415   return 0;
   347 }
   416 }
   348 
   417 
   349 /*!
   418 /*!
   355 ** @param SrcBigEndian
   424 ** @param SrcBigEndian
   356 ** @param DestByteIndex
   425 ** @param DestByteIndex
   357 ** @param DestBitIndex
   426 ** @param DestBitIndex
   358 ** @param DestBigEndian
   427 ** @param DestBigEndian
   359 **/
   428 **/
   360 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)
   429 void
   361 {
   430 CopyBits (UNS8 NbBits, UNS8 * SrcByteIndex, UNS8 SrcBitIndex,
   362   /* This loop copy as many bits that it can each time, crossing*/
   431           UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex,
   363   /* successively bytes*/
   432           UNS8 DestBigEndian)
       
   433 {
       
   434   /* This loop copy as many bits that it can each time, crossing */
       
   435   /* successively bytes */
   364   // boundaries from LSB to MSB.
   436   // boundaries from LSB to MSB.
   365   while(NbBits > 0)
   437   while (NbBits > 0)
   366     {
   438     {
   367       /* Bit missalignement between src and dest*/
   439       /* Bit missalignement between src and dest */
   368       INTEGER8 Vect = DestBitIndex - SrcBitIndex;
   440       INTEGER8 Vect = DestBitIndex - SrcBitIndex;
   369 
   441 
   370       /* We can now get src and align it to dest*/
   442       /* We can now get src and align it to dest */
   371       UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
   443       UNS8 Aligned =
   372 
   444         Vect > 0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
   373       /* Compute the nb of bit we will be able to copy*/
   445 
   374       UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex :  8 - SrcBitIndex );
   446       /* Compute the nb of bit we will be able to copy */
       
   447       UNS8 BoudaryLimit = (Vect > 0 ? 8 - DestBitIndex : 8 - SrcBitIndex);
   375       UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
   448       UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
   376 
   449 
   377       /* Create a mask that will serve in:*/
   450       /* Create a mask that will serve in: */
   378       UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
   451       UNS8 Mask =
   379 
   452         ((0xff << (DestBitIndex + BitsToCopy)) |
   380       /* - Filtering src*/
   453          (0xff >> (8 - DestBitIndex)));
       
   454 
       
   455       /* - Filtering src */
   381       UNS8 Filtered = Aligned & ~Mask;
   456       UNS8 Filtered = Aligned & ~Mask;
   382 
   457 
   383       /* - and erase bits where we write, preserve where we don't*/
   458       /* - and erase bits where we write, preserve where we don't */
   384       *DestByteIndex &= Mask;
   459       *DestByteIndex &= Mask;
   385 
   460 
   386       /* Then write.*/
   461       /* Then write. */
   387       *DestByteIndex |= Filtered ;
   462       *DestByteIndex |= Filtered;
   388 
   463 
   389       /*Compute next time cursors for src*/
   464       /*Compute next time cursors for src */
   390       if((SrcBitIndex += BitsToCopy)>7)/* cross boundary ?*/
   465       if ((SrcBitIndex += BitsToCopy) > 7)      /* cross boundary ? */
   391         {
   466         {
   392           SrcBitIndex = 0;/* First bit*/
   467           SrcBitIndex = 0;      /* First bit */
   393           SrcByteIndex += (SrcBigEndian ? -1 : 1);/* Next byte*/
   468           SrcByteIndex += (SrcBigEndian ? -1 : 1);      /* Next byte */
   394         }
   469         }
   395 
   470 
   396 
   471 
   397       /*Compute next time cursors for dest*/
   472       /*Compute next time cursors for dest */
   398       if((DestBitIndex += BitsToCopy)>7)
   473       if ((DestBitIndex += BitsToCopy) > 7)
   399         {
   474         {
   400           DestBitIndex = 0;/* First bit*/
   475           DestBitIndex = 0;     /* First bit */
   401           DestByteIndex += (DestBigEndian ? -1 : 1);/* Next byte*/
   476           DestByteIndex += (DestBigEndian ? -1 : 1);    /* Next byte */
   402         }
   477         }
   403 
   478 
   404       /*And decrement counter.*/
   479       /*And decrement counter. */
   405       NbBits -= BitsToCopy;
   480       NbBits -= BitsToCopy;
   406     }
   481     }
   407 
   482 
   408 }
   483 }
       
   484 
   409 /*!
   485 /*!
   410 **
   486 **
   411 **
   487 **
   412 ** @param d
   488 ** @param d
   413 **
   489 **
   414 ** @return
   490 ** @return
   415 **/
   491 **/
   416 
   492 
   417 UNS8 sendPDOevent( CO_Data* d)
   493 UNS8
       
   494 sendPDOevent (CO_Data * d)
   418 {
   495 {
   419   /* Calls _sendPDOevent specifying it is not a sync event */
   496   /* Calls _sendPDOevent specifying it is not a sync event */
   420   return _sendPDOevent(d, 0);
   497   return _sendPDOevent (d, 0);
   421 }
   498 }
   422 	
   499 
   423 
   500 
   424 void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum)
   501 void
   425 {
   502 PDOEventTimerAlarm (CO_Data * d, UNS32 pdoNum)
   426 	/* This is needed to avoid deletion of re-attribuated timer */
   503 {
   427 	d->PDO_status[pdoNum].event_timer = TIMER_NONE;
   504   /* This is needed to avoid deletion of re-attribuated timer */
   428 	/* force emission of PDO by artificially changing last emitted*/
   505   d->PDO_status[pdoNum].event_timer = TIMER_NONE;
   429 	d->PDO_status[pdoNum].last_message.cob_id = 0;
   506   /* force emission of PDO by artificially changing last emitted */
   430 	_sendPDOevent( d, 0 ); /* not a Sync Event*/	
   507   d->PDO_status[pdoNum].last_message.cob_id = 0;
   431 }
   508   _sendPDOevent (d, 0);         /* not a Sync Event */
   432 
   509 }
   433 void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum)
   510 
   434 {
   511 void
   435 	/* This is needed to avoid deletion of re-attribuated timer */
   512 PDOInhibitTimerAlarm (CO_Data * d, UNS32 pdoNum)
   436 	d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;
   513 {
   437 	/* Remove inhibit flag */
   514   /* This is needed to avoid deletion of re-attribuated timer */
   438 	d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;
   515   d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;
   439 	_sendPDOevent( d, 0 ); /* not a Sync Event*/
   516   /* Remove inhibit flag */
       
   517   d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;
       
   518   _sendPDOevent (d, 0);         /* not a Sync Event */
   440 }
   519 }
   441 
   520 
   442 /*!
   521 /*!
   443 **
   522 **
   444 **
   523 **
   446 ** @param isSyncEvent
   525 ** @param isSyncEvent
   447 **
   526 **
   448 ** @return
   527 ** @return
   449 **/
   528 **/
   450 
   529 
   451 UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent )
   530 UNS8
   452 { 
   531 _sendPDOevent (CO_Data * d, UNS8 isSyncEvent)
   453   UNS8 	pdoNum = 0x00;       /* number of the actual processed pdo-nr. */
   532 {
   454   UNS8* pTransmissionType = NULL;  
   533   UNS8 pdoNum = 0x00;           /* number of the actual processed pdo-nr. */
       
   534   UNS8 *pTransmissionType = NULL;
   455   UNS8 status = state3;
   535   UNS8 status = state3;
   456   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
   536   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
   457   UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
   537   UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
   458   UNS16 lastIndex = d->lastIndex->PDO_TRS;  
   538   UNS16 lastIndex = d->lastIndex->PDO_TRS;
   459 
   539 
   460   /* study all PDO stored in the objects dictionary */	
   540   /* study all PDO stored in the objects dictionary */
   461   if(offsetObjdict){
   541   if (offsetObjdict)
   462    Message pdo = Message_Initializer;
   542     {
   463    while( offsetObjdict <= lastIndex) {  
   543       Message pdo = Message_Initializer;
   464     switch( status ) {
   544       while (offsetObjdict <= lastIndex)
   465     case state3:
       
   466       if (/*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5)*/
       
   467           /* check if TPDO is not valid */ 
       
   468           *(UNS32*)d->objdict[offsetObjdict].pSubindex[1].pObject & 0x80000000) {
       
   469 	  MSG_WAR(0x3960, "Not a valid PDO ", 0x1800 + pdoNum);
       
   470 	  /*Go next TPDO*/
       
   471 	  status = state11;
       
   472 	  break;
       
   473 	}
       
   474       /* get the PDO transmission type */
       
   475       pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject;    
       
   476       MSG_WAR(0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);
       
   477 
       
   478       /* check if transmission type is SYNCRONOUS */
       
   479       /* The message is transmited every n SYNC with n=TransmissionType */      
       
   480       if( isSyncEvent && 
       
   481       	  (*pTransmissionType >= TRANS_SYNC_MIN) &&
       
   482       	  (*pTransmissionType <= TRANS_SYNC_MAX) &&
       
   483           (++d->PDO_status[pdoNum].transmit_type_parameter == *pTransmissionType) ) {
       
   484         /*Reset count of SYNC*/	
       
   485 	d->PDO_status[pdoNum].transmit_type_parameter = 0;
       
   486 	MSG_WAR(0x3964, "  PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
       
   487  {
       
   488  Message msg_init = Message_Initializer;
       
   489  pdo = msg_init;
       
   490  }	
       
   491         if(buildPDO(d, pdoNum, &pdo))
       
   492         {
   545         {
   493             MSG_ERR(0x1906, " Couldn't build TPDO number : ", pdoNum);
   546           switch (status)
   494 	    status = state11;
   547             {
   495 	    break;
   548             case state3:
   496         }
   549               if (              /*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5) */
   497 	status = state5;
   550                    /* check if TPDO is not valid */
   498       /* If transmission RTR, with data sampled on SYNC */
   551                    *(UNS32 *) d->objdict[offsetObjdict].pSubindex[1].
   499       }else if( isSyncEvent && 
   552                    pObject & 0x80000000)
   500       	  (*pTransmissionType == TRANS_RTR_SYNC)) {
   553                 {
   501           if(buildPDO(d, pdoNum, &d->PDO_status[pdoNum].last_message))
   554                   MSG_WAR (0x3960, "Not a valid PDO ", 0x1800 + pdoNum);
   502           {
   555                   /*Go next TPDO */
   503              MSG_ERR(0x1966, " Couldn't build TPDO number : ", pdoNum);
   556                   status = state11;
   504              d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_RTR_SYNC_READY; 
   557                   break;
   505           }else{
   558                 }
   506              d->PDO_status[pdoNum].transmit_type_parameter |= PDO_RTR_SYNC_READY; 
   559               /* get the PDO transmission type */
   507           }
   560               pTransmissionType =
   508 	  status = state11;
   561                 (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
   509 	  break;
   562               MSG_WAR (0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);
   510       /* If transmission on Event and not inhibited, check for changes */
   563 
   511       }else if((*pTransmissionType == TRANS_EVENT_PROFILE ||
   564               /* check if transmission type is SYNCRONOUS */
   512                *pTransmissionType == TRANS_EVENT_SPECIFIC )&&
   565               /* The message is transmited every n SYNC with n=TransmissionType */
   513               !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) {
   566               if (isSyncEvent &&
   514 	MSG_WAR(0x3968, "  PDO is on EVENT. Trans type : ", *pTransmissionType);
   567                   (*pTransmissionType >= TRANS_SYNC_MIN) &&
   515  {
   568                   (*pTransmissionType <= TRANS_SYNC_MAX) &&
   516  Message msg_init = Message_Initializer;
   569                   (++d->PDO_status[pdoNum].transmit_type_parameter ==
   517  pdo = msg_init;
   570                    *pTransmissionType))
   518  }	
   571                 {
   519         if(buildPDO(d, pdoNum, &pdo))
   572                   /*Reset count of SYNC */
   520         {
   573                   d->PDO_status[pdoNum].transmit_type_parameter = 0;
   521             MSG_ERR(0x3907, " Couldn't build TPDO number : ", pdoNum);
   574                   MSG_WAR (0x3964, "  PDO is on SYNCHRO. Trans type : ",
   522 	    status = state11;
   575                            *pTransmissionType);
   523 	    break;
   576                   {
   524         }
   577                     Message msg_init = Message_Initializer;
   525         
   578                     pdo = msg_init;
   526 	/*Compare new and old PDO*/
   579                   }
   527 	if(d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id &&
   580                   if (buildPDO (d, pdoNum, &pdo))
   528 	   d->PDO_status[pdoNum].last_message.len == pdo.len &&
   581                     {
       
   582                       MSG_ERR (0x1906, " Couldn't build TPDO number : ",
       
   583                                pdoNum);
       
   584                       status = state11;
       
   585                       break;
       
   586                     }
       
   587                   status = state5;
       
   588                   /* If transmission RTR, with data sampled on SYNC */
       
   589                 }
       
   590               else if (isSyncEvent && (*pTransmissionType == TRANS_RTR_SYNC))
       
   591                 {
       
   592                   if (buildPDO
       
   593                       (d, pdoNum, &d->PDO_status[pdoNum].last_message))
       
   594                     {
       
   595                       MSG_ERR (0x1966, " Couldn't build TPDO number : ",
       
   596                                pdoNum);
       
   597                       d->PDO_status[pdoNum].transmit_type_parameter &=
       
   598                         ~PDO_RTR_SYNC_READY;
       
   599                     }
       
   600                   else
       
   601                     {
       
   602                       d->PDO_status[pdoNum].transmit_type_parameter |=
       
   603                         PDO_RTR_SYNC_READY;
       
   604                     }
       
   605                   status = state11;
       
   606                   break;
       
   607                   /* If transmission on Event and not inhibited, check for changes */
       
   608                 }
       
   609               else if ((*pTransmissionType == TRANS_EVENT_PROFILE ||
       
   610                         *pTransmissionType == TRANS_EVENT_SPECIFIC) &&
       
   611                        !(d->PDO_status[pdoNum].
       
   612                          transmit_type_parameter & PDO_INHIBITED))
       
   613                 {
       
   614                   MSG_WAR (0x3968, "  PDO is on EVENT. Trans type : ",
       
   615                            *pTransmissionType);
       
   616                   {
       
   617                     Message msg_init = Message_Initializer;
       
   618                     pdo = msg_init;
       
   619                   }
       
   620                   if (buildPDO (d, pdoNum, &pdo))
       
   621                     {
       
   622                       MSG_ERR (0x3907, " Couldn't build TPDO number : ",
       
   623                                pdoNum);
       
   624                       status = state11;
       
   625                       break;
       
   626                     }
       
   627 
       
   628                   /*Compare new and old PDO */
       
   629                   if (d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id
       
   630                       && d->PDO_status[pdoNum].last_message.len == pdo.len &&
   529 #ifdef UNS64
   631 #ifdef UNS64
   530 	   *(UNS64*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS64*)(&pdo.data[0])){
   632                       *(UNS64 *) (&d->PDO_status[pdoNum].last_message.
   531 #else /* don't ALLOW_64BIT_OPS*/
   633                                   data[0]) == *(UNS64 *) (&pdo.data[0])
   532        *(UNS32*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS32*)(&pdo.data[0]) &&
   634 #else /* don't ALLOW_64BIT_OPS */
   533        *(UNS32*)(&d->PDO_status[pdoNum].last_message.data[4]) == *(UNS32*)(&pdo.data[4])){
   635                       *(UNS32 *) (&d->PDO_status[pdoNum].last_message.
   534 #endif 
   636                                   data[0]) == *(UNS32 *) (&pdo.data[0])
   535 	   	/* No changes -> go to next pdo*/
   637                       && *(UNS32 *) (&d->PDO_status[pdoNum].last_message.
   536 		status = state11;
   638                                      data[4]) == *(UNS32 *) (&pdo.data[4])
   537 	}else{
   639 #endif
   538 		
   640                     )
   539 		UNS16 EventTimerDuration;
   641                     {
   540         UNS16 InhibitTimerDuration;
   642                       /* No changes -> go to next pdo */
   541 		
   643                       status = state11;
   542 		MSG_WAR(0x306A, "Changes TPDO number : ", pdoNum);
   644                     }
   543 		/* Changes detected -> transmit message */
   645                   else
   544         EventTimerDuration = *(UNS16*)d->objdict[offsetObjdict].pSubindex[5].pObject;
   646                     {
   545         InhibitTimerDuration = *(UNS16*)d->objdict[offsetObjdict].pSubindex[3].pObject;
   647 
   546         
   648                       UNS16 EventTimerDuration;
   547 		status = state5;
   649                       UNS16 InhibitTimerDuration;
   548 		
   650 
   549 		/* Start both event_timer and inhibit_timer*/
   651                       MSG_WAR (0x306A, "Changes TPDO number : ", pdoNum);
   550         if(EventTimerDuration){
   652                       /* Changes detected -> transmit message */
   551            DelAlarm(d->PDO_status[pdoNum].event_timer);
   653                       EventTimerDuration =
   552            d->PDO_status[pdoNum].event_timer = SetAlarm(d, pdoNum, &PDOEventTimerAlarm, MS_TO_TIMEVAL(EventTimerDuration), 0);
   654                         *(UNS16 *) d->objdict[offsetObjdict].pSubindex[5].
   553         }
   655                         pObject;
   554 		
   656                       InhibitTimerDuration =
   555         if(InhibitTimerDuration){
   657                         *(UNS16 *) d->objdict[offsetObjdict].pSubindex[3].
   556 		   DelAlarm(d->PDO_status[pdoNum].inhibit_timer);
   658                         pObject;
   557 		   d->PDO_status[pdoNum].inhibit_timer = SetAlarm(d, pdoNum, &PDOInhibitTimerAlarm, US_TO_TIMEVAL(InhibitTimerDuration * 100), 0);
   659 
   558            /* and inhibit TPDO */
   660                       status = state5;
   559            d->PDO_status[pdoNum].transmit_type_parameter |= PDO_INHIBITED;
   661 
   560         }
   662                       /* Start both event_timer and inhibit_timer */
   561 		
   663                       if (EventTimerDuration)
   562 	}
   664                         {
   563       }else{
   665                           DelAlarm (d->PDO_status[pdoNum].event_timer);
   564 	MSG_WAR(0x306C, "  PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", *pTransmissionType);
   666                           d->PDO_status[pdoNum].event_timer =
   565 	status = state11;
   667                             SetAlarm (d, pdoNum, &PDOEventTimerAlarm,
   566       }      
   668                                       MS_TO_TIMEVAL (EventTimerDuration), 0);
   567         break;
   669                         }
   568     case state5: /*Send the pdo*/
   670 
   569 	/*store_as_last_message*/
   671                       if (InhibitTimerDuration)
   570 	d->PDO_status[pdoNum].last_message = pdo;	
   672                         {
   571 	MSG_WAR(0x396D, "sendPDO cobId :", pdo.cob_id);
   673                           DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
   572 	MSG_WAR(0x396E,  "     Nb octets  : ",  pdo.len);
   674                           d->PDO_status[pdoNum].inhibit_timer =
   573     	
   675                             SetAlarm (d, pdoNum, &PDOInhibitTimerAlarm,
   574     	canSend(d->canHandle,&pdo);
   676                                       US_TO_TIMEVAL (InhibitTimerDuration *
   575 	status = state11;
   677                                                      100), 0);
   576 	break;     
   678                           /* and inhibit TPDO */
   577     case state11: /*Go to next TPDO*/     
   679                           d->PDO_status[pdoNum].transmit_type_parameter |=
   578 	pdoNum++;
   680                             PDO_INHIBITED;
   579 	offsetObjdict++;
   681                         }
   580 	offsetObjdictMap++;
   682 
   581 	MSG_WAR(0x3970, "next pdo index : ", pdoNum);
   683                     }
   582 	status = state3;
   684                 }
   583 	break;
   685               else
   584       
   686                 {
   585     default:
   687                   MSG_WAR (0x306C,
   586       MSG_ERR(0x1972,"Unknown state has been reached : %d",status);
   688                            "  PDO is not on EVENT or synchro or not at this SYNC. Trans type : ",
   587       return 0xFF;
   689                            *pTransmissionType);
   588     }/* end switch case */
   690                   status = state11;
   589     
   691                 }
   590   }/* end while */
   692               break;
   591   }
   693             case state5:       /*Send the pdo */
       
   694               /*store_as_last_message */
       
   695               d->PDO_status[pdoNum].last_message = pdo;
       
   696               MSG_WAR (0x396D, "sendPDO cobId :", pdo.cob_id);
       
   697               MSG_WAR (0x396E, "     Nb octets  : ", pdo.len);
       
   698 
       
   699               canSend (d->canHandle, &pdo);
       
   700               status = state11;
       
   701               break;
       
   702             case state11:      /*Go to next TPDO */
       
   703               pdoNum++;
       
   704               offsetObjdict++;
       
   705               offsetObjdictMap++;
       
   706               MSG_WAR (0x3970, "next pdo index : ", pdoNum);
       
   707               status = state3;
       
   708               break;
       
   709 
       
   710             default:
       
   711               MSG_ERR (0x1972, "Unknown state has been reached : %d", status);
       
   712               return 0xFF;
       
   713             }                   /* end switch case */
       
   714 
       
   715         }                       /* end while */
       
   716     }
   592   return 0;
   717   return 0;
   593 }
   718 }
   594 
   719 
   595 /*!
   720 /*!
   596 **
   721 **
   599 ** @param OD_entry
   724 ** @param OD_entry
   600 ** @param bSubindex 
   725 ** @param bSubindex 
   601 ** @return always 0
   726 ** @return always 0
   602 **/
   727 **/
   603 
   728 
   604 UNS32 TPDO_Communication_Parameter_Callback(CO_Data* d, const indextable * OD_entry, UNS8 bSubindex)
   729 UNS32
       
   730 TPDO_Communication_Parameter_Callback (CO_Data * d,
       
   731                                        const indextable * OD_entry,
       
   732                                        UNS8 bSubindex)
   605 {
   733 {
   606   /* If PDO are actives */
   734   /* If PDO are actives */
   607   if(d->CurrentCommunicationState.csPDO) switch(bSubindex)
   735   if (d->CurrentCommunicationState.csPDO)
   608   {
   736     switch (bSubindex)
   609   	case 2: /* Changed transmition type */
   737       {
   610   	case 3: /* Changed inhibit time */
   738       case 2:                  /* Changed transmition type */
   611   	case 5: /* Changed event time */
   739       case 3:                  /* Changed inhibit time */
   612   	{
   740       case 5:                  /* Changed event time */
   613           const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS; 
   741         {
   614           UNS8 numPdo = OD_entry - TPDO_com;       /* number of the actual processed pdo-nr. */
   742           const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS;
   615         
   743           UNS8 numPdo = OD_entry - TPDO_com;    /* number of the actual processed pdo-nr. */
       
   744 
   616           /* Zap all timers and inhibit flag */
   745           /* Zap all timers and inhibit flag */
   617           d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer);
   746           d->PDO_status[numPdo].event_timer =
   618           d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer);
   747             DelAlarm (d->PDO_status[numPdo].event_timer);
       
   748           d->PDO_status[numPdo].inhibit_timer =
       
   749             DelAlarm (d->PDO_status[numPdo].inhibit_timer);
   619           d->PDO_status[numPdo].transmit_type_parameter = 0;
   750           d->PDO_status[numPdo].transmit_type_parameter = 0;
   620           /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
   751           /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
   621           PDOEventTimerAlarm(d, numPdo);
   752           PDOEventTimerAlarm (d, numPdo);
   622           return 0;
   753           return 0;
   623   	}
   754         }
   624 
   755 
   625   	default: /* other subindex are ignored*/
   756       default:                 /* other subindex are ignored */
   626   	break;
   757         break;
   627   }
   758       }
   628   return 0;
   759   return 0;
   629 }
   760 }
   630 
   761 
   631 void PDOInit(CO_Data* d)
   762 void
       
   763 PDOInit (CO_Data * d)
   632 {
   764 {
   633   /* For each TPDO mapping parameters */
   765   /* For each TPDO mapping parameters */
   634   UNS16	pdoIndex = 0x1800; /* OD index of TDPO */
   766   UNS16 pdoIndex = 0x1800;      /* OD index of TDPO */
   635 
   767 
   636   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
   768   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
   637   UNS16 lastIndex = d->lastIndex->PDO_TRS;
   769   UNS16 lastIndex = d->lastIndex->PDO_TRS;
   638   if(offsetObjdict) while( offsetObjdict <= lastIndex) {
   770   if (offsetObjdict)
   639     /* Assign callbacks to sensible TPDO mapping subindexes */
   771     while (offsetObjdict <= lastIndex)
   640     UNS32 errorCode;
   772       {
   641     ODCallback_t *CallbackList;
   773         /* Assign callbacks to sensible TPDO mapping subindexes */
   642     /* Find callback list */
   774         UNS32 errorCode;
   643     scanIndexOD (d, pdoIndex, &errorCode, &CallbackList);
   775         ODCallback_t *CallbackList;
   644     if(errorCode == OD_SUCCESSFUL && CallbackList)
   776         /* Find callback list */
   645     {
   777         scanIndexOD (d, pdoIndex, &errorCode, &CallbackList);
   646       /*Assign callbacks to corresponding subindex*/
   778         if (errorCode == OD_SUCCESSFUL && CallbackList)
   647       /* Transmission type */
   779           {
   648       CallbackList[2] = &TPDO_Communication_Parameter_Callback;
   780             /*Assign callbacks to corresponding subindex */
   649       /* Inhibit time */
   781             /* Transmission type */
   650       CallbackList[3] = &TPDO_Communication_Parameter_Callback;
   782             CallbackList[2] = &TPDO_Communication_Parameter_Callback;
   651       /* Event timer */
   783             /* Inhibit time */
   652       CallbackList[5] = &TPDO_Communication_Parameter_Callback;
   784             CallbackList[3] = &TPDO_Communication_Parameter_Callback;
   653     }
   785             /* Event timer */
   654     pdoIndex++;
   786             CallbackList[5] = &TPDO_Communication_Parameter_Callback;
   655     offsetObjdict++;
   787           }
   656   }  
   788         pdoIndex++;
       
   789         offsetObjdict++;
       
   790       }
   657 
   791 
   658   /* Trigger a non-sync event */
   792   /* Trigger a non-sync event */
   659   _sendPDOevent( d, 0 );
   793   _sendPDOevent (d, 0);
   660 }
   794 }
   661 
   795 
   662 void PDOStop(CO_Data* d)
   796 void
       
   797 PDOStop (CO_Data * d)
   663 {
   798 {
   664   /* For each TPDO mapping parameters */
   799   /* For each TPDO mapping parameters */
   665   UNS8 	pdoNum = 0x00;       /* number of the actual processed pdo-nr. */
   800   UNS8 pdoNum = 0x00;           /* number of the actual processed pdo-nr. */
   666   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
   801   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
   667   UNS16 lastIndex = d->lastIndex->PDO_TRS;
   802   UNS16 lastIndex = d->lastIndex->PDO_TRS;
   668   if(offsetObjdict) while( offsetObjdict <= lastIndex) {
   803   if (offsetObjdict)
   669   	/* Delete TPDO timers */
   804     while (offsetObjdict <= lastIndex)
   670 	d->PDO_status[pdoNum].event_timer = DelAlarm(d->PDO_status[pdoNum].event_timer);
   805       {
   671 	d->PDO_status[pdoNum].inhibit_timer = DelAlarm(d->PDO_status[pdoNum].inhibit_timer);
   806         /* Delete TPDO timers */
   672 	/* Reset transmit type parameter */
   807         d->PDO_status[pdoNum].event_timer =
   673 	d->PDO_status[pdoNum].transmit_type_parameter = 0;
   808           DelAlarm (d->PDO_status[pdoNum].event_timer);
   674 	d->PDO_status[pdoNum].last_message.cob_id = 0;
   809         d->PDO_status[pdoNum].inhibit_timer =
   675 	pdoNum++;
   810           DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
   676 	offsetObjdict++;
   811         /* Reset transmit type parameter */
   677   }  
   812         d->PDO_status[pdoNum].transmit_type_parameter = 0;
   678 }
   813         d->PDO_status[pdoNum].last_message.cob_id = 0;
       
   814         pdoNum++;
       
   815         offsetObjdict++;
       
   816       }
       
   817 }