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