CANopen资料之1：如何通过程序发送SDO,PDO报文

2万 · 2018-8-23 13:49

sdo.c
------------------------------------------------------------------------------------------------------
/*
This file is part of CanFestival, a library implementing CanOpen Stack.
pdo
---------------------------------------------------------------------------------------------------------------
/*
  This file is part of CanFestival, a library implementing CanOpen
  Stack.

  Copyright (C): Edouard TISSERANT and Francis DUPIN

  See COPYING file for copyrights details.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
  USA
*/
#include "pdo.h"
#include "objacces.h"
#include "canfestival.h"
#include "sysdep.h"

/*!
** @file pdo.c
** @author Edouard TISSERANT and Francis DUPIN
** @date Tue Jun  5 09:32:32 2007
**
** @brief
**
**
*/

2万 · 2018-8-23 13:49

/*!
**
**
** @param d
** @param TPDO_com TPDO communication parameters OD entry
** @param TPDO_map TPDO mapping parameters OD entry
**
** @return
**/

UNS8 buildPDO (CO_Data * d, UNS8 numPdo, Message * pdo)
{
  const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo;
  const indextable *TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;

  UNS8 prp_j = 0x00;
  UNS32 offset = 0x00000000;
  const UNS8 *pMappingCount = (UNS8 *) TPDO_map->pSubindex[0].pObject;

  pdo->cob_id = (UNS16) UNS16_LE(*(UNS32*)TPDO_com->pSubindex[1].pObject & 0x7FF);
  pdo->rtr = NOT_A_REQUEST;

  MSG_WAR (0x3009, "  PDO CobId is : ",
         *(UNS32 *) TPDO_com->pSubindex[1].pObject);
  MSG_WAR (0x300D, "  Number of objects mapped : ", *pMappingCount);

  do
{
   UNS8 dataType;          /* Unused */
   UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 };  /* temporary space to hold bits */

   /* pointer fo the var which holds the mapping parameter of an mapping entry  */
   UNS32 *pMappingParameter =
      (UNS32 *) TPDO_map->pSubindex[prp_j + 1].pObject;
   UNS16 index = (UNS16) ((*pMappingParameter) >> 16);
   UNS32 Size = (UNS32) (*pMappingParameter & (UNS32) 0x000000FF);    /* Size in bits */

   /* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */
   if (Size && ((offset + Size) <= 64))
      {
      UNS32 ByteSize = 1 + ((Size - 1) >> 3);       /*1->8 => 1 ; 9->16 => 2, ... */
      UNS8 subIndex =
         (UNS8) (((*pMappingParameter) >> (UNS8) 8) & (UNS32) 0x000000FF);

      MSG_WAR (0x300F, "  got mapping parameter : ", *pMappingParameter);
      MSG_WAR (0x3050, " at index : ", TPDO_map->index);
      MSG_WAR (0x3051, " sub-index : ", prp_j + 1);

      if (getODentry (d, index, subIndex, tmp, &ByteSize, &dataType, 0) !=
            OD_SUCCESSFUL)
         {
            MSG_ERR (0x1013,
                     " Couldn't find mapped variable at index-subindex-size : ",
                     (UNS32) (*pMappingParameter));
            return 0xFF;
         }
      /* copy bit per bit in little endian */
      CopyBits ((UNS8) Size, ((UNS8 *) tmp), 0, 0,
                  (UNS8 *) & pdo->data[offset >> 3], (UNS8)(offset % 8), 0);

      offset += Size;
      }
   prp_j++;
}
  while (prp_j < *pMappingCount);

  pdo->len = (UNS8)(1 + ((offset - 1) >> 3));

  MSG_WAR (0x3015, "  End scan mapped variable", 0);

  return 0;
}

2万 · 2018-8-23 13:50

/*!
**
**
** @param d
** @param cobId
**
** @return
**/
UNS8
sendPDOrequest (CO_Data * d, UNS16 RPDOIndex)
{
  UNS16 *pwCobId;
  UNS16 offset = d->firstIndex->PDO_RCV;
  UNS16 lastIndex = d->lastIndex->PDO_RCV;

  if (!d->CurrentCommunicationState.csPDO)
{
   return 0;
}

  /* Sending the request only if the cobid have been found on the PDO
   receive */
  /* part dictionary */

  MSG_WAR (0x3930, "sendPDOrequest RPDO Index : ", RPDOIndex);

  if (offset && RPDOIndex >= 0x1400)
{
   offset += RPDOIndex - 0x1400;
   if (offset <= lastIndex)
      {
      /* get the CobId */
      pwCobId = d->objdict[offset].pSubindex[1].pObject;

      MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId);
      {
         Message pdo;
         pdo.cob_id = UNS16_LE(*pwCobId);
         pdo.rtr = REQUEST;
         pdo.len = 0;
         return canSend (d->canHandle, &pdo);
      }
      }
}
  MSG_ERR (0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex);
  return 0xFF;
}

2万 · 2018-8-23 13:50

/*!
**
**
** @param d
** @param m
**
** @return
**/
UNS8
proceedPDO (CO_Data * d, Message * m)
{
  UNS8 numPdo;
  UNS8 numMap;                /* Number of the mapped varable */
  UNS8 *pMappingCount = NULL; /* count of mapped objects... */
  /* pointer to the var which is mapped to a pdo... */
  /*  void *    pMappedAppObject = NULL; */
  /* pointer fo the var which holds the mapping parameter of an
   mapping entry */
  UNS32 *pMappingParameter = NULL;
  UNS8 *pTransmissionType = NULL;    /* pointer to the transmission
                                       type */
  UNS16 *pwCobId = NULL;
  UNS8 Size;
  UNS8 offset;
  UNS8 status;
  UNS32 objDict;
  UNS16 offsetObjdict;
  UNS16 lastIndex;

  status = state2;

  MSG_WAR (0x3935, "proceedPDO, cobID : ", (UNS16_LE(m->cob_id) & 0x7ff));
  offset = 0x00;
  numPdo = 0;
  numMap = 0;
  if ((*m).rtr == NOT_A_REQUEST)
{
   offsetObjdict = d->firstIndex->PDO_RCV;
   lastIndex = d->lastIndex->PDO_RCV;

   if (offsetObjdict)
      while (offsetObjdict <= lastIndex)
      {
         switch (status)
            {

            case state2:
            pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;
            if (*pwCobId == UNS16_LE(m->cob_id))
               {
                  /* The cobId is recognized */
                  status = state4;
                  MSG_WAR (0x3936, "cobId found at index ",
                           0x1400 + numPdo);
                  break;
               }
            else
               {
                  /* received cobId does not match */
                  numPdo++;
                  offsetObjdict++;
                  status = state2;
                  break;
               }

            case state4:    /* Get Mapped Objects Number */
            /* The cobId of the message received has been found in the
               dictionnary. */
            offsetObjdict = d->firstIndex->PDO_RCV_MAP;
            lastIndex = d->lastIndex->PDO_RCV_MAP;
            pMappingCount =
               (UNS8 *) (d->objdict + offsetObjdict +
                        numPdo)->pSubindex[0].pObject;
            numMap = 0;
            while (numMap < *pMappingCount)
               {
                  UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
                  UNS32 ByteSize;
                  pMappingParameter =
                  (UNS32 *) (d->objdict + offsetObjdict +
                              numPdo)->pSubindex[numMap + 1].pObject;
                  if (pMappingParameter == NULL)
                  {
                     MSG_ERR (0x1937, "Couldn't get mapping parameter : ",
                              numMap + 1);
                     return 0xFF;
                  }
                  /* Get the addresse of the mapped variable. */
                  /* detail of *pMappingParameter : */
                  /* The 16 hight bits contains the index, the medium 8 bits
                     contains the subindex, */
                  /* and the lower 8 bits contains the size of the mapped
                     variable. */

                  Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF);

                  /* set variable only if Size != 0 and
                  * Size is lower than remaining bits in the PDO */
                  if (Size && ((offset + Size) <= (m->len << 3)))
                  {
                     /* copy bit per bit in little endian */
                     CopyBits (Size, (UNS8 *) & m->data[offset >> 3],
                              offset % 8, 0, ((UNS8 *) tmp), 0, 0);
                     /*1->8 => 1 ; 9->16 =>2, ... */
                     ByteSize = (UNS32)(1 + ((Size - 1) >> 3));

                     objDict =
                        setODentry (d, (UNS16) ((*pMappingParameter) >> 16),
                                    (UNS8) (((*pMappingParameter) >> 8) &
                                          0xFF), tmp, &ByteSize, 0);

                     if (objDict != OD_SUCCESSFUL)
                        {
                        MSG_ERR (0x1938,
                                 "error accessing to the mapped var : ",
                                 numMap + 1);
                        MSG_WAR (0x2939, "       Mapped at index : ",
                                 (*pMappingParameter) >> 16);
                        MSG_WAR (0x2940, "             subindex : ",
                                 ((*pMappingParameter) >> 8) & 0xFF);
                        return 0xFF;
                        }

                     MSG_WAR (0x3942,
                              "Variable updated by PDO cobid : ",
                              UNS16_LE(m->cob_id));
                     MSG_WAR (0x3943, "       Mapped at index : ",
                              (*pMappingParameter) >> 16);
                     MSG_WAR (0x3944, "             subindex : ",
                              ((*pMappingParameter) >> 8) & 0xFF);
                     offset += Size;
                  }
                  numMap++;
               }          /* end loop while on mapped variables */
            if (d->RxPDO_EventTimers)
            {
                  TIMEVAL EventTimerDuration = *(UNS16 *)d->objdict[offsetObjdict].pSubindex[5].pObject;
                  if(EventTimerDuration){
                     DelAlarm (d->RxPDO_EventTimers[numPdo]);
                     d->RxPDO_EventTimers[numPdo] = SetAlarm (d, numPdo, d->RxPDO_EventTimers_Handler,
                     MS_TO_TIMEVAL (EventTimerDuration), 0);
                  }
            }
            return 0;

            }                /* end switch status */
      }                   /* end while */
}                         /* end if Donnees */
  else if ((*m).rtr == REQUEST)
{
   MSG_WAR (0x3946, "Receive a PDO request cobId : ", UNS16_LE(m->cob_id));
   status = state1;
   offsetObjdict = d->firstIndex->PDO_TRS;
   lastIndex = d->lastIndex->PDO_TRS;
   if (offsetObjdict)
      while (offsetObjdict <= lastIndex)
      {
         /* study of all PDO stored in the objects dictionary */

         switch (status)
            {

            case state1:    /* check the CobId */
            /* get CobId of the dictionary which match to the received PDO
               */
            pwCobId =
               (d->objdict +
                           offsetObjdict)->pSubindex[1].pObject;
            if (*pwCobId == UNS16_LE(m->cob_id))
               {
                  status = state4;
                  break;
               }
            else
               {
                  numPdo++;
                  offsetObjdict++;
               }
            status = state1;
            break;

            case state4:    /* check transmission type */
            pTransmissionType =
               (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
            /* If PDO is to be sampled and send on RTR, do it */
            if ((*pTransmissionType == TRANS_RTR))
               {
                  status = state5;
                  break;
               }
            /* RTR_SYNC means data prepared at SYNC, transmitted on RTR */
            else if ((*pTransmissionType == TRANS_RTR_SYNC))
               {
                  if (d->PDO_status[numPdo].
                     transmit_type_parameter & PDO_RTR_SYNC_READY)
                  {
                     /*Data ready, just send */
                     canSend (d->canHandle,
                              &d->PDO_status[numPdo].last_message);
                     return 0;
                  }
                  else
                  {
                     /* if SYNC did never occur, transmit current data */
                     /* DS301 do not tell what to do in such a case... */
                     MSG_ERR (0x1947,
                              "Not ready RTR_SYNC TPDO send current data : ",
                              UNS16_LE(m->cob_id));
                     status = state5;
                  }
                  break;
               }
            else if ((*pTransmissionType == TRANS_EVENT_PROFILE) ||
                     (*pTransmissionType == TRANS_EVENT_SPECIFIC))
               {
                  /* Zap all timers and inhibit flag */
                  d->PDO_status[numPdo].event_timer =
                  DelAlarm (d->PDO_status[numPdo].event_timer);
                  d->PDO_status[numPdo].inhibit_timer =
                  DelAlarm (d->PDO_status[numPdo].inhibit_timer);
                  d->PDO_status[numPdo].transmit_type_parameter &=
                  ~PDO_INHIBITED;
                  /* Call  PDOEventTimerAlarm for this TPDO,
                  * this will trigger emission et reset timers */
                  PDOEventTimerAlarm (d, numPdo);
                  return 0;
               }
            else
               {
                  /* The requested PDO is not to send on request. So, does
                     nothing. */
                  MSG_WAR (0x2947, "PDO is not to send on request : ",
                           UNS16_LE(m->cob_id));
                  return 0xFF;
               }

            case state5:    /* build and send requested PDO */
            {
               Message pdo;
               if (buildPDO (d, numPdo, &pdo))
                  {
                  MSG_ERR (0x1948, " Couldn't build TPDO number : ", numPdo);
                  return 0xFF;
                  }
               canSend (d->canHandle, &pdo);
               return 0;
            }
            }                /* end switch status */
      }                   /* end while */
}                         /* end if Requete */

  return 0;
}

2万 · 2018-8-23 13:50

/*!
**
**
** @param NbBits
** @param SrcByteIndex
** @param SrcBitIndex
** @param SrcBigEndian
** @param DestByteIndex
** @param DestBitIndex
** @param DestBigEndian
**/
void
CopyBits (UNS8 NbBits, UNS8 * SrcByteIndex, UNS8 SrcBitIndex,
      UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex,
      UNS8 DestBigEndian)
{
  /* This loop copy as many bits that it can each time, crossing */
  /* successively bytes */
  // boundaries from LSB to MSB.
  while (NbBits > 0)
{
   /* Bit missalignement between src and dest */
   INTEGER8 Vect = DestBitIndex - SrcBitIndex;

   /* We can now get src and align it to dest */
   UNS8 Aligned =
      Vect > 0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;

   /* Compute the nb of bit we will be able to copy */
   UNS8 BoudaryLimit = (Vect > 0 ? 8 - DestBitIndex : 8 - SrcBitIndex);
   UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

   /* Create a mask that will serve in: */
   UNS8 Mask =
      ((0xff << (DestBitIndex + BitsToCopy)) |
      (0xff >> (8 - DestBitIndex)));

   /* - Filtering src */
   UNS8 Filtered = Aligned & ~Mask;

   /* - and erase bits where we write, preserve where we don't */
   *DestByteIndex &= Mask;

   /* Then write. */
   *DestByteIndex |= Filtered;

   /*Compute next time cursors for src */
   if ((SrcBitIndex += BitsToCopy) > 7)    /* cross boundary ? */
      {
      SrcBitIndex = 0;    /* First bit */
      SrcByteIndex += (SrcBigEndian ? -1 : 1);    /* Next byte */
      }

   /*Compute next time cursors for dest */
   if ((DestBitIndex += BitsToCopy) > 7)
      {
      DestBitIndex = 0;    /* First bit */
      DestByteIndex += (DestBigEndian ? -1 : 1); /* Next byte */
      }

   /*And decrement counter. */
   NbBits -= BitsToCopy;
}

}

void sendPdo(CO_Data * d, UNS32 pdoNum, Message * pdo)
{
  /*store_as_last_message */
  d->PDO_status[pdoNum].last_message = *pdo;
  MSG_WAR (0x396D, "sendPDO cobId :", UNS16_LE(pdo->cob_id));
  MSG_WAR (0x396E, "    Nb octets  : ", pdo->len);

  canSend (d->canHandle, pdo);
}

2万 · 2018-8-23 13:50

/*!
**
**
** @param d
**
** @return
**/

UNS8
sendPDOevent (CO_Data * d)
{
  /* Calls _sendPDOevent specifying it is not a sync event */
  return _sendPDOevent (d, 0);
}

UNS8
sendOnePDOevent (CO_Data * d, UNS8 pdoNum)
{
  UNS16 offsetObjdict;
  Message pdo;
  if (!d->CurrentCommunicationState.csPDO ||
   (d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED))
{
   return 0;
}

  offsetObjdict = (UNS16) (d->firstIndex->PDO_TRS + pdoNum);

  MSG_WAR (0x3968, "  PDO is on EVENT. Trans type : ",
         *((UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject));

  memset(&pdo, 0, sizeof(pdo));
  if (buildPDO (d, pdoNum, &pdo))
{
   MSG_ERR (0x3907, " Couldn't build TPDO number : ",
            pdoNum);
   return 0;
}

  /*Compare new and old PDO */
  if (d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id
   && d->PDO_status[pdoNum].last_message.len == pdo.len
   && memcmp(d->PDO_status[pdoNum].last_message.data,
                                    pdo.data, 8) == 0
)
{
   /* No changes -> go to next pdo */
   return 0;
}
  else
{

   TIMEVAL EventTimerDuration;
   TIMEVAL InhibitTimerDuration;

   MSG_WAR (0x306A, "Changes TPDO number : ", pdoNum);
   /* Changes detected -> transmit message */
   EventTimerDuration =
      *(UNS16 *) d->objdict[offsetObjdict].pSubindex[5].
      pObject;
   InhibitTimerDuration =
      *(UNS16 *) d->objdict[offsetObjdict].pSubindex[3].
      pObject;

   /* Start both event_timer and inhibit_timer */
   if (EventTimerDuration)
      {
      DelAlarm (d->PDO_status[pdoNum].event_timer);
      d->PDO_status[pdoNum].event_timer =
         SetAlarm (d, pdoNum, &PDOEventTimerAlarm,
                  MS_TO_TIMEVAL (EventTimerDuration), 0);
      }

   if (InhibitTimerDuration)
      {
      DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
      d->PDO_status[pdoNum].inhibit_timer =
         SetAlarm (d, pdoNum, &PDOInhibitTimerAlarm,
                  US_TO_TIMEVAL (InhibitTimerDuration *
                                 100), 0);
      /* and inhibit TPDO */
      d->PDO_status[pdoNum].transmit_type_parameter |=
         PDO_INHIBITED;
      }

   sendPdo(d, pdoNum, &pdo);
}
return 1;
}

void
PDOEventTimerAlarm (CO_Data * d, UNS32 pdoNum)
{
  /* This is needed to avoid deletion of re-attribuated timer */
  d->PDO_status[pdoNum].event_timer = TIMER_NONE;
  /* force emission of PDO by artificially changing last emitted */
  d->PDO_status[pdoNum].last_message.cob_id = 0;
  sendOnePDOevent (d, (UNS8) pdoNum);
}

void
PDOInhibitTimerAlarm (CO_Data * d, UNS32 pdoNum)
{
  /* This is needed to avoid deletion of re-attribuated timer */
  d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;
  /* Remove inhibit flag */
  d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;
  sendOnePDOevent (d, (UNS8) pdoNum);
}

void
_RxPDO_EventTimers_Handler(CO_Data *d, UNS32 pdoNum)
{
}

2万 · 2018-8-23 13:51

/*!
**
**
** @param d
** @param isSyncEvent
**
** @return
**/

UNS8 _sendPDOevent (CO_Data * d, UNS8 isSyncEvent)
{
  UNS8 pdoNum = 0x00;          /* number of the actual processed pdo-nr. */
  UNS8 *pTransmissionType = NULL;
  UNS8 status = state3;
  UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
  UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
  UNS16 lastIndex = d->lastIndex->PDO_TRS;

  if (!d->CurrentCommunicationState.csPDO)
{
   return 0;
}
/* study all PDO stored in the objects dictionary */
  if (offsetObjdict)
{
   Message pdo;/* = Message_Initializer;*/
   memset(&pdo, 0, sizeof(pdo));
   while (offsetObjdict <= lastIndex)
      {
      switch (status)
         {
         case state3:
            if ( /* bSubCount always 5 with objdictedit -> check disabled */
               /*d->objdict[offsetObjdict].bSubCount < 5 ||*/
               /* check if TPDO is not valid */
               *(UNS32 *) d->objdict[offsetObjdict].pSubindex[1].
               pObject & 0x80000000)
            {
               MSG_WAR (0x3960, "Not a valid PDO ", 0x1800 + pdoNum);
               /*Go next TPDO */
               status = state11;
               break;
            }
            /* get the PDO transmission type */
            pTransmissionType =
            (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
            MSG_WAR (0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);

            /* check if transmission type is SYNCRONOUS */
            /* message transmited every n SYNC with n=TransmissionType */
            if (isSyncEvent &&
               (*pTransmissionType >= TRANS_SYNC_MIN) &&
               (*pTransmissionType <= TRANS_SYNC_MAX) &&
               (++d->PDO_status[pdoNum].transmit_type_parameter ==
               *pTransmissionType))
            {
               /*Reset count of SYNC */
               d->PDO_status[pdoNum].transmit_type_parameter = 0;
               MSG_WAR (0x3964, "  PDO is on SYNCHRO. Trans type : ",
                        *pTransmissionType);
               memset(&pdo, 0, sizeof(pdo));
               /*{
                  Message msg_init = Message_Initializer;
                  pdo = msg_init;
               }*/
               if (buildPDO (d, pdoNum, &pdo))
                  {
                  MSG_ERR (0x1906, " Couldn't build TPDO number : ",
                           pdoNum);
                  status = state11;
                  break;
                  }
               status = state5;
               /* If transmission RTR, with data sampled on SYNC */
            }
            else if (isSyncEvent && (*pTransmissionType == TRANS_RTR_SYNC))
            {
               if (buildPDO
                  (d, pdoNum, &d->PDO_status[pdoNum].last_message))
                  {
                  MSG_ERR (0x1966, " Couldn't build TPDO number : ",
                           pdoNum);
                  d->PDO_status[pdoNum].transmit_type_parameter &=
                     ~PDO_RTR_SYNC_READY;
                  }
               else
                  {
                  d->PDO_status[pdoNum].transmit_type_parameter |=
                     PDO_RTR_SYNC_READY;
                  }
               status = state11;
               break;
               /* If transmission on Event and not inhibited, check for changes */
            }
            else
            if ( (isSyncEvent && (*pTransmissionType == TRANS_SYNC_ACYCLIC))
                  ||
                  (!isSyncEvent && (*pTransmissionType == TRANS_EVENT_PROFILE || *pTransmissionType == TRANS_EVENT_SPECIFIC)
                     && !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)))
            {
               sendOnePDOevent(d, pdoNum);
               status = state11;
            }
            else
            {
               MSG_WAR (0x306C,
                        "  PDO is not on EVENT or synchro or not at this SYNC. Trans type : ",
                        *pTransmissionType);
               status = state11;
            }
            break;
         case state5:    /*Send the pdo */
            sendPdo(d, pdoNum, &pdo);
            status = state11;
            break;
         case state11:    /*Go to next TPDO */
            pdoNum++;
            offsetObjdict++;
            offsetObjdictMap++;
            MSG_WAR (0x3970, "next pdo index : ", pdoNum);
            status = state3;
            break;

         default:
            MSG_ERR (0x1972, "Unknown state has been reached :", status);
            return 0xFF;
         }                /* end switch case */

      }                      /* end while */
}
  return 0;
}

2万 · 2018-8-23 13:51

/*!
**
**
** @param d
** @param OD_entry
** @param bSubindex
** @return always 0
**/

UNS32 TPDO_Communication_Parameter_Callback (CO_Data * d,
                                    const indextable * OD_entry,
                                    UNS8 bSubindex)
{
  /* If PDO are actives */
  if (d->CurrentCommunicationState.csPDO)
switch (bSubindex)
   {
   case 2:                /* Changed transmition type */
   case 3:                /* Changed inhibit time */
   case 5:                /* Changed event time */
      {
      const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS;
      UNS8 numPdo = (UNS8) (OD_entry - TPDO_com); /* number of the actual processed pdo-nr. */

      /* Zap all timers and inhibit flag */
      d->PDO_status[numPdo].event_timer =
         DelAlarm (d->PDO_status[numPdo].event_timer);
      d->PDO_status[numPdo].inhibit_timer =
         DelAlarm (d->PDO_status[numPdo].inhibit_timer);
      d->PDO_status[numPdo].transmit_type_parameter = 0;
      /* Call  PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
      PDOEventTimerAlarm (d, numPdo);
      return 0;
      }

   default:                /* other subindex are ignored */
      break;
   }
  return 0;
}

2万 · 2018-8-23 13:52

void PDOInit (CO_Data * d)
{
  /* For each TPDO mapping parameters */
  UNS16 pdoIndex = 0x1800;    /* OD index of TDPO */

  UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
  UNS16 lastIndex = d->lastIndex->PDO_TRS;
  if (offsetObjdict)
while (offsetObjdict <= lastIndex)
   {
      /* Assign callbacks to sensible TPDO mapping subindexes */
      UNS32 errorCode;
      ODCallback_t *CallbackList;
      /* Find callback list */
      scanIndexOD (d, pdoIndex, &errorCode, &CallbackList);
      if (errorCode == OD_SUCCESSFUL && CallbackList)
      {
         /*Assign callbacks to corresponding subindex */
         /* Transmission type */
         CallbackList[2] = &TPDO_Communication_Parameter_Callback;
         /* Inhibit time */
         CallbackList[3] = &TPDO_Communication_Parameter_Callback;
         /* Event timer */
         CallbackList[5] = &TPDO_Communication_Parameter_Callback;
      }
      pdoIndex++;
      offsetObjdict++;
   }

  /* Trigger a non-sync event */
  _sendPDOevent (d, 0);
}

void
PDOStop (CO_Data * d)
{
  /* For each TPDO mapping parameters */
  UNS8 pdoNum = 0x00;          /* number of the actual processed pdo-nr. */
  UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
  UNS16 lastIndex = d->lastIndex->PDO_TRS;
  if (offsetObjdict)
while (offsetObjdict <= lastIndex)
   {
      /* Delete TPDO timers */
      d->PDO_status[pdoNum].event_timer =
      DelAlarm (d->PDO_status[pdoNum].event_timer);
      d->PDO_status[pdoNum].inhibit_timer =
      DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
      /* Reset transmit type parameter */
      d->PDO_status[pdoNum].transmit_type_parameter = 0;
      d->PDO_status[pdoNum].last_message.cob_id = 0;
      pdoNum++;
      offsetObjdict++;
   }
}

Copyright (C): Edouard TISSERANT and Francis DUPIN

See COPYING file for copyrights details.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

2万 · 2018-8-23 13:53

/*!
** @file sdo.c
** @author Edouard TISSERANT and Francis DUPIN
** @date Tue Jun  5 09:32:32 2007
**
** @brief
**
**
*/

/* #define DEBUG_WAR_CONSOLE_ON */
/* #define DEBUG_ERR_CONSOLE_ON */

#include <stdlib.h>

#include "canfestival.h"
#include "sysdep.h"

/* Uncomment if your compiler does not support inline functions */
#define NO_INLINE

#ifdef NO_INLINE
#define INLINE
#else
#define INLINE inline
#endif

/*Internals prototypes*/

UNS8 GetSDOClientFromNodeId( CO_Data* d, UNS8 nodeId );

/*!
** Called by writeNetworkDict
**
** @param d
** @param nodeId
** @param index
** @param subIndex
** @param count
** @param dataType
** @param data
** @param Callback
** @param endianize
**
** @return
**/
INLINE UNS8 _writeNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index,
            UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode);

/*!
** Called by readNetworkDict
**
** @param d
** @param nodeId
** @param index
** @param subIndex
** @param dataType
** @param Callback
**
** @return
**/
INLINE UNS8 _readNetworkDict (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex,
            UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode);

/***************************************************************************/
/* SDO (un)packing macros */

/** Returns the command specifier (cs, ccs, scs) from the first byte of the SDO
*/
#define getSDOcs(byte) (byte >> 5)

/** Returns the number of bytes without data from the first byte of the SDO. Coded in 2 bits
*/
#define getSDOn2(byte) ((byte >> 2) & 3)

/** Returns the number of bytes without data from the first byte of the SDO. Coded in 3 bits
*/
#define getSDOn3(byte) ((byte >> 1) & 7)

/** Returns the transfer type from the first byte of the SDO
*/
#define getSDOe(byte) ((byte >> 1) & 1)

/** Returns the size indicator from the first byte of the SDO
*/
#define getSDOs(byte) (byte & 1)

/** Returns the indicator of end transmission from the first byte of the SDO
*/
#define getSDOc(byte) (byte & 1)

/** Returns the toggle from the first byte of the SDO
*/
#define getSDOt(byte) ((byte >> 4) & 1)

/** Returns the index from the bytes 1 and 2 of the SDO
*/
#define getSDOindex(byte1, byte2) (((UNS16)byte2 << 8) | ((UNS16)byte1))

/** Returns the subIndex from the byte 3 of the SDO
*/
#define getSDOsubIndex(byte3) (byte3)

/** Returns the subcommand in SDO block transfer
*/
#define getSDOblockSC(byte) (byte & 3)

2万 · 2018-8-23 13:53

/*!
**
**
** @param d
** @param id
**/
void SDOTimeoutAlarm(CO_Data* d, UNS32 id)
{
      UNS16 offset;
      UNS8 nodeId;
      /* Get the client->server cobid.*/
      offset = d->firstIndex->SDO_CLT;
      if ((offset == 0) || ((offset+d->transfers[id].CliServNbr) > d->lastIndex->SDO_CLT)) {
            return ;
      }
      nodeId = (UNS8) *((UNS32*) d->objdict[offset+d->transfers[id].CliServNbr].pSubindex[3].pObject);
      MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);
      MSG_WAR(0x2A02, "server node id : ", nodeId);
      MSG_WAR(0x2A02, "       index : ", d->transfers[id].index);
      MSG_WAR(0x2A02, "    subIndex : ", d->transfers[id].subIndex);
      /* Reset timer handler */
      d->transfers[id].timer = TIMER_NONE;
      /*Set aborted state*/
      d->transfers[id].state = SDO_ABORTED_INTERNAL;
      /* Sending a SDO abort */
      sendSDOabort(d, d->transfers[id].whoami, d->transfers[id].CliServNbr,
                     d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);
      d->transfers[id].abortCode = SDOABT_TIMED_OUT;
      /* Call the user function to inform of the problem.*/
      if(d->transfers[id].Callback)
            /*If ther is a callback, it is responsible to close SDO transfer (client)*/
            (*d->transfers[id].Callback)(d, nodeId);
      /*Reset the line if (whoami == SDO_SERVER) or the callback did not close the line.
      Otherwise this sdo transfer would never be closed. */
      if(d->transfers[id].abortCode == SDOABT_TIMED_OUT)
            resetSDOline(d, (UNS8)id);
}

#define StopSDO_TIMER(id) \
      MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\
d->transfers[id].timer = DelAlarm(d->transfers[id].timer);

#define StartSDO_TIMER(id) \
      MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\
d->transfers[id].timer = SetAlarm(d,id,&SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0);

#define RestartSDO_TIMER(id) \
      MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\
if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) }

/*!
** Reset all sdo buffers
**
** @param d
**/
void resetSDO (CO_Data* d)
{
      UNS8 j;

      /* transfer structure initialization */
      for (j = 0 ; j < SDO_MAX_SIMULTANEOUS_TRANSFERS ; j++)
            resetSDOline(d, j);
}

/*!
**
**
** @param d
** @param line
**
** @return
**/
UNS32 SDOlineToObjdict (CO_Data* d, UNS8 line)
{
      UNS32 size;
      UNS32 errorCode;
      MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line);
      /* if SDO initiated with e=0 and s=0 count is null, offset carry effective size*/
      if( d->transfers[line].count == 0)
            d->transfers[line].count = d->transfers[line].offset;
      size = d->transfers[line].count;

#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION
      if (size > SDO_MAX_LENGTH_TRANSFER)
      {
            errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
                              (void *) d->transfers[line].dynamicData, &size, 1);
      }
      else
      {
            errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
                              (void *) d->transfers[line].data, &size, 1);
      }
#else //SDO_DYNAMIC_BUFFER_ALLOCATION
      errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,
                     (void *) d->transfers[line].data, &size, 1);
#endif //SDO_DYNAMIC_BUFFER_ALLOCATION

      if (errorCode != OD_SUCCESSFUL)
            return errorCode;
      MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);
      return 0;

}