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