pdo.c

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