/*
-This file is part of CanFestival, a library implementing CanOpen Stack.
+ This file is part of CanFestival, a library implementing CanOpen
+ Stack.
-Copyright (C): Edouard TISSERANT and Francis DUPIN
+ Copyright (C): Edouard TISSERANT and Francis DUPIN
-See COPYING file for copyrights details.
+ 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 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.
+ 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
+ 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
+**
+**
+*/
-/****************************************************************************/
-UNS8 sendPDO(CO_Data* d, s_PDO pdo, UNS8 req)
+/*!
+**
+**
+** @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)
{
- UNS8 i;
- if( d->nodeState == Operational ) {
- Message m;
-
- /* Message copy for sending */
- m.cob_id.w = pdo.cobId & 0x7FF; /* Because the cobId is 11 bytes length */
- if ( req == NOT_A_REQUEST ) {
- UNS8 i;
- m.rtr = NOT_A_REQUEST;
- m.len = pdo.len;
- /* memcpy(&m.data, &pdo.data, m.len); */
- /* This Memcpy depends on packing structure. Avoid */
- for (i = 0 ; i < pdo.len ; i++)
- m.data[i] = pdo.data[i];
- }
- else {
- m.rtr = REQUEST;
- m.len = 0;
+ 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;
+ UNS8 offset = 0x00;
+ const UNS8 *pMappingCount = (UNS8 *) TPDO_map->pSubindex[0].pObject;
+
+ pdo->cob_id = 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);
+ UNS8 Size = (UNS8) (*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))
+ {
+ UNS8 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 (Size, ((UNS8 *) tmp), 0, 0,
+ (UNS8 *) & pdo->data[offset >> 3], offset % 8, 0);
+
+ offset += Size;
+ }
+ prp_j++;
}
+ while (prp_j < *pMappingCount);
- MSG_WAR(0x3901, "sendPDO cobId :", m.cob_id.w);
- MSG_WAR(0x3902, " Nb octets : ", m.len);
- for (i = 0 ; i < m.len ; i++) {
- MSG_WAR(0x3903," data : ", m.data[i]);
- }
-
- return canSend(d->canHandle,&m);
- } /* end if */
- return 0xFF;
-}
+ pdo->len = 1 + ((offset - 1) >> 3);
-/***************************************************************************/
-UNS8 PDOmGR(CO_Data* d, UNS32 cobId) /* PDO Manager */
-{
- UNS8 res;
- UNS8 i;
- s_PDO pdo;
-
- MSG_WAR(0x3905, "PDOmGR",0);
-
- /* if PDO is waiting for transmission,
- preparation of the message to send */
- pdo.cobId = cobId;
- pdo.len = d->process_var.count;
- /* memcpy(&(pdo.data), &(process_var.data), pdo.len); */
- /* Ce memcpy devrait être portable */
- for ( i = 0 ; i < pdo.len ; i++)
- pdo.data[i] = d->process_var.data[i];
-
- res = sendPDO(d, pdo, NOT_A_REQUEST);
-
- return res;
-}
+ MSG_WAR (0x3015, " End scan mapped variable", 0);
-#if 0
-/*********************************************************************/
-/* TODO : implement bit mapping */
-/*********************************************************************/
-
-UNS8 buildPDO(CO_Data* d, UNS16 index)
-{ /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */
- UNS16 ind;
- UNS8 subInd;
-
- 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 Size;
- UNS8 dataType;
- UNS8 offset;
- UNS16 offsetObjdict;
- UNS16 offsetObjdictPrm;
- UNS32 objDict;
-
- subInd=(UNS8)0x00;
- offset = 0x00;
- ind = index - 0x1800;
-
- MSG_WAR(0x3910,"Prepare PDO to send index :", index);
-
- /* only operational state allows PDO transmission */
- if( d->nodeState != Operational ) {
- MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index);
- return 0xFF;
- }
- offsetObjdictPrm = d->firstIndex->PDO_TRS;
- offsetObjdict = d->firstIndex->PDO_TRS_MAP;
-
- if (offsetObjdictPrm && offsetObjdict)
- {
- /* get mapped objects number to transmit with this PDO */
- pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject;
- MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount);
- MSG_WAR(0x3913, " at index : ", 0x1A00 + ind);
- while (subInd < *pMappingCount) { /* Loop on mapped variables */
- /* get mapping parameters */
- pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject;
- MSG_WAR(0x3914, "Get the mapping at index : ", (UNS16)0x1A00 + ind);
- MSG_WAR(0x3915, " subIndex : ", subInd + 1);
- MSG_WAR(0x3916, " value : ", *(UNS32 *)pMappingParameter);
- /* Get the mapped variable */
- Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
- objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16),
- (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF),
- (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
-
- if (objDict != OD_SUCCESSFUL) {
- MSG_WAR(0x2919, "error accessing to the mapped var : ", subInd + 1);
- MSG_WAR(0x2920, " Mapped at index : ", (*pMappingParameter) >> 16);
- MSG_WAR(0x2921, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
- return 0xFF;
- }
-
- offset += Size;
- d->process_var.count = offset;
- subInd++;
- }/* end Loop on mapped variables */
- }
return 0;
}
-#endif
-/**************************************************************************/
-UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId )
-{
- UNS32 * pwCobId;
- UNS16 offset;
- UNS16 lastIndex;
- UNS8 err;
-
- MSG_WAR(0x3930, "sendPDOrequest ",0);
- /* Sending the request only if the cobid have been found on the PDO receive */
+
+/*!
+**
+**
+** @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;
+
+ /* Sending the request only if the cobid have been found on the PDO
+ receive */
/* part dictionary */
- offset = d->firstIndex->PDO_RCV;
- lastIndex = d->lastIndex->PDO_RCV;
- if (offset)
- while (offset <= lastIndex) {
- /*get the CobId*/
- pwCobId = d->objdict[offset].pSubindex[1].pObject;
-
- if ( *pwCobId == cobId ) {
- s_PDO pdo;
- pdo.cobId = *pwCobId;
- pdo.len = 0;
- err = sendPDO(d, pdo, REQUEST);
- return err;
- }
- offset++;
- }
- MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId);
+
+ 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;
}
-
-/***********************************************************************/
-UNS8 proceedPDO(CO_Data* d, Message *m)
-{
- UNS8 numPdo;
- UNS8 numMap; /* Number of the mapped varable */
- UNS8 i;
- UNS8 * pMappingCount = NULL; /* count of mapped objects... */
+/*!
+**
+**
+** @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 dataType;
- UNS8 offset;
- UNS8 status;
- UNS32 objDict;
- UNS16 offsetObjdict;
- UNS16 lastIndex;
- status = state1;
-
- MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));
+ /* 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 state1: /* data are stored in process_var array */
- /* memcpy(&(process_var.data), &m->data, (*m).len); */
- /* Ce memcpy devrait être portable. */
- for ( i = 0 ; i < m->len ; i++)
- d->process_var.data[i] = m->data[i];
- d->process_var.count = (*m).len;
-
- status = state2;
- break;
-
- case state2:
- /* get CobId of the dictionary correspondant to the received PDO */
- pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;
- /* check the CobId coherance */
- /*pwCobId is the cobId read in the dictionary at the state 3 */
- if ( *pwCobId == (*m).cob_id.w ){
- /* 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 = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
- numMap = 0;
- while (numMap < *pMappingCount) {
- UNS8 tmp[]= {0,0,0,0,0,0,0,0};
- UNS8 ByteSize;
- pMappingParameter = (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);
-
- /* copy bit per bit in little endian */
- CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
-
- ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
-
- 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 with value received by PDO cobid : ", m->cob_id.w);
- MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16);
- MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
- /* MSG_WAR(0x3945, " data : ",*((UNS32 *)pMappedAppObject)); */
- offset += Size;
- numMap++;
- } /* end loop while on mapped variables */
-
- offset=0x00;
- numMap = 0;
- return 0;
-
- }/* end switch status */
- }/* end while */
- }/* end if Donnees */
-
-
- else if ((*m).rtr == REQUEST ){
- MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);
+ 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 */
+ pwCobId =
+ 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 };
+ UNS8 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 = 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 */
- pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject;
- if ( *pwCobId == (*m).cob_id.w ) {
- status = state4;
- break;
- }
- else {
- numPdo++;
- offsetObjdict++;
- }
- status = state1;
- break;
-
-
- case state4: /* check transmission type (after request?) */
- pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;
- if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) {
- status = state5;
- break;
- }
- else {
- /* The requested PDO is not to send on request. So, does nothing. */
- MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);
- return 0xFF;
- }
-
- case state5: /* get mapped objects number */
- offsetObjdict = d->firstIndex->PDO_TRS_MAP;
- lastIndex = d->lastIndex->PDO_TRS_MAP;
- pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
- numMap = 0;
- while (numMap < *pMappingCount) {
- pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
- /* Get the mapped variable */
- Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
- objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16),
- (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF),
- (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
- if (objDict != OD_SUCCESSFUL) {
- MSG_ERR(0x1948, "error accessing to the mapped var : ", numMap + 1);
- MSG_WAR(0x2949, " Mapped at index : ", (*pMappingParameter) >> 16);
- MSG_WAR(0x2950, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
- return 0xFF;
- }
- offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3);
- d->process_var.count = offset;
- numMap++;
-
- } /* end while */
- PDOmGR( d, *pwCobId ); /* Transmit the PDO */
- return 0;
-
- }/* end switch status */
- }/* end while */
- }/* end if Requete */
-
+ 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 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;
+ }
+
+}
+
+/*!
+**
+**
+** @param d
+**
+** @return
+**/
-void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)
+UNS8
+sendPDOevent (CO_Data * d)
{
- //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;
- }
+ /* Calls _sendPDOevent specifying it is not a sync event */
+ return _sendPDOevent (d, 0);
+}
+
+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;
+ _sendPDOevent (d, 0); /* not a Sync Event */
}
-#if 0
-
-/*********************************************************************/
-/* TODO : reimplement this using CallBacks */
-/*********************************************************************/
-
-UNS8 sendPDOevent( CO_Data* d, void * variable )
-{ /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */
- UNS32 objDict = 0;
- UNS8 ind, sub_ind;
- UNS8 status;
- UNS8 offset;
- UNS8 * pMappingCount = NULL;
- UNS32 * pMappingParameter = NULL;
- void * pMappedAppObject = NULL;
- UNS8 * pTransmissionType = NULL; /* pointer to the transmission type */
- UNS32 * pwCobId = NULL;
- UNS8 * pSize;
- UNS8 size;
- UNS8 dataType;
- UNS16 offsetObjdict;
- UNS16 offsetObjdictPrm;
- UNS16 lastIndex;
- UNS8 numMap;
- ind = 0x00;
- sub_ind = 1;
- offset = 0x00;
- pSize = &size;
- status = state1;
-
- /* look for the index and subindex where the variable is mapped */
- /* Then, send the pdo which contains the variable. */
-
- MSG_WAR (0x3960, "sendPDOevent", 0);
- offsetObjdictPrm = d->firstIndex->PDO_TRS;
-
- offsetObjdict = d->firstIndex->PDO_TRS_MAP;
- lastIndex = d->lastIndex->PDO_TRS_MAP;
-
- if (offsetObjdictPrm && offsetObjdict)
- /* Loop on PDO Transmit */
- while(offsetObjdict <= lastIndex){
- /* Check the transmission mode */
- pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject;
- if (*pTransmissionType != TRANS_EVENT) {
- ind++;
- offsetObjdict++;
- offsetObjdictPrm++;
- continue;
- }
- pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject;
- numMap = 1; /* mapped variable */
- while (numMap <= *pMappingCount) {
- pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject;
- /* Get the variable */
- objDict = getODentry( d,
- (UNS16)((*pMappingParameter) >> 16),
- (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF),
- (void * *)&pMappedAppObject, pSize, &dataType, 0 );
- if( objDict != OD_SUCCESSFUL ) {
- MSG_WAR(0x2961, "Error in dict. at index : ",
- (*pMappingParameter) >> (UNS8)16);
-
- MSG_WAR(0x2962, " subindex : ",
- ((*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
- return 0xFF;
- }
- if (pMappedAppObject == variable) { // Variable found !
- MSG_WAR(0x3963, "Variable to send found at index : ",
- (*pMappingParameter) >> 16);
- MSG_WAR(0x3964, " subIndex : ",
- ((*pMappingParameter) >> 8 ) & 0x000000FF);
- buildPDO(d, 0x1800 + ind);
- /* Get the cobId */
- pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject;
- PDOmGR( d, *pwCobId ); /* Send the PDO */
- return 0;
- }
- numMap++;
- } /* End loop on mapped variable */
- ind++;
- offsetObjdict++;
- offsetObjdictPrm++;
- } /* End loop while on PDO */
-
- MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0);
- return 0xFF;
+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;
+ _sendPDOevent (d, 0); /* not a Sync Event */
+}
+
+/*!
+**
+**
+** @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;
+
+ /* 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))
+ ||
+ ((*pTransmissionType == TRANS_EVENT_PROFILE
+ || *pTransmissionType == TRANS_EVENT_SPECIFIC)
+ && !(d->PDO_status[pdoNum].
+ transmit_type_parameter & PDO_INHIBITED)))
+ {
+ MSG_WAR (0x3968, " PDO is on EVENT. Trans type : ",
+ *pTransmissionType);
+ memset(&pdo, 0, sizeof(pdo));
+ /*{
+ Message msg_init = Message_Initializer;
+ pdo = msg_init;
+ }*/
+ if (buildPDO (d, pdoNum, &pdo))
+ {
+ MSG_ERR (0x3907, " Couldn't build TPDO number : ",
+ pdoNum);
+ status = state11;
+ break;
+ }
+
+ /*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 */
+ status = state11;
+ }
+ else
+ {
+
+ UNS16 EventTimerDuration;
+ UNS16 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;
+
+ status = state5;
+
+ /* 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;
+ }
+
+ }
+ }
+ 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 */
+ /*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);
+ 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 = 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++;
+ }
}
-#endif
projects / CanFestival-3.git / blobdiff