pdo.c

00001 /*
00002   This file is part of CanFestival, a library implementing CanOpen
00003   Stack.
00004 
00005   Copyright (C): Edouard TISSERANT and Francis DUPIN
00006 
00007   See COPYING file for copyrights details.
00008 
00009   This library is free software; you can redistribute it and/or
00010   modify it under the terms of the GNU Lesser General Public
00011   License as published by the Free Software Foundation; either
00012   version 2.1 of the License, or (at your option) any later version.
00013 
00014   This library is distributed in the hope that it will be useful,
00015   but WITHOUT ANY WARRANTY; without even the implied warranty of
00016   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00017   Lesser General Public License for more details.
00018 
00019   You should have received a copy of the GNU Lesser General Public
00020   License along with this library; if not, write to the Free Software
00021   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
00022   USA
00023 */
00024 #include "pdo.h"
00025 #include "objacces.h"
00026 #include "canfestival.h"
00027 #include "sysdep.h"
00028 
00029 /*!
00030 ** @file   pdo.c
00031 ** @author Edouard TISSERANT and Francis DUPIN
00032 ** @date   Tue Jun  5 09:32:32 2007
00033 **
00034 ** @brief
00035 **
00036 **
00037 */
00038 
00039 /*!
00040 **
00041 **
00042 ** @param d
00043 ** @param TPDO_com TPDO communication parameters OD entry
00044 ** @param TPDO_map TPDO mapping parameters OD entry
00045 **
00046 ** @return
00047 **/
00048 
00049 UNS8 buildPDO (CO_Data * d, UNS8 numPdo, Message * pdo)
00050 {
00051   const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo;
00052   const indextable *TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;
00053 
00054   UNS8 prp_j = 0x00;
00055   UNS32 offset = 0x00000000;
00056   const UNS8 *pMappingCount = (UNS8 *) TPDO_map->pSubindex[0].pObject;
00057 
00058   pdo->cob_id = (UNS16) UNS16_LE(*(UNS32*)TPDO_com->pSubindex[1].pObject & 0x7FF);
00059   pdo->rtr = NOT_A_REQUEST;
00060 
00061   MSG_WAR (0x3009, "  PDO CobId is : ",
00062            *(UNS32 *) TPDO_com->pSubindex[1].pObject);
00063   MSG_WAR (0x300D, "  Number of objects mapped : ", *pMappingCount);
00064 
00065   do
00066     {
00067       UNS8 dataType;            /* Unused */
00068       UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 };  /* temporary space to hold bits */
00069 
00070       /* pointer fo the var which holds the mapping parameter of an mapping entry  */
00071       UNS32 *pMappingParameter =
00072         (UNS32 *) TPDO_map->pSubindex[prp_j + 1].pObject;
00073       UNS16 index = (UNS16) ((*pMappingParameter) >> 16);
00074       UNS32 Size = (UNS32) (*pMappingParameter & (UNS32) 0x000000FF);     /* Size in bits */
00075 
00076       /* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */
00077       if (Size && ((offset + Size) <= 64))
00078         {
00079           UNS32 ByteSize = 1 + ((Size - 1) >> 3);        /*1->8 => 1 ; 9->16 => 2, ... */
00080           UNS8 subIndex =
00081             (UNS8) (((*pMappingParameter) >> (UNS8) 8) & (UNS32) 0x000000FF);
00082 
00083           MSG_WAR (0x300F, "  got mapping parameter : ", *pMappingParameter);
00084           MSG_WAR (0x3050, "    at index : ", TPDO_map->index);
00085           MSG_WAR (0x3051, "    sub-index : ", prp_j + 1);
00086 
00087           if (getODentry (d, index, subIndex, tmp, &ByteSize, &dataType, 0) !=
00088               OD_SUCCESSFUL)
00089             {
00090               MSG_ERR (0x1013,
00091                        " Couldn't find mapped variable at index-subindex-size : ",
00092                        (UNS16) (*pMappingParameter));
00093               return 0xFF;
00094             }
00095           /* copy bit per bit in little endian */
00096           CopyBits  ((UNS8) Size, ((UNS8 *) tmp), 0, 0,
00097                     (UNS8 *) & pdo->data[offset >> 3], (UNS8)(offset % 8), 0);
00098 
00099           offset += Size;
00100         }
00101       prp_j++;
00102     }
00103   while (prp_j < *pMappingCount);
00104 
00105   pdo->len = (UNS8)(1 + ((offset - 1) >> 3));
00106 
00107   MSG_WAR (0x3015, "  End scan mapped variable", 0);
00108 
00109   return 0;
00110 }
00111 
00112 /*!
00113 **
00114 **
00115 ** @param d
00116 ** @param cobId
00117 **
00118 ** @return
00119 **/
00120 UNS8
00121 sendPDOrequest (CO_Data * d, UNS16 RPDOIndex)
00122 {
00123   UNS32 *pwCobId;
00124   UNS16 offset = d->firstIndex->PDO_RCV;
00125   UNS16 lastIndex = d->lastIndex->PDO_RCV;
00126 
00127   if (!d->CurrentCommunicationState.csPDO)
00128     {
00129       return 0;
00130     }
00131 
00132   /* Sending the request only if the cobid have been found on the PDO
00133      receive */
00134   /* part dictionary */
00135 
00136   MSG_WAR (0x3930, "sendPDOrequest RPDO Index : ", RPDOIndex);
00137 
00138   if (offset && RPDOIndex >= 0x1400)
00139     {
00140       offset += RPDOIndex - 0x1400;
00141       if (offset <= lastIndex)
00142         {
00143           /* get the CobId */
00144           // mbed error 513 - needs typecast
00145           pwCobId = (unsigned int*)d->objdict[offset].pSubindex[1].pObject;
00146 
00147           MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId);
00148           {
00149             Message pdo;
00150             pdo.cob_id = (UNS16)UNS16_LE(*pwCobId);
00151             pdo.rtr = REQUEST;
00152             pdo.len = 0;
00153             return canSend (d->canHandle, &pdo);
00154           }
00155         }
00156     }
00157   MSG_ERR (0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex);
00158   return 0xFF;
00159 }
00160 
00161 
00162 /*!
00163 **
00164 **
00165 ** @param d
00166 ** @param m
00167 **
00168 ** @return
00169 **/
00170 UNS8
00171 proceedPDO (CO_Data * d, Message * m)
00172 {
00173   UNS8 numPdo;
00174   UNS8 numMap;                  /* Number of the mapped varable */
00175   UNS8 *pMappingCount = NULL;   /* count of mapped objects... */
00176   /* pointer to the var which is mapped to a pdo... */
00177   /*  void *     pMappedAppObject = NULL;   */
00178   /* pointer fo the var which holds the mapping parameter of an
00179      mapping entry */
00180   UNS32 *pMappingParameter = NULL;
00181   UNS8 *pTransmissionType = NULL;       /* pointer to the transmission
00182                                            type */
00183   UNS32 *pwCobId = NULL;
00184   UNS8 Size;
00185   UNS8 offset;
00186   UNS8 status;
00187   UNS32 objDict;
00188   UNS16 offsetObjdict;
00189   UNS16 lastIndex;
00190 
00191   status = state2;
00192 
00193   MSG_WAR (0x3935, "proceedPDO, cobID : ", (UNS16_LE(m->cob_id) & 0x7ff));
00194   offset = 0x00;
00195   numPdo = 0;
00196   numMap = 0;
00197   if ((*m).rtr == NOT_A_REQUEST)
00198     {                           /* The PDO received is not a
00199                                    request. */
00200 
00201       offsetObjdict = d->firstIndex->PDO_RCV;
00202       lastIndex = d->lastIndex->PDO_RCV;
00203 
00204       /* study of all the PDO stored in the dictionary */
00205       if (offsetObjdict)
00206         while (offsetObjdict <= lastIndex)
00207           {
00208 
00209             switch (status)
00210               {
00211 
00212               case state2:
00213                 /* get CobId of the dictionary correspondant to the received
00214                    PDO */
00215                 // mbed error 513 - needs typecast
00216                 pwCobId = (unsigned int*)
00217                   d->objdict[offsetObjdict].pSubindex[1].pObject;
00218                 /* check the CobId coherance */
00219                 /*pwCobId is the cobId read in the dictionary at the state 3
00220                  */
00221                 if (*pwCobId == UNS16_LE(m->cob_id))
00222                   {
00223                     /* The cobId is recognized */
00224                     status = state4;
00225                     MSG_WAR (0x3936, "cobId found at index ",
00226                              0x1400 + numPdo);
00227                     break;
00228                   }
00229                 else
00230                   {
00231                     /* cobId received does not match with those write in the
00232                        dictionnary */
00233                     numPdo++;
00234                     offsetObjdict++;
00235                     status = state2;
00236                     break;
00237                   }
00238 
00239               case state4:     /* Get Mapped Objects Number */
00240                 /* The cobId of the message received has been found in the
00241                    dictionnary. */
00242                 offsetObjdict = d->firstIndex->PDO_RCV_MAP;
00243                 lastIndex = d->lastIndex->PDO_RCV_MAP;
00244                 pMappingCount =
00245                   (UNS8 *) (d->objdict + offsetObjdict +
00246                             numPdo)->pSubindex[0].pObject;
00247                 numMap = 0;
00248                 while (numMap < *pMappingCount)
00249                   {
00250                     UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
00251                     UNS32 ByteSize;
00252                     pMappingParameter =
00253                       (UNS32 *) (d->objdict + offsetObjdict +
00254                                  numPdo)->pSubindex[numMap + 1].pObject;
00255                     if (pMappingParameter == NULL)
00256                       {
00257                         MSG_ERR (0x1937, "Couldn't get mapping parameter : ",
00258                                  numMap + 1);
00259                         return 0xFF;
00260                       }
00261                     /* Get the addresse of the mapped variable. */
00262                     /* detail of *pMappingParameter : */
00263                     /* The 16 hight bits contains the index, the medium 8 bits
00264                        contains the subindex, */
00265                     /* and the lower 8 bits contains the size of the mapped
00266                        variable. */
00267 
00268                     Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF);
00269 
00270                     /* set variable only if Size != 0 and 
00271                      * Size is lower than remaining bits in the PDO */
00272                     if (Size && ((offset + Size) <= (m->len << 3)))
00273                       {
00274                         /* copy bit per bit in little endian */
00275                         CopyBits  (Size, (UNS8 *) & m->data[offset >> 3],
00276                                   offset % 8, 0, ((UNS8 *) tmp), 0, 0);
00277                         /*1->8 => 1 ; 9->16 =>2, ... */
00278                         ByteSize = (UNS32)(1 + ((Size - 1) >> 3));
00279 
00280                         objDict =
00281                           setODentry (d, (UNS16) ((*pMappingParameter) >> 16),
00282                                       (UNS8) (((*pMappingParameter) >> 8) &
00283                                               0xFF), tmp, &ByteSize, 0);
00284 
00285                         if (objDict != OD_SUCCESSFUL)
00286                           {
00287                             MSG_ERR (0x1938,
00288                                      "error accessing to the mapped var : ",
00289                                      numMap + 1);
00290                             MSG_WAR (0x2939, "         Mapped at index : ",
00291                                      (*pMappingParameter) >> 16);
00292                             MSG_WAR (0x2940, "                subindex : ",
00293                                      ((*pMappingParameter) >> 8) & 0xFF);
00294                             return 0xFF;
00295                           }
00296 
00297                         MSG_WAR (0x3942,
00298                                  "Variable updated by PDO cobid : ",
00299                                  UNS16_LE(m->cob_id));
00300                         MSG_WAR (0x3943, "         Mapped at index : ",
00301                                  (*pMappingParameter) >> 16);
00302                         MSG_WAR (0x3944, "                subindex : ",
00303                                  ((*pMappingParameter) >> 8) & 0xFF);
00304                         offset += Size;
00305                       }
00306                     numMap++;
00307                   }             /* end loop while on mapped variables */
00308 
00309                 return 0;
00310 
00311               }                 /* end switch status */
00312           }                     /* end while */
00313     }                           /* end if Donnees */
00314   else if ((*m).rtr == REQUEST)
00315     {
00316       MSG_WAR (0x3946, "Receive a PDO request cobId : ", UNS16_LE(m->cob_id));
00317       status = state1;
00318       offsetObjdict = d->firstIndex->PDO_TRS;
00319       lastIndex = d->lastIndex->PDO_TRS;
00320       if (offsetObjdict)
00321         while (offsetObjdict <= lastIndex)
00322           {
00323             /* study of all PDO stored in the objects dictionary */
00324 
00325             switch (status)
00326               {
00327 
00328               case state1:     /* check the CobId */
00329                 /* get CobId of the dictionary which match to the received PDO
00330                  */
00331                 // mbed error 513 - needs typecast
00332                 pwCobId = (unsigned int*)
00333                    (d->objdict +
00334                              offsetObjdict)->pSubindex[1].pObject;
00335                 if (*pwCobId == UNS16_LE(m->cob_id))
00336                   {
00337                     status = state4;
00338                     break;
00339                   }
00340                 else
00341                   {
00342                     numPdo++;
00343                     offsetObjdict++;
00344                   }
00345                 status = state1;
00346                 break;
00347 
00348 
00349               case state4:     /* check transmission type */
00350                 pTransmissionType =
00351                   (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
00352                 /* If PDO is to be sampled and send on RTR, do it */
00353                 if ((*pTransmissionType == TRANS_RTR))
00354                   {
00355                     status = state5;
00356                     break;
00357                   }
00358                 /* RTR_SYNC means data prepared at SYNC, transmitted on RTR */
00359                 else if ((*pTransmissionType == TRANS_RTR_SYNC))
00360                   {
00361                     if (d->PDO_status[numPdo].
00362                         transmit_type_parameter & PDO_RTR_SYNC_READY)
00363                       {
00364                         /*Data ready, just send */
00365                         canSend (d->canHandle,
00366                                  &d->PDO_status[numPdo].last_message);
00367                         return 0;
00368                       }
00369                     else
00370                       {
00371                         /* if SYNC did never occur, transmit current data */
00372                         /* DS301 do not tell what to do in such a case... */
00373                         MSG_ERR (0x1947,
00374                                  "Not ready RTR_SYNC TPDO send current data : ",
00375                                  UNS16_LE(m->cob_id));
00376                         status = state5;
00377                       }
00378                     break;
00379                   }
00380                 else if ((*pTransmissionType == TRANS_EVENT_PROFILE) ||
00381                          (*pTransmissionType == TRANS_EVENT_SPECIFIC))
00382                   {
00383                     /* Zap all timers and inhibit flag */
00384                     d->PDO_status[numPdo].event_timer =
00385                       DelAlarm (d->PDO_status[numPdo].event_timer);
00386                     d->PDO_status[numPdo].inhibit_timer =
00387                       DelAlarm (d->PDO_status[numPdo].inhibit_timer);
00388                     d->PDO_status[numPdo].transmit_type_parameter &=
00389                       ~PDO_INHIBITED;
00390                     /* Call  PDOEventTimerAlarm for this TPDO, 
00391                      * this will trigger emission et reset timers */
00392                     PDOEventTimerAlarm (d, numPdo);
00393                     return 0;
00394                   }
00395                 else
00396                   {
00397                     /* The requested PDO is not to send on request. So, does
00398                        nothing. */
00399                     MSG_WAR (0x2947, "PDO is not to send on request : ",
00400                              UNS16_LE(m->cob_id));
00401                     return 0xFF;
00402                   }
00403 
00404               case state5:     /* build and send requested PDO */
00405                 {
00406                   Message pdo;
00407                   if (buildPDO (d, numPdo, &pdo))
00408                     {
00409                       MSG_ERR (0x1948, " Couldn't build TPDO n&#65533;", numPdo);
00410                       return 0xFF;
00411                     }
00412                   canSend (d->canHandle, &pdo);
00413                   return 0;
00414                 }
00415               }                 /* end switch status */
00416           }                     /* end while */
00417     }                           /* end if Requete */
00418 
00419   return 0;
00420 }
00421 
00422 /*!
00423 **
00424 **
00425 ** @param NbBits
00426 ** @param SrcByteIndex
00427 ** @param SrcBitIndex
00428 ** @param SrcBigEndian
00429 ** @param DestByteIndex
00430 ** @param DestBitIndex
00431 ** @param DestBigEndian
00432 **/
00433 void
00434 CopyBits  (UNS8 NbBits, UNS8 * SrcByteIndex, UNS8 SrcBitIndex,
00435           UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex,
00436           UNS8 DestBigEndian)
00437 {
00438   /* This loop copy as many bits that it can each time, crossing */
00439   /* successively bytes */
00440   // boundaries from LSB to MSB.
00441   while (NbBits > 0)
00442     {
00443       /* Bit missalignement between src and dest */
00444       INTEGER8 Vect = DestBitIndex - SrcBitIndex;
00445 
00446       /* We can now get src and align it to dest */
00447       UNS8 Aligned =
00448         Vect > 0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
00449 
00450       /* Compute the nb of bit we will be able to copy */
00451       UNS8 BoudaryLimit = (Vect > 0 ? 8 - DestBitIndex : 8 - SrcBitIndex);
00452       UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
00453 
00454       /* Create a mask that will serve in: */
00455       UNS8 Mask =
00456         ((0xff << (DestBitIndex + BitsToCopy)) |
00457          (0xff >> (8 - DestBitIndex)));
00458 
00459       /* - Filtering src */
00460       UNS8 Filtered = Aligned & ~Mask;
00461 
00462       /* - and erase bits where we write, preserve where we don't */
00463       *DestByteIndex &= Mask;
00464 
00465       /* Then write. */
00466       *DestByteIndex |= Filtered;
00467 
00468       /*Compute next time cursors for src */
00469       if ((SrcBitIndex += BitsToCopy) > 7)      /* cross boundary ? */
00470         {
00471           SrcBitIndex = 0;      /* First bit */
00472           SrcByteIndex += (SrcBigEndian ? -1 : 1);      /* Next byte */
00473         }
00474 
00475 
00476       /*Compute next time cursors for dest */
00477       if ((DestBitIndex += BitsToCopy) > 7)
00478         {
00479           DestBitIndex = 0;     /* First bit */
00480           DestByteIndex += (DestBigEndian ? -1 : 1);    /* Next byte */
00481         }
00482 
00483       /*And decrement counter. */
00484       NbBits -= BitsToCopy;
00485     }
00486 
00487 }
00488 
00489 static void sendPdo(CO_Data * d, UNS32 pdoNum, Message * pdo)
00490 {
00491   /*store_as_last_message */
00492   d->PDO_status[pdoNum].last_message = *pdo;
00493   MSG_WAR (0x396D, "sendPDO cobId :", UNS16_LE(pdo.cob_id));
00494   MSG_WAR (0x396E, "     Nb octets  : ", pdo.len);
00495 
00496   canSend (d->canHandle, pdo);
00497 }
00498 
00499 
00500 /*!
00501 **
00502 **
00503 ** @param d
00504 **
00505 ** @return
00506 **/
00507 
00508 UNS8
00509 sendPDOevent (CO_Data * d)
00510 {
00511   /* Calls _sendPDOevent specifying it is not a sync event */
00512   return _sendPDOevent (d, 0);
00513 }
00514 
00515 UNS8
00516 sendOnePDOevent (CO_Data * d, UNS8 pdoNum)
00517 {
00518   UNS16 offsetObjdict;
00519   Message pdo;
00520   if (!d->CurrentCommunicationState.csPDO ||
00521       (d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED))
00522     {
00523       return 0;
00524     }
00525 
00526   offsetObjdict = (UNS16) (d->firstIndex->PDO_TRS + pdoNum);
00527   MSG_WAR (0x3968, "  PDO is on EVENT. Trans type : ",
00528            *pTransmissionType);
00529   
00530   memset(&pdo, 0, sizeof(pdo));
00531   if (buildPDO (d, pdoNum, &pdo))
00532     {
00533       MSG_ERR (0x3907, " Couldn't build TPDO number : ",
00534                pdoNum);
00535       return 0;
00536     }
00537 
00538   /*Compare new and old PDO */
00539   if (d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id
00540       && d->PDO_status[pdoNum].last_message.len == pdo.len
00541       && memcmp(d->PDO_status[pdoNum].last_message.data,
00542                     pdo.data, 8) == 0
00543     )
00544     {
00545       /* No changes -> go to next pdo */
00546       return 0;
00547     }
00548   else
00549     {
00550 
00551       TIMEVAL EventTimerDuration;
00552       TIMEVAL InhibitTimerDuration;
00553 
00554       MSG_WAR (0x306A, "Changes TPDO number : ", pdoNum);
00555       /* Changes detected -> transmit message */
00556       EventTimerDuration =
00557         *(UNS16 *) d->objdict[offsetObjdict].pSubindex[5].
00558         pObject;
00559       InhibitTimerDuration =
00560         *(UNS16 *) d->objdict[offsetObjdict].pSubindex[3].
00561         pObject;
00562 
00563       /* Start both event_timer and inhibit_timer */
00564       if (EventTimerDuration)
00565         {
00566           DelAlarm (d->PDO_status[pdoNum].event_timer);
00567           d->PDO_status[pdoNum].event_timer =
00568             SetAlarm (d, pdoNum, &PDOEventTimerAlarm,
00569                       MS_TO_TIMEVAL (EventTimerDuration), 0);
00570         }
00571 
00572       if (InhibitTimerDuration)
00573         {
00574           DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
00575           d->PDO_status[pdoNum].inhibit_timer =
00576             SetAlarm (d, pdoNum, &PDOInhibitTimerAlarm,
00577                       US_TO_TIMEVAL (InhibitTimerDuration *
00578                                      100), 0);
00579           /* and inhibit TPDO */
00580           d->PDO_status[pdoNum].transmit_type_parameter |=
00581             PDO_INHIBITED;
00582         }
00583 
00584       sendPdo(d, pdoNum, &pdo);
00585     }
00586     return 1;
00587 }
00588 
00589 void
00590 PDOEventTimerAlarm (CO_Data * d, UNS32 pdoNum)
00591 {
00592   /* This is needed to avoid deletion of re-attribuated timer */
00593   d->PDO_status[pdoNum].event_timer = TIMER_NONE;
00594   /* force emission of PDO by artificially changing last emitted */
00595   d->PDO_status[pdoNum].last_message.cob_id = 0;
00596   sendOnePDOevent (d, (UNS8) pdoNum);
00597 }
00598 
00599 void
00600 PDOInhibitTimerAlarm (CO_Data * d, UNS32 pdoNum)
00601 {
00602   /* This is needed to avoid deletion of re-attribuated timer */
00603   d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;
00604   /* Remove inhibit flag */
00605   d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;
00606   sendOnePDOevent (d, (UNS8) pdoNum);
00607 }
00608 
00609 /*!
00610 **
00611 **
00612 ** @param d
00613 ** @param isSyncEvent
00614 **
00615 ** @return
00616 **/
00617 
00618 UNS8
00619 _sendPDOevent (CO_Data * d, UNS8 isSyncEvent)
00620 {
00621   UNS8 pdoNum = 0x00;           /* number of the actual processed pdo-nr. */
00622   UNS8 *pTransmissionType = NULL;
00623   UNS8 status = state3;
00624   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
00625   UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
00626   UNS16 lastIndex = d->lastIndex->PDO_TRS;
00627 
00628   if (!d->CurrentCommunicationState.csPDO)
00629     {
00630       return 0;
00631     }
00632 
00633 
00634   /* study all PDO stored in the objects dictionary */
00635   if (offsetObjdict)
00636     {
00637       Message pdo;/* = Message_Initializer;*/
00638       memset(&pdo, 0, sizeof(pdo));
00639       while (offsetObjdict <= lastIndex)
00640         {
00641           switch (status)
00642             {
00643             case state3:
00644               if ( /* bSubCount always 5 with objdictedit -> check disabled */
00645                    /*d->objdict[offsetObjdict].bSubCount < 5 ||*/
00646                    /* check if TPDO is not valid */
00647                    *(UNS32 *) d->objdict[offsetObjdict].pSubindex[1].
00648                    pObject & 0x80000000)
00649                 {
00650                   MSG_WAR (0x3960, "Not a valid PDO ", 0x1800 + pdoNum);
00651                   /*Go next TPDO */
00652                   status = state11;
00653                   break;
00654                 }
00655               /* get the PDO transmission type */
00656               pTransmissionType =
00657                 (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
00658               MSG_WAR (0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);
00659 
00660               /* check if transmission type is SYNCRONOUS */
00661               /* message transmited every n SYNC with n=TransmissionType */
00662               if (isSyncEvent &&
00663                   (*pTransmissionType >= TRANS_SYNC_MIN) &&
00664                   (*pTransmissionType <= TRANS_SYNC_MAX) &&
00665                   (++d->PDO_status[pdoNum].transmit_type_parameter ==
00666                    *pTransmissionType))
00667                 {
00668                   /*Reset count of SYNC */
00669                   d->PDO_status[pdoNum].transmit_type_parameter = 0;
00670                   MSG_WAR (0x3964, "  PDO is on SYNCHRO. Trans type : ",
00671                            *pTransmissionType);
00672                   memset(&pdo, 0, sizeof(pdo));
00673                   /*{
00674                     Message msg_init = Message_Initializer;
00675                     pdo = msg_init;
00676                   }*/
00677                   if (buildPDO (d, pdoNum, &pdo))
00678                     {
00679                       MSG_ERR (0x1906, " Couldn't build TPDO number : ",
00680                                pdoNum);
00681                       status = state11;
00682                       break;
00683                     }
00684                   status = state5;
00685                   /* If transmission RTR, with data sampled on SYNC */
00686                 }
00687               else if (isSyncEvent && (*pTransmissionType == TRANS_RTR_SYNC))
00688                 {
00689                   if (buildPDO
00690                       (d, pdoNum, &d->PDO_status[pdoNum].last_message))
00691                     {
00692                       MSG_ERR (0x1966, " Couldn't build TPDO number : ",
00693                                pdoNum);
00694                       d->PDO_status[pdoNum].transmit_type_parameter &=
00695                         ~PDO_RTR_SYNC_READY;
00696                     }
00697                   else
00698                     {
00699                       d->PDO_status[pdoNum].transmit_type_parameter |=
00700                         PDO_RTR_SYNC_READY;
00701                     }
00702                   status = state11;
00703                   break;
00704                   /* If transmission on Event and not inhibited, check for changes */
00705                 }
00706               else
00707                 if ( (isSyncEvent && (*pTransmissionType == TRANS_SYNC_ACYCLIC))
00708                      ||
00709                      (!isSyncEvent && (*pTransmissionType == TRANS_EVENT_PROFILE || *pTransmissionType == TRANS_EVENT_SPECIFIC)
00710                        && !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)))
00711                 {
00712                   sendOnePDOevent(d, pdoNum);
00713                   status = state11;
00714                 }
00715               else
00716                 {
00717                   MSG_WAR (0x306C,
00718                            "  PDO is not on EVENT or synchro or not at this SYNC. Trans type : ",
00719                            *pTransmissionType);
00720                   status = state11;
00721                 }
00722               break;
00723             case state5:       /*Send the pdo */
00724               sendPdo(d, pdoNum, &pdo);
00725               status = state11;
00726               break;
00727             case state11:      /*Go to next TPDO */
00728               pdoNum++;
00729               offsetObjdict++;
00730               offsetObjdictMap++;
00731               MSG_WAR (0x3970, "next pdo index : ", pdoNum);
00732               status = state3;
00733               break;
00734 
00735             default:
00736               MSG_ERR (0x1972, "Unknown state has been reached : %d", status);
00737               return 0xFF;
00738             }                   /* end switch case */
00739 
00740         }                       /* end while */
00741     }
00742   return 0;
00743 }
00744 
00745 /*!
00746 **
00747 **
00748 ** @param d
00749 ** @param OD_entry
00750 ** @param bSubindex
00751 ** @return always 0
00752 **/
00753 
00754 UNS32
00755 TPDO_Communication_Parameter_Callback  (CO_Data * d,
00756                                        const indextable * OD_entry,
00757                                        UNS8 bSubindex)
00758 {
00759   /* If PDO are actives */
00760   if (d->CurrentCommunicationState.csPDO)
00761     switch (bSubindex)
00762       {
00763       case 2:                  /* Changed transmition type */
00764       case 3:                  /* Changed inhibit time */
00765       case 5:                  /* Changed event time */
00766         {
00767           const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS;
00768           UNS8 numPdo = (UNS8) (OD_entry - TPDO_com);    /* number of the actual processed pdo-nr. */
00769 
00770           /* Zap all timers and inhibit flag */
00771           d->PDO_status[numPdo].event_timer =
00772             DelAlarm (d->PDO_status[numPdo].event_timer);
00773           d->PDO_status[numPdo].inhibit_timer =
00774             DelAlarm (d->PDO_status[numPdo].inhibit_timer);
00775           d->PDO_status[numPdo].transmit_type_parameter = 0;
00776           /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
00777           PDOEventTimerAlarm (d, numPdo);
00778           return 0;
00779         }
00780 
00781       default:                 /* other subindex are ignored */
00782         break;
00783       }
00784   return 0;
00785 }
00786 
00787 void
00788 PDOInit (CO_Data * d)
00789 {
00790   /* For each TPDO mapping parameters */
00791   UNS16 pdoIndex = 0x1800;      /* OD index of TDPO */
00792 
00793   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
00794   UNS16 lastIndex = d->lastIndex->PDO_TRS;
00795   if (offsetObjdict)
00796     while (offsetObjdict <= lastIndex)
00797       {
00798         /* Assign callbacks to sensible TPDO mapping subindexes */
00799         UNS32 errorCode;
00800         ODCallback_t *CallbackList;
00801         /* Find callback list */
00802         scanIndexOD (d, pdoIndex, &errorCode, &CallbackList);
00803         if (errorCode == OD_SUCCESSFUL && CallbackList)
00804           {
00805             /*Assign callbacks to corresponding subindex */
00806             /* Transmission type */
00807             CallbackList[2] = &TPDO_Communication_Parameter_Callback ;
00808             /* Inhibit time */
00809             CallbackList[3] = &TPDO_Communication_Parameter_Callback ;
00810             /* Event timer */
00811             CallbackList[5] = &TPDO_Communication_Parameter_Callback ;
00812           }
00813         pdoIndex++;
00814         offsetObjdict++;
00815       }
00816 
00817   /* Trigger a non-sync event */
00818   _sendPDOevent (d, 0);
00819 }
00820 
00821 void
00822 PDOStop (CO_Data * d)
00823 {
00824   /* For each TPDO mapping parameters */
00825   UNS8 pdoNum = 0x00;           /* number of the actual processed pdo-nr. */
00826   UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
00827   UNS16 lastIndex = d->lastIndex->PDO_TRS;
00828   if (offsetObjdict)
00829     while (offsetObjdict <= lastIndex)
00830       {
00831         /* Delete TPDO timers */
00832         d->PDO_status[pdoNum].event_timer =
00833           DelAlarm (d->PDO_status[pdoNum].event_timer);
00834         d->PDO_status[pdoNum].inhibit_timer =
00835           DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
00836         /* Reset transmit type parameter */
00837         d->PDO_status[pdoNum].transmit_type_parameter = 0;
00838         d->PDO_status[pdoNum].last_message.cob_id = 0;
00839         pdoNum++;
00840         offsetObjdict++;
00841       }
00842 }