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