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