src/pdo.c

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