Add functions to configure and send an overlapping IOmap
This commit is contained in:
rtlaka
parent
fc945ba86b
commit
28d82fa8fd
|
|
@ -820,36 +820,10 @@ static int ecx_get_threadcount(void)
|
|||
return thrc;
|
||||
}
|
||||
|
||||
/** Map all PDOs in one group of slaves to IOmap.
|
||||
*
|
||||
* @param[in] context = context struct
|
||||
* @param[out] pIOmap = pointer to IOmap
|
||||
* @param[in] group = group to map, 0 = all groups
|
||||
* @return IOmap size
|
||||
*/
|
||||
int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
|
||||
static void ecx_config_find_mappings(ecx_contextt *context, uint8 group)
|
||||
{
|
||||
uint16 slave, configadr;
|
||||
int BitCount, ByteCount, FMMUsize, FMMUdone;
|
||||
uint16 SMlength, EndAddr;
|
||||
uint8 BitPos;
|
||||
uint8 SMc, FMMUc;
|
||||
uint32 LogAddr = 0;
|
||||
uint32 oLogAddr = 0;
|
||||
uint32 diff;
|
||||
uint16 currentsegment = 0;
|
||||
uint32 segmentsize = 0;
|
||||
int thrn, thrc;
|
||||
|
||||
if ((*(context->slavecount) > 0) && (group < context->maxgroup))
|
||||
{
|
||||
EC_PRINT("ec_config_map_group IOmap:%p group:%d\n", pIOmap, group);
|
||||
LogAddr = context->grouplist[group].logstartaddr;
|
||||
oLogAddr = LogAddr;
|
||||
BitPos = 0;
|
||||
context->grouplist[group].nsegments = 0;
|
||||
context->grouplist[group].outputsWKC = 0;
|
||||
context->grouplist[group].inputsWKC = 0;
|
||||
uint16 slave;
|
||||
|
||||
for (thrn = 0; thrn < EC_MAX_MAPT; thrn++)
|
||||
{
|
||||
|
|
@ -899,30 +873,161 @@ int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
|
|||
ecx_map_sm(context, slave);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* do input mapping of slave and program FMMUs */
|
||||
for (slave = 1; slave <= *(context->slavecount); slave++)
|
||||
static void ecx_config_create_input_mappings(ecx_contextt *context, void *pIOmap,
|
||||
uint8 group, int16 slave, uint32 * LogAddr, uint8 * BitPos)
|
||||
{
|
||||
int BitCount = 0;
|
||||
int ByteCount = 0;
|
||||
int FMMUsize = 0;
|
||||
int FMMUdone = 0;
|
||||
uint8 SMc = 0;
|
||||
uint16 EndAddr;
|
||||
uint16 SMlength;
|
||||
uint16 configadr;
|
||||
uint8 FMMUc;
|
||||
|
||||
EC_PRINT(" =Slave %d, INPUT MAPPING\n", slave);
|
||||
|
||||
configadr = context->slavelist[slave].configadr;
|
||||
FMMUc = context->slavelist[slave].FMMUunused;
|
||||
if (context->slavelist[slave].Obits) /* find free FMMU */
|
||||
{
|
||||
while (context->slavelist[slave].FMMU[FMMUc].LogStart)
|
||||
{
|
||||
FMMUc++;
|
||||
}
|
||||
}
|
||||
/* search for SM that contribute to the input mapping */
|
||||
while ((SMc < (EC_MAXSM - 1)) && (FMMUdone < ((context->slavelist[slave].Ibits + 7) / 8)))
|
||||
{
|
||||
EC_PRINT(" FMMU %d\n", FMMUc);
|
||||
while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4))
|
||||
{
|
||||
SMc++;
|
||||
}
|
||||
EC_PRINT(" SM%d\n", SMc);
|
||||
context->slavelist[slave].FMMU[FMMUc].PhysStart =
|
||||
context->slavelist[slave].SM[SMc].StartAddr;
|
||||
SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
|
||||
ByteCount += SMlength;
|
||||
BitCount += SMlength * 8;
|
||||
EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
|
||||
while ((BitCount < context->slavelist[slave].Ibits) && (SMc < (EC_MAXSM - 1))) /* more SM for input */
|
||||
{
|
||||
SMc++;
|
||||
while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4))
|
||||
{
|
||||
SMc++;
|
||||
}
|
||||
/* if addresses from more SM connect use one FMMU otherwise break up in mutiple FMMU */
|
||||
if (etohs(context->slavelist[slave].SM[SMc].StartAddr) > EndAddr)
|
||||
{
|
||||
break;
|
||||
}
|
||||
EC_PRINT(" SM%d\n", SMc);
|
||||
SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
|
||||
ByteCount += SMlength;
|
||||
BitCount += SMlength * 8;
|
||||
EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
|
||||
}
|
||||
|
||||
/* bit oriented slave */
|
||||
if (!context->slavelist[slave].Ibytes)
|
||||
{
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos;
|
||||
*BitPos += context->slavelist[slave].Ibits - 1;
|
||||
if (*BitPos > 7)
|
||||
{
|
||||
*LogAddr += 1;
|
||||
*BitPos -= 8;
|
||||
}
|
||||
FMMUsize = *LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1;
|
||||
context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos;
|
||||
*BitPos += 1;
|
||||
if (*BitPos > 7)
|
||||
{
|
||||
*LogAddr += 1;
|
||||
*BitPos -= 8;
|
||||
}
|
||||
}
|
||||
/* byte oriented slave */
|
||||
else
|
||||
{
|
||||
if (*BitPos)
|
||||
{
|
||||
*LogAddr += 1;
|
||||
*BitPos = 0;
|
||||
}
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos;
|
||||
*BitPos = 7;
|
||||
FMMUsize = ByteCount;
|
||||
if ((FMMUsize + FMMUdone)> (int)context->slavelist[slave].Ibytes)
|
||||
{
|
||||
FMMUsize = context->slavelist[slave].Ibytes - FMMUdone;
|
||||
}
|
||||
*LogAddr += FMMUsize;
|
||||
context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos;
|
||||
*BitPos = 0;
|
||||
}
|
||||
FMMUdone += FMMUsize;
|
||||
if (context->slavelist[slave].FMMU[FMMUc].LogLength)
|
||||
{
|
||||
context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0;
|
||||
context->slavelist[slave].FMMU[FMMUc].FMMUtype = 1;
|
||||
context->slavelist[slave].FMMU[FMMUc].FMMUactive = 1;
|
||||
/* program FMMU for input */
|
||||
ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc),
|
||||
sizeof(ec_fmmut), &(context->slavelist[slave].FMMU[FMMUc]), EC_TIMEOUTRET3);
|
||||
/* add one for an input FMMU */
|
||||
context->grouplist[group].inputsWKC++;
|
||||
}
|
||||
if (!context->slavelist[slave].inputs)
|
||||
{
|
||||
context->slavelist[slave].inputs =
|
||||
(uint8 *)(pIOmap)+etohl(context->slavelist[slave].FMMU[FMMUc].LogStart);
|
||||
context->slavelist[slave].Istartbit =
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStartbit;
|
||||
EC_PRINT(" Inputs %p startbit %d\n",
|
||||
context->slavelist[slave].inputs,
|
||||
context->slavelist[slave].Istartbit);
|
||||
}
|
||||
FMMUc++;
|
||||
}
|
||||
context->slavelist[slave].FMMUunused = FMMUc;
|
||||
}
|
||||
|
||||
static void ecx_config_create_output_mappings(ecx_contextt *context, void *pIOmap,
|
||||
uint8 group, int16 slave, uint32 * LogAddr, uint8 * BitPos)
|
||||
{
|
||||
int BitCount = 0;
|
||||
int ByteCount = 0;
|
||||
int FMMUsize = 0;
|
||||
int FMMUdone = 0;
|
||||
uint8 SMc = 0;
|
||||
uint16 EndAddr;
|
||||
uint16 SMlength;
|
||||
uint16 configadr;
|
||||
uint8 FMMUc;
|
||||
|
||||
EC_PRINT(" OUTPUT MAPPING\n");
|
||||
|
||||
FMMUc = context->slavelist[slave].FMMUunused;
|
||||
configadr = context->slavelist[slave].configadr;
|
||||
|
||||
if (!group || (group == context->slavelist[slave].group))
|
||||
{
|
||||
FMMUc = context->slavelist[slave].FMMUunused;
|
||||
SMc = 0;
|
||||
BitCount = 0;
|
||||
ByteCount = 0;
|
||||
EndAddr = 0;
|
||||
FMMUsize = 0;
|
||||
FMMUdone = 0;
|
||||
/* create output mapping */
|
||||
if (context->slavelist[slave].Obits)
|
||||
{
|
||||
EC_PRINT(" OUTPUT MAPPING\n");
|
||||
/* search for SM that contribute to the output mapping */
|
||||
while ((SMc < (EC_MAXSM - 1)) && (FMMUdone < ((context->slavelist[slave].Obits + 7) / 8)))
|
||||
{
|
||||
EC_PRINT(" FMMU %d\n", FMMUc);
|
||||
while ( (SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3)) SMc++;
|
||||
while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3))
|
||||
{
|
||||
SMc++;
|
||||
}
|
||||
EC_PRINT(" SM%d\n", SMc);
|
||||
context->slavelist[slave].FMMU[FMMUc].PhysStart =
|
||||
context->slavelist[slave].SM[SMc].StartAddr;
|
||||
|
|
@ -933,7 +1038,10 @@ int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
|
|||
while ((BitCount < context->slavelist[slave].Obits) && (SMc < (EC_MAXSM - 1))) /* more SM for output */
|
||||
{
|
||||
SMc++;
|
||||
while ( (SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3)) SMc++;
|
||||
while ((SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 3))
|
||||
{
|
||||
SMc++;
|
||||
}
|
||||
/* if addresses from more SM connect use one FMMU otherwise break up in mutiple FMMU */
|
||||
if (etohs(context->slavelist[slave].SM[SMc].StartAddr) > EndAddr)
|
||||
{
|
||||
|
|
@ -949,44 +1057,44 @@ int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
|
|||
/* bit oriented slave */
|
||||
if (!context->slavelist[slave].Obytes)
|
||||
{
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(LogAddr);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStartbit = BitPos;
|
||||
BitPos += context->slavelist[slave].Obits - 1;
|
||||
if (BitPos > 7)
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos;
|
||||
*BitPos += context->slavelist[slave].Obits - 1;
|
||||
if (*BitPos > 7)
|
||||
{
|
||||
LogAddr++;
|
||||
BitPos -= 8;
|
||||
*LogAddr += 1;
|
||||
*BitPos -= 8;
|
||||
}
|
||||
FMMUsize = LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1;
|
||||
FMMUsize = *LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1;
|
||||
context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogEndbit = BitPos;
|
||||
BitPos ++;
|
||||
if (BitPos > 7)
|
||||
context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos;
|
||||
*BitPos += 1;
|
||||
if (*BitPos > 7)
|
||||
{
|
||||
LogAddr++;
|
||||
BitPos -= 8;
|
||||
*LogAddr += 1;
|
||||
*BitPos -= 8;
|
||||
}
|
||||
}
|
||||
/* byte oriented slave */
|
||||
else
|
||||
{
|
||||
if (BitPos)
|
||||
if (*BitPos)
|
||||
{
|
||||
LogAddr++;
|
||||
BitPos = 0;
|
||||
*LogAddr += 1;
|
||||
*BitPos = 0;
|
||||
}
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(LogAddr);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStartbit = BitPos;
|
||||
BitPos = 7;
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(*LogAddr);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStartbit = *BitPos;
|
||||
*BitPos = 7;
|
||||
FMMUsize = ByteCount;
|
||||
if ((FMMUsize + FMMUdone)> (int)context->slavelist[slave].Obytes)
|
||||
{
|
||||
FMMUsize = context->slavelist[slave].Obytes - FMMUdone;
|
||||
}
|
||||
LogAddr += FMMUsize;
|
||||
*LogAddr += FMMUsize;
|
||||
context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogEndbit = BitPos;
|
||||
BitPos = 0;
|
||||
context->slavelist[slave].FMMU[FMMUc].LogEndbit = *BitPos;
|
||||
*BitPos = 0;
|
||||
}
|
||||
FMMUdone += FMMUsize;
|
||||
context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0;
|
||||
|
|
@ -1010,6 +1118,51 @@ int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
|
|||
FMMUc++;
|
||||
}
|
||||
context->slavelist[slave].FMMUunused = FMMUc;
|
||||
}
|
||||
|
||||
/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
|
||||
* in sequential order (legacy SOEM way).
|
||||
*
|
||||
*
|
||||
* @param[in] context = context struct
|
||||
* @param[out] pIOmap = pointer to IOmap
|
||||
* @param[in] group = group to map, 0 = all groups
|
||||
* @return IOmap size
|
||||
*/
|
||||
int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
|
||||
{
|
||||
uint16 slave, configadr;
|
||||
uint8 BitPos;
|
||||
uint32 LogAddr = 0;
|
||||
uint32 oLogAddr = 0;
|
||||
uint32 diff;
|
||||
uint16 currentsegment = 0;
|
||||
uint32 segmentsize = 0;
|
||||
|
||||
if ((*(context->slavecount) > 0) && (group < context->maxgroup))
|
||||
{
|
||||
EC_PRINT("ec_config_map_group IOmap:%p group:%d\n", pIOmap, group);
|
||||
LogAddr = context->grouplist[group].logstartaddr;
|
||||
oLogAddr = LogAddr;
|
||||
BitPos = 0;
|
||||
context->grouplist[group].nsegments = 0;
|
||||
context->grouplist[group].outputsWKC = 0;
|
||||
context->grouplist[group].inputsWKC = 0;
|
||||
|
||||
/* Find mappings and program syncmanagers */
|
||||
ecx_config_find_mappings(context, group);
|
||||
|
||||
/* do output mapping of slave and program FMMUs */
|
||||
for (slave = 1; slave <= *(context->slavecount); slave++)
|
||||
{
|
||||
configadr = context->slavelist[slave].configadr;
|
||||
|
||||
if (!group || (group == context->slavelist[slave].group))
|
||||
{
|
||||
/* create output mapping */
|
||||
if (context->slavelist[slave].Obits)
|
||||
{
|
||||
ecx_config_create_output_mappings (context, pIOmap, group, slave, &LogAddr, &BitPos);
|
||||
diff = LogAddr - oLogAddr;
|
||||
oLogAddr = LogAddr;
|
||||
if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
|
||||
|
|
@ -1064,116 +1217,11 @@ int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
|
|||
configadr = context->slavelist[slave].configadr;
|
||||
if (!group || (group == context->slavelist[slave].group))
|
||||
{
|
||||
FMMUc = context->slavelist[slave].FMMUunused;
|
||||
if (context->slavelist[slave].Obits) /* find free FMMU */
|
||||
{
|
||||
while ( context->slavelist[slave].FMMU[FMMUc].LogStart ) FMMUc++;
|
||||
}
|
||||
SMc = 0;
|
||||
BitCount = 0;
|
||||
ByteCount = 0;
|
||||
EndAddr = 0;
|
||||
FMMUsize = 0;
|
||||
FMMUdone = 0;
|
||||
/* create input mapping */
|
||||
if (context->slavelist[slave].Ibits)
|
||||
{
|
||||
EC_PRINT(" =Slave %d, INPUT MAPPING\n", slave);
|
||||
/* search for SM that contribute to the input mapping */
|
||||
while ( (SMc < (EC_MAXSM - 1)) && (FMMUdone < ((context->slavelist[slave].Ibits + 7) / 8)))
|
||||
{
|
||||
EC_PRINT(" FMMU %d\n", FMMUc);
|
||||
while ( (SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4)) SMc++;
|
||||
EC_PRINT(" SM%d\n", SMc);
|
||||
context->slavelist[slave].FMMU[FMMUc].PhysStart =
|
||||
context->slavelist[slave].SM[SMc].StartAddr;
|
||||
SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
|
||||
ByteCount += SMlength;
|
||||
BitCount += SMlength * 8;
|
||||
EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
|
||||
while ( (BitCount < context->slavelist[slave].Ibits) && (SMc < (EC_MAXSM - 1)) ) /* more SM for input */
|
||||
{
|
||||
SMc++;
|
||||
while ( (SMc < (EC_MAXSM - 1)) && (context->slavelist[slave].SMtype[SMc] != 4)) SMc++;
|
||||
/* if addresses from more SM connect use one FMMU otherwise break up in mutiple FMMU */
|
||||
if ( etohs(context->slavelist[slave].SM[SMc].StartAddr) > EndAddr )
|
||||
{
|
||||
break;
|
||||
}
|
||||
EC_PRINT(" SM%d\n", SMc);
|
||||
SMlength = etohs(context->slavelist[slave].SM[SMc].SMlength);
|
||||
ByteCount += SMlength;
|
||||
BitCount += SMlength * 8;
|
||||
EndAddr = etohs(context->slavelist[slave].SM[SMc].StartAddr) + SMlength;
|
||||
}
|
||||
|
||||
/* bit oriented slave */
|
||||
if (!context->slavelist[slave].Ibytes)
|
||||
{
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(LogAddr);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStartbit = BitPos;
|
||||
BitPos += context->slavelist[slave].Ibits - 1;
|
||||
if (BitPos > 7)
|
||||
{
|
||||
LogAddr++;
|
||||
BitPos -= 8;
|
||||
}
|
||||
FMMUsize = LogAddr - etohl(context->slavelist[slave].FMMU[FMMUc].LogStart) + 1;
|
||||
context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogEndbit = BitPos;
|
||||
BitPos ++;
|
||||
if (BitPos > 7)
|
||||
{
|
||||
LogAddr++;
|
||||
BitPos -= 8;
|
||||
}
|
||||
}
|
||||
/* byte oriented slave */
|
||||
else
|
||||
{
|
||||
if (BitPos)
|
||||
{
|
||||
LogAddr++;
|
||||
BitPos = 0;
|
||||
}
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStart = htoel(LogAddr);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStartbit = BitPos;
|
||||
BitPos = 7;
|
||||
FMMUsize = ByteCount;
|
||||
if ((FMMUsize + FMMUdone)> (int)context->slavelist[slave].Ibytes)
|
||||
{
|
||||
FMMUsize = context->slavelist[slave].Ibytes - FMMUdone;
|
||||
}
|
||||
LogAddr += FMMUsize;
|
||||
context->slavelist[slave].FMMU[FMMUc].LogLength = htoes(FMMUsize);
|
||||
context->slavelist[slave].FMMU[FMMUc].LogEndbit = BitPos;
|
||||
BitPos = 0;
|
||||
}
|
||||
FMMUdone += FMMUsize;
|
||||
if (context->slavelist[slave].FMMU[FMMUc].LogLength)
|
||||
{
|
||||
context->slavelist[slave].FMMU[FMMUc].PhysStartBit = 0;
|
||||
context->slavelist[slave].FMMU[FMMUc].FMMUtype = 1;
|
||||
context->slavelist[slave].FMMU[FMMUc].FMMUactive = 1;
|
||||
/* program FMMU for input */
|
||||
ecx_FPWR(context->port, configadr, ECT_REG_FMMU0 + (sizeof(ec_fmmut) * FMMUc),
|
||||
sizeof(ec_fmmut), &(context->slavelist[slave].FMMU[FMMUc]), EC_TIMEOUTRET3);
|
||||
/* add one for an input FMMU */
|
||||
context->grouplist[group].inputsWKC++;
|
||||
}
|
||||
if (!context->slavelist[slave].inputs)
|
||||
{
|
||||
context->slavelist[slave].inputs =
|
||||
(uint8 *)(pIOmap) + etohl(context->slavelist[slave].FMMU[FMMUc].LogStart);
|
||||
context->slavelist[slave].Istartbit =
|
||||
context->slavelist[slave].FMMU[FMMUc].LogStartbit;
|
||||
EC_PRINT(" Inputs %p startbit %d\n",
|
||||
context->slavelist[slave].inputs,
|
||||
context->slavelist[slave].Istartbit);
|
||||
}
|
||||
FMMUc++;
|
||||
}
|
||||
context->slavelist[slave].FMMUunused = FMMUc;
|
||||
ecx_config_create_input_mappings(context, pIOmap, group, slave, &LogAddr, &BitPos);
|
||||
diff = LogAddr - oLogAddr;
|
||||
oLogAddr = LogAddr;
|
||||
if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
|
||||
|
|
@ -1238,6 +1286,136 @@ int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
|
|||
return 0;
|
||||
}
|
||||
|
||||
/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
|
||||
* overlapping. NOTE: Must use this for TI ESC when using LRW.
|
||||
*
|
||||
* @param[in] context = context struct
|
||||
* @param[out] pIOmap = pointer to IOmap
|
||||
* @param[in] group = group to map, 0 = all groups
|
||||
* @return IOmap size
|
||||
*/
|
||||
int ecx_config_overlap_map_group(ecx_contextt *context, void *pIOmap, uint8 group)
|
||||
{
|
||||
uint16 slave, configadr;
|
||||
uint8 BitPos;
|
||||
uint32 mLogAddr = 0;
|
||||
uint32 siLogAddr = 0;
|
||||
uint32 soLogAddr = 0;
|
||||
uint32 tempLogAddr;
|
||||
uint32 diff;
|
||||
uint16 currentsegment = 0;
|
||||
uint32 segmentsize = 0;
|
||||
|
||||
if ((*(context->slavecount) > 0) && (group < context->maxgroup))
|
||||
{
|
||||
EC_PRINT("ec_config_map_group IOmap:%p group:%d\n", pIOmap, group);
|
||||
mLogAddr = context->grouplist[group].logstartaddr;
|
||||
siLogAddr = mLogAddr;
|
||||
soLogAddr = mLogAddr;
|
||||
BitPos = 0;
|
||||
context->grouplist[group].nsegments = 0;
|
||||
context->grouplist[group].outputsWKC = 0;
|
||||
context->grouplist[group].inputsWKC = 0;
|
||||
|
||||
/* Find mappings and program syncmanagers */
|
||||
ecx_config_find_mappings(context, group);
|
||||
|
||||
/* do IO mapping of slave and program FMMUs */
|
||||
for (slave = 1; slave <= *(context->slavecount); slave++)
|
||||
{
|
||||
configadr = context->slavelist[slave].configadr;
|
||||
siLogAddr = soLogAddr = mLogAddr;
|
||||
|
||||
if (!group || (group == context->slavelist[slave].group))
|
||||
{
|
||||
/* create output mapping */
|
||||
if (context->slavelist[slave].Obits)
|
||||
{
|
||||
|
||||
ecx_config_create_output_mappings(context, pIOmap, group,
|
||||
slave, &soLogAddr, &BitPos);
|
||||
if (BitPos)
|
||||
{
|
||||
soLogAddr++;
|
||||
BitPos = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* create input mapping */
|
||||
if (context->slavelist[slave].Ibits)
|
||||
{
|
||||
ecx_config_create_input_mappings(context, pIOmap, group,
|
||||
slave, &siLogAddr, &BitPos);
|
||||
if (BitPos)
|
||||
{
|
||||
siLogAddr++;
|
||||
BitPos = 0;
|
||||
}
|
||||
}
|
||||
|
||||
tempLogAddr = (siLogAddr > soLogAddr) ? siLogAddr : soLogAddr;
|
||||
diff = tempLogAddr - mLogAddr;
|
||||
mLogAddr = tempLogAddr;
|
||||
|
||||
if ((segmentsize + diff) > (EC_MAXLRWDATA - EC_FIRSTDCDATAGRAM))
|
||||
{
|
||||
context->grouplist[group].IOsegment[currentsegment] = segmentsize;
|
||||
if (currentsegment < (EC_MAXIOSEGMENTS - 1))
|
||||
{
|
||||
currentsegment++;
|
||||
segmentsize = diff;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
segmentsize += diff;
|
||||
}
|
||||
|
||||
ecx_eeprom2pdi(context, slave); /* set Eeprom control to PDI */
|
||||
ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_SAFE_OP), EC_TIMEOUTRET3); /* set safeop status */
|
||||
|
||||
if (context->slavelist[slave].blockLRW)
|
||||
{
|
||||
context->grouplist[group].blockLRW++;
|
||||
}
|
||||
context->grouplist[group].Ebuscurrent += context->slavelist[slave].Ebuscurrent;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
context->grouplist[group].IOsegment[currentsegment] = segmentsize;
|
||||
context->grouplist[group].nsegments = currentsegment + 1;
|
||||
context->grouplist[group].Isegment = 0;
|
||||
context->grouplist[group].Ioffset = 0;
|
||||
|
||||
context->grouplist[group].Obytes = soLogAddr;
|
||||
context->grouplist[group].Ibytes = siLogAddr;
|
||||
context->grouplist[group].outputs = pIOmap;
|
||||
context->grouplist[group].inputs = (uint8 *)pIOmap + context->grouplist[group].Obytes;
|
||||
|
||||
/* Move calculated inputs with OBytes offset*/
|
||||
for (slave = 1; slave <= *(context->slavecount); slave++)
|
||||
{
|
||||
context->slavelist[slave].inputs += context->grouplist[group].Obytes;
|
||||
}
|
||||
|
||||
if (!group)
|
||||
{
|
||||
context->slavelist[0].outputs = pIOmap;
|
||||
context->slavelist[0].Obytes = soLogAddr; /* store output bytes in master record */
|
||||
context->slavelist[0].inputs = (uint8 *)pIOmap + context->slavelist[0].Obytes;
|
||||
context->slavelist[0].Ibytes = siLogAddr;
|
||||
}
|
||||
|
||||
EC_PRINT("IOmapSize %d\n", context->grouplist[group].Obytes + context->grouplist[group].Ibytes);
|
||||
|
||||
return (context->grouplist[group].Obytes + context->grouplist[group].Ibytes);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/** Recover slave.
|
||||
*
|
||||
* @param[in] context = context struct
|
||||
|
|
@ -1368,7 +1546,8 @@ int ec_config_init(uint8 usetable)
|
|||
return ecx_config_init(&ecx_context, usetable);
|
||||
}
|
||||
|
||||
/** Map all PDOs in one group of slaves to IOmap.
|
||||
/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
|
||||
* in sequential order (legacy SOEM way).
|
||||
*
|
||||
* @param[out] pIOmap = pointer to IOmap
|
||||
* @param[in] group = group to map, 0 = all groups
|
||||
|
|
@ -1380,7 +1559,21 @@ int ec_config_map_group(void *pIOmap, uint8 group)
|
|||
return ecx_config_map_group(&ecx_context, pIOmap, group);
|
||||
}
|
||||
|
||||
/** Map all PDOs from slaves to IOmap.
|
||||
/** Map all PDOs in one group of slaves to IOmap with Outputs/Inputs
|
||||
* overlapping. NOTE: Must use this for TI ESC when using LRW.
|
||||
*
|
||||
* @param[out] pIOmap = pointer to IOmap
|
||||
* @param[in] group = group to map, 0 = all groups
|
||||
* @return IOmap size
|
||||
* @see ecx_config_overlap_map_group
|
||||
*/
|
||||
int ec_config_overlap_map_group(void *pIOmap, uint8 group)
|
||||
{
|
||||
return ecx_config_overlap_map_group(&ecx_context, pIOmap, group);
|
||||
}
|
||||
|
||||
/** Map all PDOs from slaves to IOmap with Outputs/Inputs
|
||||
* in sequential order (legacy SOEM way).
|
||||
*
|
||||
* @param[out] pIOmap = pointer to IOmap
|
||||
* @return IOmap size
|
||||
|
|
@ -1390,6 +1583,17 @@ int ec_config_map(void *pIOmap)
|
|||
return ec_config_map_group(pIOmap, 0);
|
||||
}
|
||||
|
||||
/** Map all PDOs from slaves to IOmap with Outputs/Inputs
|
||||
* overlapping. NOTE: Must use this for TI ESC when using LRW.
|
||||
*
|
||||
* @param[out] pIOmap = pointer to IOmap
|
||||
* @return IOmap size
|
||||
*/
|
||||
int ec_config_overlap_map(void *pIOmap)
|
||||
{
|
||||
return ec_config_overlap_map_group(pIOmap, 0);
|
||||
}
|
||||
|
||||
/** Enumerate / map and init all slaves.
|
||||
*
|
||||
* @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise
|
||||
|
|
@ -1407,6 +1611,23 @@ int ec_config(uint8 usetable, void *pIOmap)
|
|||
return wkc;
|
||||
}
|
||||
|
||||
/** Enumerate / map and init all slaves.
|
||||
*
|
||||
* @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise
|
||||
* @param[out] pIOmap = pointer to IOmap
|
||||
* @return Workcounter of slave discover datagram = number of slaves found
|
||||
*/
|
||||
int ec_config_overlap(uint8 usetable, void *pIOmap)
|
||||
{
|
||||
int wkc;
|
||||
wkc = ec_config_init(usetable);
|
||||
if (wkc)
|
||||
{
|
||||
ec_config_overlap_map(pIOmap);
|
||||
}
|
||||
return wkc;
|
||||
}
|
||||
|
||||
/** Recover slave.
|
||||
*
|
||||
* @param[in] slave = slave to recover
|
||||
|
|
|
|||
|
|
@ -22,14 +22,18 @@ extern "C"
|
|||
#ifdef EC_VER1
|
||||
int ec_config_init(uint8 usetable);
|
||||
int ec_config_map(void *pIOmap);
|
||||
int ec_config_overlap_map(void *pIOmap);
|
||||
int ec_config_map_group(void *pIOmap, uint8 group);
|
||||
int ec_config_overlap_map_group(void *pIOmap, uint8 group);
|
||||
int ec_config(uint8 usetable, void *pIOmap);
|
||||
int ec_config_overlap(uint8 usetable, void *pIOmap);
|
||||
int ec_recover_slave(uint16 slave, int timeout);
|
||||
int ec_reconfig_slave(uint16 slave, int timeout);
|
||||
#endif
|
||||
|
||||
int ecx_config_init(ecx_contextt *context, uint8 usetable);
|
||||
int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group);
|
||||
int ecx_config_overlap_map_group(ecx_contextt *context, void *pIOmap, uint8 group);
|
||||
int ecx_recover_slave(ecx_contextt *context, uint16 slave, int timeout);
|
||||
int ecx_reconfig_slave(ecx_contextt *context, uint16 slave, int timeout);
|
||||
|
||||
|
|
|
|||
|
|
@ -1671,7 +1671,7 @@ static void ecx_clearindex(ecx_contextt *context) {
|
|||
* @param[in] group = group number
|
||||
* @return >0 if processdata is transmitted.
|
||||
*/
|
||||
int ecx_send_processdata_group(ecx_contextt *context, uint8 group)
|
||||
static int ecx_main_send_processdata(ecx_contextt *context, uint8 group, boolean use_overlap_io)
|
||||
{
|
||||
uint32 LogAdr;
|
||||
uint16 w1, w2;
|
||||
|
|
@ -1681,13 +1681,29 @@ int ecx_send_processdata_group(ecx_contextt *context, uint8 group)
|
|||
uint8* data;
|
||||
boolean first=FALSE;
|
||||
uint16 currentsegment = 0;
|
||||
uint32 iomapinputoffset;
|
||||
|
||||
wkc = 0;
|
||||
if(context->grouplist[group].hasdc)
|
||||
{
|
||||
first = TRUE;
|
||||
}
|
||||
|
||||
/* For overlapping IO map use the biggest */
|
||||
if(use_overlap_io == TRUE)
|
||||
{
|
||||
/* For overlap IOmap make the frame EQ big to biggest part */
|
||||
length = (context->grouplist[group].Obytes > context->grouplist[group].Ibytes) ?
|
||||
context->grouplist[group].Obytes : context->grouplist[group].Ibytes;
|
||||
/* Save the offset used to compensate where to save inputs when frame returns */
|
||||
iomapinputoffset = context->grouplist[group].Obytes;
|
||||
}
|
||||
else
|
||||
{
|
||||
length = context->grouplist[group].Obytes + context->grouplist[group].Ibytes;
|
||||
iomapinputoffset = 0;
|
||||
}
|
||||
|
||||
LogAdr = context->grouplist[group].logstartaddr;
|
||||
if(length)
|
||||
{
|
||||
|
|
@ -1786,6 +1802,8 @@ int ecx_send_processdata_group(ecx_contextt *context, uint8 group)
|
|||
else
|
||||
{
|
||||
data = context->grouplist[group].inputs;
|
||||
/* Clear offset, don't compensate for overlapping IOmap if we only got inputs */
|
||||
iomapinputoffset = 0;
|
||||
}
|
||||
/* segment transfer if needed */
|
||||
do
|
||||
|
|
@ -1807,8 +1825,12 @@ int ecx_send_processdata_group(ecx_contextt *context, uint8 group)
|
|||
}
|
||||
/* send frame */
|
||||
ecx_outframe_red(context->port, idx);
|
||||
/* push index and data pointer on stack */
|
||||
ecx_pushindex(context, idx, data, sublength);
|
||||
/* push index and data pointer on stack.
|
||||
* the iomapinputoffset compensate for where the inputs are stored
|
||||
* in the IOmap if we use an overlapping IOmap. If a regular IOmap
|
||||
* is used it should always be 0.
|
||||
*/
|
||||
ecx_pushindex(context, idx, (data + iomapinputoffset), sublength);
|
||||
length -= sublength;
|
||||
LogAdr += sublength;
|
||||
data += sublength;
|
||||
|
|
@ -1819,6 +1841,40 @@ int ecx_send_processdata_group(ecx_contextt *context, uint8 group)
|
|||
return wkc;
|
||||
}
|
||||
|
||||
/** Transmit processdata to slaves.
|
||||
* Uses LRW, or LRD/LWR if LRW is not allowed (blockLRW).
|
||||
* Both the input and output processdata are transmitted in the overlapped IOmap.
|
||||
* The outputs with the actual data, the inputs replace the output data in the
|
||||
* returning frame. The inputs are gathered with the receive processdata function.
|
||||
* In contrast to the base LRW function this function is non-blocking.
|
||||
* If the processdata does not fit in one datagram, multiple are used.
|
||||
* In order to recombine the slave response, a stack is used.
|
||||
* @param[in] context = context struct
|
||||
* @param[in] group = group number
|
||||
* @return >0 if processdata is transmitted.
|
||||
*/
|
||||
int ecx_send_overlap_processdata_group(ecx_contextt *context, uint8 group)
|
||||
{
|
||||
return ecx_main_send_processdata(context, group, TRUE);
|
||||
}
|
||||
|
||||
/** Transmit processdata to slaves.
|
||||
* Uses LRW, or LRD/LWR if LRW is not allowed (blockLRW).
|
||||
* Both the input and output processdata are transmitted.
|
||||
* The outputs with the actual data, the inputs have a placeholder.
|
||||
* The inputs are gathered with the receive processdata function.
|
||||
* In contrast to the base LRW function this function is non-blocking.
|
||||
* If the processdata does not fit in one datagram, multiple are used.
|
||||
* In order to recombine the slave response, a stack is used.
|
||||
* @param[in] context = context struct
|
||||
* @param[in] group = group number
|
||||
* @return >0 if processdata is transmitted.
|
||||
*/
|
||||
int ecx_send_processdata_group(ecx_contextt *context, uint8 group)
|
||||
{
|
||||
return ecx_main_send_processdata(context, group, FALSE);
|
||||
}
|
||||
|
||||
/** Receive processdata from slaves.
|
||||
* Second part from ec_send_processdata().
|
||||
* Received datagrams are recombined with the processdata with help from the stack.
|
||||
|
|
@ -1911,6 +1967,11 @@ int ecx_send_processdata(ecx_contextt *context)
|
|||
return ecx_send_processdata_group(context, 0);
|
||||
}
|
||||
|
||||
int ecx_send_overlap_processdata(ecx_contextt *context)
|
||||
{
|
||||
return ecx_send_overlap_processdata_group(context, 0);
|
||||
}
|
||||
|
||||
int ecx_receive_processdata(ecx_contextt *context, int timeout)
|
||||
{
|
||||
return ecx_receive_processdata_group(context, 0, timeout);
|
||||
|
|
@ -2265,6 +2326,24 @@ int ec_send_processdata_group(uint8 group)
|
|||
return ecx_send_processdata_group (&ecx_context, group);
|
||||
}
|
||||
|
||||
/** Transmit processdata to slaves.
|
||||
* Uses LRW, or LRD/LWR if LRW is not allowed (blockLRW).
|
||||
* Both the input and output processdata are transmitted in the overlapped IOmap.
|
||||
* The outputs with the actual data, the inputs replace the output data in the
|
||||
* returning frame. The inputs are gathered with the receive processdata function.
|
||||
* In contrast to the base LRW function this function is non-blocking.
|
||||
* If the processdata does not fit in one datagram, multiple are used.
|
||||
* In order to recombine the slave response, a stack is used.
|
||||
* @param[in] context = context struct
|
||||
* @param[in] group = group number
|
||||
* @return >0 if processdata is transmitted.
|
||||
* @see ecx_send_overlap_processdata_group
|
||||
*/
|
||||
int ec_send_overlap_processdata_group(uint8 group)
|
||||
{
|
||||
return ecx_send_overlap_processdata_group(&ecx_context, group);
|
||||
}
|
||||
|
||||
/** Receive processdata from slaves.
|
||||
* Second part from ec_send_processdata().
|
||||
* Received datagrams are recombined with the processdata with help from the stack.
|
||||
|
|
@ -2284,6 +2363,11 @@ int ec_send_processdata(void)
|
|||
return ec_send_processdata_group(0);
|
||||
}
|
||||
|
||||
int ec_send_overlap_processdata(void)
|
||||
{
|
||||
return ec_send_overlap_processdata_group(0);
|
||||
}
|
||||
|
||||
int ec_receive_processdata(int timeout)
|
||||
{
|
||||
return ec_receive_processdata_group(0, timeout);
|
||||
|
|
|
|||
|
|
@ -467,8 +467,10 @@ int ec_writeeepromFP(uint16 configadr, uint16 eeproma, uint16 data, int timeout)
|
|||
void ec_readeeprom1(uint16 slave, uint16 eeproma);
|
||||
uint32 ec_readeeprom2(uint16 slave, int timeout);
|
||||
int ec_send_processdata_group(uint8 group);
|
||||
int ec_send_overlap_processdata_group(uint8 group);
|
||||
int ec_receive_processdata_group(uint8 group, int timeout);
|
||||
int ec_send_processdata(void);
|
||||
int ec_send_overlap_processdata(void);
|
||||
int ec_receive_processdata(int timeout);
|
||||
#endif
|
||||
|
||||
|
|
@ -507,9 +509,10 @@ uint64 ecx_readeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma,
|
|||
int ecx_writeeepromFP(ecx_contextt *context, uint16 configadr, uint16 eeproma, uint16 data, int timeout);
|
||||
void ecx_readeeprom1(ecx_contextt *context, uint16 slave, uint16 eeproma);
|
||||
uint32 ecx_readeeprom2(ecx_contextt *context, uint16 slave, int timeout);
|
||||
int ecx_send_processdata_group(ecx_contextt *context, uint8 group);
|
||||
int ecx_send_overlap_processdata_group(ecx_contextt *context, uint8 group);
|
||||
int ecx_receive_processdata_group(ecx_contextt *context, uint8 group, int timeout);
|
||||
int ecx_send_processdata(ecx_contextt *context);
|
||||
int ecx_send_overlap_processdata(ecx_contextt *context);
|
||||
int ecx_receive_processdata(ecx_contextt *context, int timeout);
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
|
|
|||
Loading…
Reference in New Issue