ln-dotnet
/
SOEM
3
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
SOEM/soem/ethercatsoe.c

390 lines
14 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;
int 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 + MBX_HDR_SET_CNT(cnt); /* 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;
int 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 = (uint16)(psize / maxdata);
}
SoEp->MbxHeader.length = htoes((uint16)(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 + MBX_HDR_SET_CNT(cnt); /* 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, uint32 *Osize, uint32 *Isize)
{
int retVal = 0;
int wkc;
int psize;
uint8 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, uint32 *Osize, uint32 *Isize)
{
return ecx_readIDNmap(&ecx_context, slave, Osize, Isize);
}
#endif
Reference in New Issue View Git Blame Copy Permalink