libs/SOEM
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
SOEM/soem/ethercatsoe.c
ArthurKetels cbc8f36e87
Fix bug in ecx_readIDNmap(), Osize and Isize 
Osize and Isize where reset to 16 at every new drive number, thus loosing all lower drive mapping data. Changed to add 16 to Osize and Isize.
2020-10-01 23:00:01 +02:00

390 lines
13 KiB
C
Raw Blame History

/*
* Licensed under the GNU General Public License version 2 with exceptions. See
* LICENSE file in the project root for full license information
*/
/** \file
* \brief
* Servo over EtherCAT (SoE) Module.
*/
#include <stdio.h>
#include <string.h>
#include "osal.h"
#include "oshw.h"
#include "ethercattype.h"
#include "ethercatbase.h"
#include "ethercatmain.h"
#include "ethercatsoe.h"
#define EC_SOE_MAX_DRIVES 8
/** SoE (Servo over EtherCAT) mailbox structure */
PACKED_BEGIN
typedef struct PACKED
{
ec_mbxheadert MbxHeader;
uint8 opCode :3;
uint8 incomplete :1;
uint8 error :1;
uint8 driveNo :3;
uint8 elementflags;
union
{
uint16 idn;
uint16 fragmentsleft;
};
} ec_SoEt;
PACKED_END
/** Report SoE error.
*
* @param[in] context = context struct
* @param[in] Slave = Slave number
* @param[in] idn = IDN that generated error
* @param[in] Error = Error code, see EtherCAT documentation for list
*/
void ecx_SoEerror(ecx_contextt *context, uint16 Slave, uint16 idn, uint16 Error)
{
ec_errort Ec;
memset(&Ec, 0, sizeof(Ec));
Ec.Time = osal_current_time();
Ec.Slave = Slave;
Ec.Index = idn;
Ec.SubIdx = 0;
*(context->ecaterror) = TRUE;
Ec.Etype = EC_ERR_TYPE_SOE_ERROR;
Ec.ErrorCode = Error;
ecx_pusherror(context, &Ec);
}
/** SoE read, blocking.
*
* The IDN object of the selected slave and DriveNo is read. If a response
* is larger than the mailbox size then the response is segmented. The function
* will combine all segments and copy them to the parameter buffer.
*
* @param[in] context = context struct
* @param[in] slave = Slave number
* @param[in] driveNo = Drive number in slave
* @param[in] elementflags = Flags to select what properties of IDN are to be transferred.
* @param[in] idn = IDN.
* @param[in,out] psize = Size in bytes of parameter buffer, returns bytes read from SoE.
* @param[out] p = Pointer to parameter buffer
* @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM
* @return Workcounter from last slave response
*/
int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
{
ec_SoEt *SoEp, *aSoEp;
uint16 totalsize, framedatasize;
int wkc;
uint8 *bp;
uint8 *mp;
uint16 *errorcode;
ec_mbxbuft MbxIn, MbxOut;
uint8 cnt;
boolean NotLast;
ec_clearmbx(&MbxIn);
/* Empty slave out mailbox if something is in. Timeout set to 0 */
wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
ec_clearmbx(&MbxOut);
aSoEp = (ec_SoEt *)&MbxIn;
SoEp = (ec_SoEt *)&MbxOut;
SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert));
SoEp->MbxHeader.address = htoes(0x0000);
SoEp->MbxHeader.priority = 0x00;
/* get new mailbox count value, used as session handle */
cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
context->slavelist[slave].mbx_cnt = cnt;
SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */
SoEp->opCode = ECT_SOE_READREQ;
SoEp->incomplete = 0;
SoEp->error = 0;
SoEp->driveNo = driveNo;
SoEp->elementflags = elementflags;
SoEp->idn = htoes(idn);
totalsize = 0;
bp = p;
mp = (uint8 *)&MbxIn + sizeof(ec_SoEt);
NotLast = TRUE;
/* send SoE request to slave */
wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
if (wkc > 0) /* succeeded to place mailbox in slave ? */
{
while (NotLast)
{
/* clean mailboxbuffer */
ec_clearmbx(&MbxIn);
/* read slave response */
wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
if (wkc > 0) /* succeeded to read slave response ? */
{
/* slave response should be SoE, ReadRes */
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
(aSoEp->opCode == ECT_SOE_READRES) &&
(aSoEp->error == 0) &&
(aSoEp->driveNo == driveNo) &&
(aSoEp->elementflags == elementflags))
{
framedatasize = etohs(aSoEp->MbxHeader.length) - sizeof(ec_SoEt) + sizeof(ec_mbxheadert);
totalsize += framedatasize;
/* Does parameter fit in parameter buffer ? */
if (totalsize <= *psize)
{
/* copy parameter data in parameter buffer */
memcpy(bp, mp, framedatasize);
/* increment buffer pointer */
bp += framedatasize;
}
else
{
framedatasize -= totalsize - *psize;
totalsize = *psize;
/* copy parameter data in parameter buffer */
if (framedatasize > 0) memcpy(bp, mp, framedatasize);
}
if (!aSoEp->incomplete)
{
NotLast = FALSE;
*psize = totalsize;
}
}
/* other slave response */
else
{
NotLast = FALSE;
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
(aSoEp->opCode == ECT_SOE_READRES) &&
(aSoEp->error == 1))
{
mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
errorcode = (uint16 *)mp;
ecx_SoEerror(context, slave, idn, *errorcode);
}
else
{
ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */
}
wkc = 0;
}
}
else
{
NotLast = FALSE;
ecx_packeterror(context, slave, idn, 0, 4); /* no response */
}
}
}
return wkc;
}
/** SoE write, blocking.
*
* The IDN object of the selected slave and DriveNo is written. If a response
* is larger than the mailbox size then the response is segmented.
*
* @param[in] context = context struct
* @param[in] slave = Slave number
* @param[in] driveNo = Drive number in slave
* @param[in] elementflags = Flags to select what properties of IDN are to be transferred.
* @param[in] idn = IDN.
* @param[in] psize = Size in bytes of parameter buffer.
* @param[out] p = Pointer to parameter buffer
* @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM
* @return Workcounter from last slave response
*/
int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
{
ec_SoEt *SoEp, *aSoEp;
uint16 framedatasize, maxdata;
int wkc;
uint8 *mp;
uint8 *hp;
uint16 *errorcode;
ec_mbxbuft MbxIn, MbxOut;
uint8 cnt;
boolean NotLast;
ec_clearmbx(&MbxIn);
/* Empty slave out mailbox if something is in. Timeout set to 0 */
wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
ec_clearmbx(&MbxOut);
aSoEp = (ec_SoEt *)&MbxIn;
SoEp = (ec_SoEt *)&MbxOut;
SoEp->MbxHeader.address = htoes(0x0000);
SoEp->MbxHeader.priority = 0x00;
SoEp->opCode = ECT_SOE_WRITEREQ;
SoEp->error = 0;
SoEp->driveNo = driveNo;
SoEp->elementflags = elementflags;
hp = p;
mp = (uint8 *)&MbxOut + sizeof(ec_SoEt);
maxdata = context->slavelist[slave].mbx_l - sizeof(ec_SoEt);
NotLast = TRUE;
while (NotLast)
{
framedatasize = psize;
NotLast = FALSE;
SoEp->idn = htoes(idn);
SoEp->incomplete = 0;
if (framedatasize > maxdata)
{
framedatasize = maxdata; /* segmented transfer needed */
NotLast = TRUE;
SoEp->incomplete = 1;
SoEp->fragmentsleft = psize / maxdata;
}
SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert) + framedatasize);
/* get new mailbox counter, used for session handle */
cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
context->slavelist[slave].mbx_cnt = cnt;
SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */
/* copy parameter data to mailbox */
memcpy(mp, hp, framedatasize);
hp += framedatasize;
psize -= framedatasize;
/* send SoE request to slave */
wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
if (wkc > 0) /* succeeded to place mailbox in slave ? */
{
if (!NotLast || !ecx_mbxempty(context, slave, timeout))
{
/* clean mailboxbuffer */
ec_clearmbx(&MbxIn);
/* read slave response */
wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
if (wkc > 0) /* succeeded to read slave response ? */
{
NotLast = FALSE;
/* slave response should be SoE, WriteRes */
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
(aSoEp->opCode == ECT_SOE_WRITERES) &&
(aSoEp->error == 0) &&
(aSoEp->driveNo == driveNo) &&
(aSoEp->elementflags == elementflags))
{
/* SoE write succeeded */
}
/* other slave response */
else
{
if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
(aSoEp->opCode == ECT_SOE_READRES) &&
(aSoEp->error == 1))
{
mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
errorcode = (uint16 *)mp;
ecx_SoEerror(context, slave, idn, *errorcode);
}
else
{
ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */
}
wkc = 0;
}
}
else
{
ecx_packeterror(context, slave, idn, 0, 4); /* no response */
}
}
}
}
return wkc;
}
/** SoE read AT and MTD mapping.
*
* SoE has standard indexes defined for mapping. This function
* tries to read them and collect a full input and output mapping size
* of designated slave.
*
* @param[in] context = context struct
* @param[in] slave = Slave number
* @param[out] Osize = Size in bits of output mapping (MTD) found
* @param[out] Isize = Size in bits of input mapping (AT) found
* @return >0 if mapping successful.
*/
int ecx_readIDNmap(ecx_contextt *context, uint16 slave, int *Osize, int *Isize)
{
int retVal = 0;
int wkc;
int psize;
int driveNr;
uint16 entries, itemcount;
ec_SoEmappingt SoEmapping;
ec_SoEattributet SoEattribute;
*Isize = 0;
*Osize = 0;
for(driveNr = 0; driveNr < EC_SOE_MAX_DRIVES; driveNr++)
{
psize = sizeof(SoEmapping);
/* read output mapping via SoE */
wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_MDTCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
{
/* command word (uint16) is always mapped but not in list */
*Osize += 16;
for (itemcount = 0 ; itemcount < entries ; itemcount++)
{
psize = sizeof(SoEattribute);
/* read attribute of each IDN in mapping list */
wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
if ((wkc > 0) && (!SoEattribute.list))
{
/* length : 0 = 8bit, 1 = 16bit .... */
*Osize += (int)8 << SoEattribute.length;
}
}
}
psize = sizeof(SoEmapping);
/* read input mapping via SoE */
wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_ATCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
{
/* status word (uint16) is always mapped but not in list */
*Isize += 16;
for (itemcount = 0 ; itemcount < entries ; itemcount++)
{
psize = sizeof(SoEattribute);
/* read attribute of each IDN in mapping list */
wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
if ((wkc > 0) && (!SoEattribute.list))
{
/* length : 0 = 8bit, 1 = 16bit .... */
*Isize += (int)8 << SoEattribute.length;
}
}
}
}
/* found some I/O bits ? */
if ((*Isize > 0) || (*Osize > 0))
{
retVal = 1;
}
return retVal;
}
#ifdef EC_VER1
int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
{
return ecx_SoEread(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout);
}
int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
{
return ecx_SoEwrite(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout);
}
int ec_readIDNmap(uint16 slave, int *Osize, int *Isize)
{
return ecx_readIDNmap(&ecx_context, slave, Osize, Isize);
}
#endif
Reference in New Issue View Git Blame Copy Permalink