etisserant@0: /*
nico@208: This file is part of CanFestival, a library implementing CanOpen
nico@208: Stack.
nico@208:
nico@208: Copyright (C): Edouard TISSERANT and Francis DUPIN
nico@208:
nico@208: See COPYING file for copyrights details.
nico@208:
nico@208: This library is free software; you can redistribute it and/or
nico@208: modify it under the terms of the GNU Lesser General Public
nico@208: License as published by the Free Software Foundation; either
nico@208: version 2.1 of the License, or (at your option) any later version.
nico@208:
nico@208: This library is distributed in the hope that it will be useful,
nico@208: but WITHOUT ANY WARRANTY; without even the implied warranty of
nico@208: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
nico@208: Lesser General Public License for more details.
nico@208:
nico@208: You should have received a copy of the GNU Lesser General Public
nico@208: License along with this library; if not, write to the Free Software
nico@208: Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
nico@208: USA
etisserant@0: */
etisserant@0: #include "pdo.h"
etisserant@0: #include "objacces.h"
etisserant@149: #include "canfestival.h"
etisserant@370: #include "sysdep.h"
etisserant@0:
nico@208: /*!
nico@208: ** @file pdo.c
nico@208: ** @author Edouard TISSERANT and Francis DUPIN
nico@208: ** @date Tue Jun 5 09:32:32 2007
nico@208: **
nico@208: ** @brief
nico@208: **
nico@208: **
nico@208: */
etisserant@235:
etisserant@235: /*!
etisserant@235: **
nico@208: **
nico@208: ** @param d
etisserant@235: ** @param TPDO_com TPDO communication parameters OD entry
etisserant@235: ** @param TPDO_map TPDO mapping parameters OD entry
nico@208: **
nico@208: ** @return
nico@208: **/
etisserant@235:
peter@524: UNS8 buildPDO (CO_Data * d, UNS8 numPdo, Message * pdo)
etisserant@425: {
etisserant@425: const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo;
peter@524: const indextable *TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;
etisserant@425:
etisserant@425: UNS8 prp_j = 0x00;
etisserant@425: UNS8 offset = 0x00;
etisserant@425: const UNS8 *pMappingCount = (UNS8 *) TPDO_map->pSubindex[0].pObject;
etisserant@425:
etisserant@428: pdo->cob_id = UNS16_LE(*(UNS32*)TPDO_com->pSubindex[1].pObject & 0x7FF);
etisserant@425: pdo->rtr = NOT_A_REQUEST;
etisserant@425:
etisserant@425: MSG_WAR (0x3009, " PDO CobId is : ",
etisserant@425: *(UNS32 *) TPDO_com->pSubindex[1].pObject);
etisserant@425: MSG_WAR (0x300D, " Number of objects mapped : ", *pMappingCount);
etisserant@425:
etisserant@425: do
etisserant@425: {
etisserant@425: UNS8 dataType; /* Unused */
etisserant@425: UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 }; /* temporary space to hold bits */
etisserant@425:
etisserant@425: /* pointer fo the var which holds the mapping parameter of an mapping entry */
etisserant@425: UNS32 *pMappingParameter =
etisserant@425: (UNS32 *) TPDO_map->pSubindex[prp_j + 1].pObject;
etisserant@425: UNS16 index = (UNS16) ((*pMappingParameter) >> 16);
etisserant@539: UNS32 Size = (UNS32) (*pMappingParameter & (UNS32) 0x000000FF); /* Size in bits */
etisserant@425:
etisserant@425: /* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */
etisserant@425: if (Size && ((offset + Size) <= 64))
etisserant@425: {
etisserant@539: UNS32 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
etisserant@425: UNS8 subIndex =
etisserant@425: (UNS8) (((*pMappingParameter) >> (UNS8) 8) & (UNS32) 0x000000FF);
etisserant@425:
etisserant@425: MSG_WAR (0x300F, " got mapping parameter : ", *pMappingParameter);
etisserant@425: MSG_WAR (0x3050, " at index : ", TPDO_map->index);
etisserant@425: MSG_WAR (0x3051, " sub-index : ", prp_j + 1);
etisserant@425:
etisserant@425: if (getODentry (d, index, subIndex, tmp, &ByteSize, &dataType, 0) !=
etisserant@425: OD_SUCCESSFUL)
etisserant@425: {
etisserant@425: MSG_ERR (0x1013,
etisserant@425: " Couldn't find mapped variable at index-subindex-size : ",
etisserant@425: (UNS16) (*pMappingParameter));
etisserant@425: return 0xFF;
etisserant@425: }
etisserant@425: /* copy bit per bit in little endian */
etisserant@425: CopyBits (Size, ((UNS8 *) tmp), 0, 0,
etisserant@425: (UNS8 *) & pdo->data[offset >> 3], offset % 8, 0);
etisserant@425:
etisserant@425: offset += Size;
etisserant@425: }
etisserant@425: prp_j++;
etisserant@425: }
etisserant@425: while (prp_j < *pMappingCount);
etisserant@425:
etisserant@425: pdo->len = 1 + ((offset - 1) >> 3);
etisserant@425:
etisserant@425: MSG_WAR (0x3015, " End scan mapped variable", 0);
etisserant@425:
etisserant@425: return 0;
etisserant@235: }
nico@208:
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: ** @param cobId
nico@208: **
nico@208: ** @return
nico@208: **/
etisserant@425: UNS8
etisserant@425: sendPDOrequest (CO_Data * d, UNS16 RPDOIndex)
etisserant@425: {
etisserant@447: UNS32 *pwCobId;
etisserant@236: UNS16 offset = d->firstIndex->PDO_RCV;
etisserant@236: UNS16 lastIndex = d->lastIndex->PDO_RCV;
etisserant@236:
nico@215: /* Sending the request only if the cobid have been found on the PDO
nico@208: receive */
nico@215: /* part dictionary */
etisserant@425:
etisserant@425: MSG_WAR (0x3930, "sendPDOrequest RPDO Index : ", RPDOIndex);
etisserant@425:
etisserant@425: if (offset && RPDOIndex >= 0x1400)
etisserant@425: {
etisserant@425: offset += RPDOIndex - 0x1400;
etisserant@425: if (offset <= lastIndex)
etisserant@425: {
etisserant@425: /* get the CobId */
etisserant@447: pwCobId = d->objdict[offset].pSubindex[1].pObject;
etisserant@425:
etisserant@425: MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId);
etisserant@425: {
etisserant@425: Message pdo;
etisserant@447: pdo.cob_id = UNS16_LE(*pwCobId);
etisserant@425: pdo.rtr = REQUEST;
etisserant@425: pdo.len = 0;
etisserant@425: return canSend (d->canHandle, &pdo);
etisserant@425: }
etisserant@425: }
nico@208: }
etisserant@425: MSG_ERR (0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex);
etisserant@0: return 0xFF;
etisserant@0: }
etisserant@0:
etisserant@0:
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: ** @param m
nico@208: **
nico@208: ** @return
nico@208: **/
etisserant@425: UNS8
etisserant@425: proceedPDO (CO_Data * d, Message * m)
etisserant@425: {
etisserant@425: UNS8 numPdo;
etisserant@425: UNS8 numMap; /* Number of the mapped varable */
etisserant@425: UNS8 *pMappingCount = NULL; /* count of mapped objects... */
nico@215: /* pointer to the var which is mapped to a pdo... */
nico@215: /* void * pMappedAppObject = NULL; */
nico@215: /* pointer fo the var which holds the mapping parameter of an
nico@208: mapping entry */
etisserant@425: UNS32 *pMappingParameter = NULL;
etisserant@425: UNS8 *pTransmissionType = NULL; /* pointer to the transmission
etisserant@425: type */
etisserant@447: UNS32 *pwCobId = NULL;
etisserant@425: UNS8 Size;
etisserant@425: UNS8 offset;
etisserant@425: UNS8 status;
etisserant@425: UNS32 objDict;
etisserant@425: UNS16 offsetObjdict;
etisserant@425: UNS16 lastIndex;
etisserant@425:
etisserant@235: status = state2;
nico@208:
etisserant@447: MSG_WAR (0x3935, "proceedPDO, cobID : ", (UNS16_LE(m->cob_id) & 0x7ff));
etisserant@0: offset = 0x00;
etisserant@0: numPdo = 0;
etisserant@0: numMap = 0;
etisserant@425: if ((*m).rtr == NOT_A_REQUEST)
etisserant@425: { /* The PDO received is not a
etisserant@425: request. */
etisserant@425:
etisserant@425: offsetObjdict = d->firstIndex->PDO_RCV;
etisserant@425: lastIndex = d->lastIndex->PDO_RCV;
etisserant@425:
etisserant@425: /* study of all the PDO stored in the dictionary */
etisserant@425: if (offsetObjdict)
etisserant@425: while (offsetObjdict <= lastIndex)
etisserant@425: {
etisserant@425:
etisserant@425: switch (status)
etisserant@425: {
etisserant@425:
etisserant@425: case state2:
etisserant@425: /* get CobId of the dictionary correspondant to the received
etisserant@425: PDO */
etisserant@425: pwCobId =
etisserant@447: d->objdict[offsetObjdict].pSubindex[1].pObject;
etisserant@425: /* check the CobId coherance */
etisserant@425: /*pwCobId is the cobId read in the dictionary at the state 3
etisserant@425: */
etisserant@447: if (*pwCobId == UNS16_LE(m->cob_id))
etisserant@425: {
etisserant@425: /* The cobId is recognized */
etisserant@425: status = state4;
etisserant@425: MSG_WAR (0x3936, "cobId found at index ",
etisserant@425: 0x1400 + numPdo);
etisserant@425: break;
etisserant@425: }
etisserant@425: else
etisserant@425: {
etisserant@425: /* cobId received does not match with those write in the
etisserant@425: dictionnary */
etisserant@425: numPdo++;
etisserant@425: offsetObjdict++;
etisserant@425: status = state2;
etisserant@425: break;
etisserant@425: }
etisserant@425:
etisserant@425: case state4: /* Get Mapped Objects Number */
etisserant@425: /* The cobId of the message received has been found in the
etisserant@425: dictionnary. */
etisserant@425: offsetObjdict = d->firstIndex->PDO_RCV_MAP;
etisserant@425: lastIndex = d->lastIndex->PDO_RCV_MAP;
etisserant@425: pMappingCount =
etisserant@425: (UNS8 *) (d->objdict + offsetObjdict +
etisserant@425: numPdo)->pSubindex[0].pObject;
etisserant@425: numMap = 0;
etisserant@425: while (numMap < *pMappingCount)
etisserant@425: {
etisserant@425: UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
etisserant@539: UNS32 ByteSize;
etisserant@425: pMappingParameter =
etisserant@425: (UNS32 *) (d->objdict + offsetObjdict +
etisserant@425: numPdo)->pSubindex[numMap + 1].pObject;
etisserant@425: if (pMappingParameter == NULL)
etisserant@425: {
etisserant@425: MSG_ERR (0x1937, "Couldn't get mapping parameter : ",
etisserant@425: numMap + 1);
etisserant@425: return 0xFF;
etisserant@425: }
etisserant@425: /* Get the addresse of the mapped variable. */
etisserant@425: /* detail of *pMappingParameter : */
etisserant@425: /* The 16 hight bits contains the index, the medium 8 bits
etisserant@425: contains the subindex, */
etisserant@425: /* and the lower 8 bits contains the size of the mapped
etisserant@425: variable. */
etisserant@425:
etisserant@425: Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF);
etisserant@425:
etisserant@497: /* set variable only if Size != 0 and
etisserant@497: * Size is lower than remaining bits in the PDO */
etisserant@425: if (Size && ((offset + Size) <= (m->len << 3)))
etisserant@425: {
etisserant@425: /* copy bit per bit in little endian */
etisserant@425: CopyBits (Size, (UNS8 *) & m->data[offset >> 3],
etisserant@425: offset % 8, 0, ((UNS8 *) tmp), 0, 0);
etisserant@497: /*1->8 => 1 ; 9->16 =>2, ... */
etisserant@539: ByteSize = (UNS32)(1 + ((Size - 1) >> 3));
etisserant@425:
etisserant@425: objDict =
etisserant@425: setODentry (d, (UNS16) ((*pMappingParameter) >> 16),
etisserant@425: (UNS8) (((*pMappingParameter) >> 8) &
etisserant@425: 0xFF), tmp, &ByteSize, 0);
etisserant@425:
etisserant@425: if (objDict != OD_SUCCESSFUL)
etisserant@425: {
etisserant@425: MSG_ERR (0x1938,
etisserant@425: "error accessing to the mapped var : ",
etisserant@425: numMap + 1);
etisserant@425: MSG_WAR (0x2939, " Mapped at index : ",
etisserant@425: (*pMappingParameter) >> 16);
etisserant@425: MSG_WAR (0x2940, " subindex : ",
etisserant@425: ((*pMappingParameter) >> 8) & 0xFF);
etisserant@425: return 0xFF;
etisserant@425: }
etisserant@425:
etisserant@425: MSG_WAR (0x3942,
etisserant@497: "Variable updated by PDO cobid : ",
etisserant@447: UNS16_LE(m->cob_id));
etisserant@425: MSG_WAR (0x3943, " Mapped at index : ",
etisserant@425: (*pMappingParameter) >> 16);
etisserant@425: MSG_WAR (0x3944, " subindex : ",
etisserant@425: ((*pMappingParameter) >> 8) & 0xFF);
etisserant@425: offset += Size;
etisserant@425: }
etisserant@425: numMap++;
etisserant@425: } /* end loop while on mapped variables */
etisserant@425:
etisserant@425: return 0;
etisserant@425:
etisserant@425: } /* end switch status */
etisserant@425: } /* end while */
etisserant@425: } /* end if Donnees */
etisserant@425: else if ((*m).rtr == REQUEST)
etisserant@425: {
etisserant@447: MSG_WAR (0x3946, "Receive a PDO request cobId : ", UNS16_LE(m->cob_id));
etisserant@425: status = state1;
etisserant@425: offsetObjdict = d->firstIndex->PDO_TRS;
etisserant@425: lastIndex = d->lastIndex->PDO_TRS;
etisserant@425: if (offsetObjdict)
etisserant@425: while (offsetObjdict <= lastIndex)
etisserant@425: {
etisserant@425: /* study of all PDO stored in the objects dictionary */
etisserant@425:
etisserant@425: switch (status)
etisserant@425: {
etisserant@425:
etisserant@425: case state1: /* check the CobId */
etisserant@425: /* get CobId of the dictionary which match to the received PDO
etisserant@425: */
etisserant@425: pwCobId =
etisserant@447: (d->objdict +
etisserant@425: offsetObjdict)->pSubindex[1].pObject;
etisserant@447: if (*pwCobId == UNS16_LE(m->cob_id))
etisserant@425: {
etisserant@425: status = state4;
etisserant@425: break;
etisserant@425: }
etisserant@425: else
etisserant@425: {
etisserant@425: numPdo++;
etisserant@425: offsetObjdict++;
etisserant@425: }
etisserant@425: status = state1;
etisserant@425: break;
etisserant@425:
etisserant@425:
etisserant@425: case state4: /* check transmission type */
etisserant@425: pTransmissionType =
etisserant@425: (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
etisserant@425: /* If PDO is to be sampled and send on RTR, do it */
etisserant@425: if ((*pTransmissionType == TRANS_RTR))
etisserant@425: {
etisserant@425: status = state5;
etisserant@425: break;
etisserant@425: }
etisserant@497: /* RTR_SYNC means data prepared at SYNC, transmitted on RTR */
etisserant@425: else if ((*pTransmissionType == TRANS_RTR_SYNC))
etisserant@425: {
etisserant@425: if (d->PDO_status[numPdo].
etisserant@425: transmit_type_parameter & PDO_RTR_SYNC_READY)
etisserant@425: {
etisserant@425: /*Data ready, just send */
etisserant@425: canSend (d->canHandle,
etisserant@425: &d->PDO_status[numPdo].last_message);
etisserant@425: return 0;
etisserant@425: }
etisserant@425: else
etisserant@425: {
etisserant@497: /* if SYNC did never occur, transmit current data */
etisserant@425: /* DS301 do not tell what to do in such a case... */
etisserant@425: MSG_ERR (0x1947,
etisserant@425: "Not ready RTR_SYNC TPDO send current data : ",
etisserant@447: UNS16_LE(m->cob_id));
etisserant@425: status = state5;
etisserant@425: }
etisserant@425: break;
etisserant@425: }
etisserant@425: else if ((*pTransmissionType == TRANS_EVENT_PROFILE) ||
etisserant@425: (*pTransmissionType == TRANS_EVENT_SPECIFIC))
etisserant@425: {
etisserant@425: /* Zap all timers and inhibit flag */
etisserant@425: d->PDO_status[numPdo].event_timer =
etisserant@425: DelAlarm (d->PDO_status[numPdo].event_timer);
etisserant@425: d->PDO_status[numPdo].inhibit_timer =
etisserant@425: DelAlarm (d->PDO_status[numPdo].inhibit_timer);
etisserant@425: d->PDO_status[numPdo].transmit_type_parameter &=
etisserant@425: ~PDO_INHIBITED;
etisserant@497: /* Call PDOEventTimerAlarm for this TPDO,
etisserant@497: * this will trigger emission et reset timers */
etisserant@425: PDOEventTimerAlarm (d, numPdo);
etisserant@425: return 0;
etisserant@425: }
etisserant@425: else
etisserant@425: {
etisserant@425: /* The requested PDO is not to send on request. So, does
etisserant@425: nothing. */
etisserant@425: MSG_WAR (0x2947, "PDO is not to send on request : ",
etisserant@447: UNS16_LE(m->cob_id));
etisserant@425: return 0xFF;
etisserant@425: }
etisserant@425:
etisserant@425: case state5: /* build and send requested PDO */
etisserant@425: {
etisserant@425: Message pdo;
etisserant@425: if (buildPDO (d, numPdo, &pdo))
etisserant@425: {
etisserant@447: MSG_ERR (0x1948, " Couldn't build TPDO n�", numPdo);
etisserant@425: return 0xFF;
etisserant@425: }
etisserant@425: canSend (d->canHandle, &pdo);
etisserant@425: return 0;
etisserant@425: }
etisserant@425: } /* end switch status */
etisserant@425: } /* end while */
etisserant@425: } /* end if Requete */
nico@208:
etisserant@0: return 0;
etisserant@0: }
etisserant@0:
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param NbBits
nico@208: ** @param SrcByteIndex
nico@208: ** @param SrcBitIndex
nico@208: ** @param SrcBigEndian
nico@208: ** @param DestByteIndex
nico@208: ** @param DestBitIndex
nico@208: ** @param DestBigEndian
nico@208: **/
etisserant@425: void
etisserant@425: CopyBits (UNS8 NbBits, UNS8 * SrcByteIndex, UNS8 SrcBitIndex,
etisserant@425: UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex,
etisserant@425: UNS8 DestBigEndian)
etisserant@425: {
etisserant@425: /* This loop copy as many bits that it can each time, crossing */
etisserant@425: /* successively bytes */
nico@208: // boundaries from LSB to MSB.
etisserant@425: while (NbBits > 0)
nico@208: {
etisserant@425: /* Bit missalignement between src and dest */
nico@208: INTEGER8 Vect = DestBitIndex - SrcBitIndex;
nico@208:
etisserant@425: /* We can now get src and align it to dest */
etisserant@425: UNS8 Aligned =
etisserant@425: Vect > 0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
etisserant@425:
etisserant@425: /* Compute the nb of bit we will be able to copy */
etisserant@425: UNS8 BoudaryLimit = (Vect > 0 ? 8 - DestBitIndex : 8 - SrcBitIndex);
nico@208: UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
nico@208:
etisserant@425: /* Create a mask that will serve in: */
etisserant@425: UNS8 Mask =
etisserant@425: ((0xff << (DestBitIndex + BitsToCopy)) |
etisserant@425: (0xff >> (8 - DestBitIndex)));
etisserant@425:
etisserant@425: /* - Filtering src */
nico@208: UNS8 Filtered = Aligned & ~Mask;
nico@208:
etisserant@425: /* - and erase bits where we write, preserve where we don't */
nico@208: *DestByteIndex &= Mask;
nico@208:
etisserant@425: /* Then write. */
etisserant@425: *DestByteIndex |= Filtered;
etisserant@425:
etisserant@425: /*Compute next time cursors for src */
etisserant@425: if ((SrcBitIndex += BitsToCopy) > 7) /* cross boundary ? */
nico@208: {
etisserant@425: SrcBitIndex = 0; /* First bit */
etisserant@425: SrcByteIndex += (SrcBigEndian ? -1 : 1); /* Next byte */
nico@215: }
nico@215:
nico@215:
etisserant@425: /*Compute next time cursors for dest */
etisserant@425: if ((DestBitIndex += BitsToCopy) > 7)
nico@208: {
etisserant@425: DestBitIndex = 0; /* First bit */
etisserant@425: DestByteIndex += (DestBigEndian ? -1 : 1); /* Next byte */
nico@215: }
nico@215:
etisserant@425: /*And decrement counter. */
nico@208: NbBits -= BitsToCopy;
nico@208: }
greg@201:
greg@201: }
etisserant@425:
nico@208: /*!
nico@208: **
nico@208: **
nico@208: ** @param d
nico@208: **
nico@208: ** @return
nico@208: **/
etisserant@235:
etisserant@425: UNS8
etisserant@425: sendPDOevent (CO_Data * d)
etisserant@235: {
etisserant@235: /* Calls _sendPDOevent specifying it is not a sync event */
etisserant@425: return _sendPDOevent (d, 0);
etisserant@425: }
etisserant@425:
etisserant@425:
etisserant@425: void
etisserant@425: PDOEventTimerAlarm (CO_Data * d, UNS32 pdoNum)
etisserant@425: {
etisserant@425: /* This is needed to avoid deletion of re-attribuated timer */
etisserant@425: d->PDO_status[pdoNum].event_timer = TIMER_NONE;
etisserant@425: /* force emission of PDO by artificially changing last emitted */
etisserant@425: d->PDO_status[pdoNum].last_message.cob_id = 0;
etisserant@425: _sendPDOevent (d, 0); /* not a Sync Event */
etisserant@425: }
etisserant@425:
etisserant@425: void
etisserant@425: PDOInhibitTimerAlarm (CO_Data * d, UNS32 pdoNum)
etisserant@425: {
etisserant@425: /* This is needed to avoid deletion of re-attribuated timer */
etisserant@425: d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;
etisserant@425: /* Remove inhibit flag */
etisserant@425: d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;
etisserant@425: _sendPDOevent (d, 0); /* not a Sync Event */
etisserant@235: }
etisserant@235:
etisserant@235: /*!
etisserant@235: **
etisserant@235: **
etisserant@235: ** @param d
etisserant@235: ** @param isSyncEvent
etisserant@235: **
etisserant@235: ** @return
etisserant@235: **/
etisserant@235:
etisserant@425: UNS8
etisserant@425: _sendPDOevent (CO_Data * d, UNS8 isSyncEvent)
etisserant@425: {
etisserant@425: UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */
etisserant@425: UNS8 *pTransmissionType = NULL;
etisserant@235: UNS8 status = state3;
etisserant@235: UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
etisserant@235: UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
etisserant@425: UNS16 lastIndex = d->lastIndex->PDO_TRS;
etisserant@425:
etisserant@425: /* study all PDO stored in the objects dictionary */
etisserant@425: if (offsetObjdict)
etisserant@425: {
etisserant@497: Message pdo;/* = Message_Initializer;*/
etisserant@497: memset(&pdo, 0, sizeof(pdo));
etisserant@425: while (offsetObjdict <= lastIndex)
etisserant@235: {
etisserant@425: switch (status)
etisserant@425: {
etisserant@425: case state3:
etisserant@497: if ( /* bSubCount always 5 with objdictedit -> check disabled */
etisserant@497: /*d->objdict[offsetObjdict].bSubCount < 5 ||*/
etisserant@425: /* check if TPDO is not valid */
etisserant@425: *(UNS32 *) d->objdict[offsetObjdict].pSubindex[1].
etisserant@425: pObject & 0x80000000)
etisserant@425: {
etisserant@425: MSG_WAR (0x3960, "Not a valid PDO ", 0x1800 + pdoNum);
etisserant@425: /*Go next TPDO */
etisserant@425: status = state11;
etisserant@425: break;
etisserant@425: }
etisserant@425: /* get the PDO transmission type */
etisserant@425: pTransmissionType =
etisserant@425: (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject;
etisserant@425: MSG_WAR (0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);
etisserant@425:
etisserant@425: /* check if transmission type is SYNCRONOUS */
etisserant@497: /* message transmited every n SYNC with n=TransmissionType */
etisserant@425: if (isSyncEvent &&
etisserant@425: (*pTransmissionType >= TRANS_SYNC_MIN) &&
etisserant@425: (*pTransmissionType <= TRANS_SYNC_MAX) &&
etisserant@425: (++d->PDO_status[pdoNum].transmit_type_parameter ==
etisserant@425: *pTransmissionType))
etisserant@425: {
etisserant@425: /*Reset count of SYNC */
etisserant@425: d->PDO_status[pdoNum].transmit_type_parameter = 0;
etisserant@425: MSG_WAR (0x3964, " PDO is on SYNCHRO. Trans type : ",
etisserant@425: *pTransmissionType);
etisserant@497: memset(&pdo, 0, sizeof(pdo));
etisserant@497: /*{
etisserant@425: Message msg_init = Message_Initializer;
etisserant@425: pdo = msg_init;
etisserant@497: }*/
etisserant@425: if (buildPDO (d, pdoNum, &pdo))
etisserant@425: {
etisserant@425: MSG_ERR (0x1906, " Couldn't build TPDO number : ",
etisserant@425: pdoNum);
etisserant@425: status = state11;
etisserant@425: break;
etisserant@425: }
etisserant@425: status = state5;
etisserant@425: /* If transmission RTR, with data sampled on SYNC */
etisserant@425: }
etisserant@425: else if (isSyncEvent && (*pTransmissionType == TRANS_RTR_SYNC))
etisserant@425: {
etisserant@425: if (buildPDO
etisserant@425: (d, pdoNum, &d->PDO_status[pdoNum].last_message))
etisserant@425: {
etisserant@425: MSG_ERR (0x1966, " Couldn't build TPDO number : ",
etisserant@425: pdoNum);
etisserant@425: d->PDO_status[pdoNum].transmit_type_parameter &=
etisserant@425: ~PDO_RTR_SYNC_READY;
etisserant@425: }
etisserant@425: else
etisserant@425: {
etisserant@425: d->PDO_status[pdoNum].transmit_type_parameter |=
etisserant@425: PDO_RTR_SYNC_READY;
etisserant@425: }
etisserant@425: status = state11;
etisserant@425: break;
etisserant@425: /* If transmission on Event and not inhibited, check for changes */
etisserant@425: }
etisserant@426: else
etisserant@426: if ((isSyncEvent
etisserant@426: && (*pTransmissionType == TRANS_SYNC_ACYCLIC))
etisserant@426: ||
etisserant@426: ((*pTransmissionType == TRANS_EVENT_PROFILE
etisserant@426: || *pTransmissionType == TRANS_EVENT_SPECIFIC)
etisserant@426: && !(d->PDO_status[pdoNum].
etisserant@426: transmit_type_parameter & PDO_INHIBITED)))
etisserant@425: {
etisserant@425: MSG_WAR (0x3968, " PDO is on EVENT. Trans type : ",
etisserant@425: *pTransmissionType);
etisserant@497: memset(&pdo, 0, sizeof(pdo));
etisserant@497: /*{
etisserant@425: Message msg_init = Message_Initializer;
etisserant@425: pdo = msg_init;
etisserant@497: }*/
etisserant@425: if (buildPDO (d, pdoNum, &pdo))
etisserant@425: {
etisserant@425: MSG_ERR (0x3907, " Couldn't build TPDO number : ",
etisserant@425: pdoNum);
etisserant@425: status = state11;
etisserant@425: break;
etisserant@425: }
etisserant@425:
etisserant@425: /*Compare new and old PDO */
etisserant@425: if (d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id
etisserant@497: && d->PDO_status[pdoNum].last_message.len == pdo.len
etisserant@497: && memcmp(d->PDO_status[pdoNum].last_message.data,
etisserant@474: pdo.data, 8) == 0
etisserant@425: )
etisserant@425: {
etisserant@425: /* No changes -> go to next pdo */
etisserant@425: status = state11;
etisserant@425: }
etisserant@425: else
etisserant@425: {
etisserant@425:
peter@524: TIMEVAL EventTimerDuration;
peter@524: TIMEVAL InhibitTimerDuration;
etisserant@425:
etisserant@425: MSG_WAR (0x306A, "Changes TPDO number : ", pdoNum);
etisserant@425: /* Changes detected -> transmit message */
etisserant@425: EventTimerDuration =
etisserant@425: *(UNS16 *) d->objdict[offsetObjdict].pSubindex[5].
etisserant@425: pObject;
etisserant@425: InhibitTimerDuration =
etisserant@425: *(UNS16 *) d->objdict[offsetObjdict].pSubindex[3].
etisserant@425: pObject;
etisserant@425:
etisserant@425: status = state5;
etisserant@425:
etisserant@425: /* Start both event_timer and inhibit_timer */
etisserant@425: if (EventTimerDuration)
etisserant@425: {
etisserant@425: DelAlarm (d->PDO_status[pdoNum].event_timer);
etisserant@425: d->PDO_status[pdoNum].event_timer =
etisserant@425: SetAlarm (d, pdoNum, &PDOEventTimerAlarm,
etisserant@425: MS_TO_TIMEVAL (EventTimerDuration), 0);
etisserant@425: }
etisserant@425:
etisserant@425: if (InhibitTimerDuration)
etisserant@425: {
etisserant@425: DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
etisserant@425: d->PDO_status[pdoNum].inhibit_timer =
etisserant@425: SetAlarm (d, pdoNum, &PDOInhibitTimerAlarm,
etisserant@425: US_TO_TIMEVAL (InhibitTimerDuration *
etisserant@425: 100), 0);
etisserant@425: /* and inhibit TPDO */
etisserant@425: d->PDO_status[pdoNum].transmit_type_parameter |=
etisserant@425: PDO_INHIBITED;
etisserant@425: }
etisserant@425:
etisserant@425: }
etisserant@425: }
etisserant@425: else
etisserant@425: {
etisserant@425: MSG_WAR (0x306C,
etisserant@425: " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ",
etisserant@425: *pTransmissionType);
etisserant@425: status = state11;
etisserant@425: }
etisserant@425: break;
etisserant@425: case state5: /*Send the pdo */
etisserant@425: /*store_as_last_message */
etisserant@425: d->PDO_status[pdoNum].last_message = pdo;
etisserant@447: MSG_WAR (0x396D, "sendPDO cobId :", UNS16_LE(pdo.cob_id));
etisserant@425: MSG_WAR (0x396E, " Nb octets : ", pdo.len);
etisserant@425:
etisserant@425: canSend (d->canHandle, &pdo);
etisserant@425: status = state11;
etisserant@425: break;
etisserant@425: case state11: /*Go to next TPDO */
etisserant@425: pdoNum++;
etisserant@425: offsetObjdict++;
etisserant@425: offsetObjdictMap++;
etisserant@425: MSG_WAR (0x3970, "next pdo index : ", pdoNum);
etisserant@425: status = state3;
etisserant@425: break;
etisserant@425:
etisserant@425: default:
etisserant@425: MSG_ERR (0x1972, "Unknown state has been reached : %d", status);
etisserant@425: return 0xFF;
etisserant@425: } /* end switch case */
etisserant@425:
etisserant@425: } /* end while */
etisserant@425: }
etisserant@235: return 0;
etisserant@235: }
etisserant@235:
etisserant@236: /*!
etisserant@236: **
etisserant@236: **
etisserant@236: ** @param d
etisserant@236: ** @param OD_entry
peter@524: ** @param bSubindex
etisserant@236: ** @return always 0
etisserant@236: **/
etisserant@236:
etisserant@425: UNS32
etisserant@425: TPDO_Communication_Parameter_Callback (CO_Data * d,
etisserant@425: const indextable * OD_entry,
etisserant@425: UNS8 bSubindex)
etisserant@236: {
etisserant@236: /* If PDO are actives */
etisserant@425: if (d->CurrentCommunicationState.csPDO)
etisserant@425: switch (bSubindex)
etisserant@425: {
etisserant@425: case 2: /* Changed transmition type */
etisserant@425: case 3: /* Changed inhibit time */
etisserant@425: case 5: /* Changed event time */
etisserant@425: {
etisserant@425: const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS;
etisserant@425: UNS8 numPdo = OD_entry - TPDO_com; /* number of the actual processed pdo-nr. */
etisserant@425:
etisserant@236: /* Zap all timers and inhibit flag */
etisserant@425: d->PDO_status[numPdo].event_timer =
etisserant@425: DelAlarm (d->PDO_status[numPdo].event_timer);
etisserant@425: d->PDO_status[numPdo].inhibit_timer =
etisserant@425: DelAlarm (d->PDO_status[numPdo].inhibit_timer);
etisserant@236: d->PDO_status[numPdo].transmit_type_parameter = 0;
etisserant@236: /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
etisserant@425: PDOEventTimerAlarm (d, numPdo);
etisserant@236: return 0;
etisserant@425: }
etisserant@425:
etisserant@425: default: /* other subindex are ignored */
etisserant@425: break;
etisserant@425: }
etisserant@236: return 0;
etisserant@236: }
etisserant@236:
etisserant@425: void
etisserant@425: PDOInit (CO_Data * d)
etisserant@235: {
etisserant@236: /* For each TPDO mapping parameters */
etisserant@425: UNS16 pdoIndex = 0x1800; /* OD index of TDPO */
etisserant@236:
etisserant@236: UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
etisserant@236: UNS16 lastIndex = d->lastIndex->PDO_TRS;
etisserant@425: if (offsetObjdict)
etisserant@425: while (offsetObjdict <= lastIndex)
etisserant@425: {
etisserant@425: /* Assign callbacks to sensible TPDO mapping subindexes */
etisserant@425: UNS32 errorCode;
etisserant@425: ODCallback_t *CallbackList;
etisserant@425: /* Find callback list */
etisserant@425: scanIndexOD (d, pdoIndex, &errorCode, &CallbackList);
etisserant@425: if (errorCode == OD_SUCCESSFUL && CallbackList)
etisserant@425: {
etisserant@425: /*Assign callbacks to corresponding subindex */
etisserant@425: /* Transmission type */
etisserant@425: CallbackList[2] = &TPDO_Communication_Parameter_Callback;
etisserant@425: /* Inhibit time */
etisserant@425: CallbackList[3] = &TPDO_Communication_Parameter_Callback;
etisserant@425: /* Event timer */
etisserant@425: CallbackList[5] = &TPDO_Communication_Parameter_Callback;
etisserant@425: }
etisserant@425: pdoIndex++;
etisserant@425: offsetObjdict++;
etisserant@425: }
etisserant@236:
etisserant@236: /* Trigger a non-sync event */
etisserant@425: _sendPDOevent (d, 0);
etisserant@425: }
etisserant@425:
etisserant@425: void
etisserant@425: PDOStop (CO_Data * d)
etisserant@235: {
etisserant@236: /* For each TPDO mapping parameters */
etisserant@425: UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */
etisserant@235: UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
etisserant@235: UNS16 lastIndex = d->lastIndex->PDO_TRS;
etisserant@425: if (offsetObjdict)
etisserant@425: while (offsetObjdict <= lastIndex)
etisserant@425: {
etisserant@425: /* Delete TPDO timers */
etisserant@425: d->PDO_status[pdoNum].event_timer =
etisserant@425: DelAlarm (d->PDO_status[pdoNum].event_timer);
etisserant@425: d->PDO_status[pdoNum].inhibit_timer =
etisserant@425: DelAlarm (d->PDO_status[pdoNum].inhibit_timer);
etisserant@425: /* Reset transmit type parameter */
etisserant@425: d->PDO_status[pdoNum].transmit_type_parameter = 0;
etisserant@425: d->PDO_status[pdoNum].last_message.cob_id = 0;
etisserant@425: pdoNum++;
etisserant@425: offsetObjdict++;
etisserant@425: }
etisserant@425: }