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