Sam Grove
CANfestival - an open source CANopen framework
Diff: framework/src/pdo.c
- Revision:
- 0:6219434a0cb5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/framework/src/pdo.c Mon May 30 07:14:41 2011 +0000
@@ -0,0 +1,841 @@
+/*
+ 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
+**
+**
+*/
+
+/*!
+**
+**
+** @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 : ",
+ (UNS16) (*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;
+}
+
+/*!
+**
+**
+** @param d
+** @param cobId
+**
+** @return
+**/
+UNS8
+sendPDOrequest (CO_Data * d, UNS16 RPDOIndex)
+{
+ UNS32 *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 */
+ // mbed error 513 - needs typecast
+ pwCobId = (unsigned int*)d->objdict[offset].pSubindex[1].pObject;
+
+ MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId);
+ {
+ Message pdo;
+ pdo.cob_id = (UNS16)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;
+}
+
+
+/*!
+**
+**
+** @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 */
+ UNS32 *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)
+ { /* The PDO received is not a
+ request. */
+
+ offsetObjdict = d->firstIndex->PDO_RCV;
+ lastIndex = d->lastIndex->PDO_RCV;
+
+ /* study of all the PDO stored in the dictionary */
+ if (offsetObjdict)
+ while (offsetObjdict <= lastIndex)
+ {
+
+ switch (status)
+ {
+
+ case state2:
+ /* get CobId of the dictionary correspondant to the received
+ PDO */
+ // mbed error 513 - needs typecast
+ pwCobId = (unsigned int*)
+ d->objdict[offsetObjdict].pSubindex[1].pObject;
+ /* check the CobId coherance */
+ /*pwCobId is the cobId read in the dictionary at the state 3
+ */
+ if (*pwCobId == UNS16_LE(m->cob_id))
+ {
+ /* The cobId is recognized */
+ status = state4;
+ MSG_WAR (0x3936, "cobId found at index ",
+ 0x1400 + numPdo);
+ break;
+ }
+ else
+ {
+ /* cobId received does not match with those write in the
+ dictionnary */
+ 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 */
+
+ 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
+ */
+ // mbed error 513 - needs typecast
+ pwCobId = (unsigned int*)
+ (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 n�", numPdo);
+ return 0xFF;
+ }
+ canSend (d->canHandle, &pdo);
+ return 0;
+ }
+ } /* end switch status */
+ } /* end while */
+ } /* end if Requete */
+
+ return 0;
+}
+
+/*!
+**
+**
+** @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;
+ }
+
+}
+
+static 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);
+}
+
+
+/*!
+**
+**
+** @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 : ",
+ *pTransmissionType);
+
+ 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);
+}
+
+/*!
+**
+**
+** @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 : %d", status);
+ return 0xFF;
+ } /* end switch case */
+
+ } /* end while */
+ }
+ return 0;
+}
+
+/*!
+**
+**
+** @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;
+}
+
+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++;
+ }
+}