bool ConnManager::configure() {
if (!ec_init("rteth0")) {
log(RTT::Error) << "[ECatConn] ConnManager: ec_init() failed!"
<< RTT::endlog();
return false;
}
ec_config_init(FALSE);
log(RTT::Info) << "[ECatConn] " << ec_slavecount <<
" EtherCAT slaves identified." << RTT::endlog();
if (ec_slavecount < 1) {
log(RTT::Error) <<
"[ECatConn] Failed to identify any slaves! Failing to init."
<< RTT::endlog();
return false;
}
ec_configdc();
ec_config_map(IOmap);
// Wait for SAFE-OP
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
return true;
}
/** 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(uint8 usetable, void *pIOmap)
{
int wkc;
wkc = ec_config_init(usetable);
if (wkc)
ec_config_map(pIOmap);
return wkc;
}
void simpletest(char *ifname)
{
int i, j, oloop, iloop, wkc_count, chk;
needlf = FALSE;
inOP = FALSE;
printf("Starting simple test\n");
/* initialise SOEM, bind socket to ifname */
if (ec_init(ifname))
{
printf("ec_init on %s succeeded.\n",ifname);
/* find and auto-config slaves */
if ( ec_config_init(FALSE) > 0 )
{
printf("%d slaves found and configured.\n",ec_slavecount);
ec_config_map(&IOmap);
ec_configdc();
printf("Slaves mapped, state to SAFE_OP.\n");
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
oloop = ec_slave[0].Obytes;
if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1;
if (oloop > 8) oloop = 8;
iloop = ec_slave[0].Ibytes;
if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1;
if (iloop > 8) iloop = 8;
printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]);
printf("Request operational state for all slaves\n");
expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
printf("Calculated workcounter %d\n", expectedWKC);
ec_slave[0].state = EC_STATE_OPERATIONAL;
/* send one valid process data to make outputs in slaves happy*/
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
/* request OP state for all slaves */
ec_writestate(0);
chk = 40;
/* wait for all slaves to reach OP state */
do
{
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
}
while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
if (ec_slave[0].state == EC_STATE_OPERATIONAL )
{
printf("Operational state reached for all slaves.\n");
wkc_count = 0;
inOP = TRUE;
/* cyclic loop */
for(i = 1; i <= 10000; i++)
{
ec_send_processdata();
wkc = ec_receive_processdata(EC_TIMEOUTRET);
if(wkc >= expectedWKC)
{
printf("Processdata cycle %4d, WKC %d , O:", i, wkc);
for(j = 0 ; j < oloop; j++)
{
printf(" %2.2x", *(ec_slave[0].outputs + j));
}
printf(" I:");
for(j = 0 ; j < iloop; j++)
{
printf(" %2.2x", *(ec_slave[0].inputs + j));
}
printf(" T:%lld\r",ec_DCtime);
needlf = TRUE;
}
usleep(5000);
}
inOP = FALSE;
}
else
{
printf("Not all slaves reached operational state.\n");
ec_readstate();
for(i = 1; i<=ec_slavecount ; i++)
{
if(ec_slave[i].state != EC_STATE_OPERATIONAL)
{
printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
}
}
}
printf("\nRequest init state for all slaves\n");
ec_slave[0].state = EC_STATE_INIT;
/* request INIT state for all slaves */
ec_writestate(0);
}
else
{
printf("No slaves found!\n");
}
printf("End simple test, close socket\n");
/* stop SOEM, close socket */
ec_close();
}
else
{
printf("No socket connection on %s\nExcecute as root\n",ifname);
}
}
void simpletest(char *ifname)
{
int i, j, oloop, iloop, wkc_count, chk, slc;
UINT mmResult;
needlf = FALSE;
inOP = FALSE;
printf("Starting simple test\n");
/* initialise SOEM, bind socket to ifname */
if (ec_init(ifname))
{
printf("ec_init on %s succeeded.\n",ifname);
/* find and auto-config slaves */
if ( ec_config_init(FALSE) > 0 )
{
printf("%d slaves found and configured.\n",ec_slavecount);
if((ec_slavecount > 1))
{
for(slc = 1; slc <= ec_slavecount; slc++)
{
// beckhoff EL7031, using ec_slave[].name is not very reliable
if((ec_slave[slc].eep_man == 0x00000002) && (ec_slave[slc].eep_id == 0x1b773052))
{
printf("Found %s at position %d\n", ec_slave[slc].name, slc);
// link slave specific setup to preop->safeop hook
ec_slave[slc].PO2SOconfig = &EL7031setup;
}
// Copley Controls EAP, using ec_slave[].name is not very reliable
if((ec_slave[slc].eep_man == 0x000000ab) && (ec_slave[slc].eep_id == 0x00000380))
{
printf("Found %s at position %d\n", ec_slave[slc].name, slc);
// link slave specific setup to preop->safeop hook
ec_slave[slc].PO2SOconfig = &AEPsetup;
}
}
}
ec_config_map(&IOmap);
ec_configdc();
printf("Slaves mapped, state to SAFE_OP.\n");
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
oloop = ec_slave[0].Obytes;
if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1;
if (oloop > 8) oloop = 8;
iloop = ec_slave[0].Ibytes;
if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1;
if (iloop > 8) iloop = 8;
printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]);
printf("Request operational state for all slaves\n");
expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
printf("Calculated workcounter %d\n", expectedWKC);
ec_slave[0].state = EC_STATE_OPERATIONAL;
/* send one valid process data to make outputs in slaves happy*/
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
/* start RT thread as periodic MM timer */
mmResult = timeSetEvent(1, 0, RTthread, 0, TIME_PERIODIC);
/* request OP state for all slaves */
ec_writestate(0);
chk = 40;
/* wait for all slaves to reach OP state */
do
{
ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
}
while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
if (ec_slave[0].state == EC_STATE_OPERATIONAL )
{
printf("Operational state reached for all slaves.\n");
wkc_count = 0;
inOP = TRUE;
/* cyclic loop, reads data from RT thread */
for(i = 1; i <= 500; i++)
{
if(wkc >= expectedWKC)
{
printf("Processdata cycle %4d, WKC %d , O:", rtcnt, wkc);
for(j = 0 ; j < oloop; j++)
{
printf(" %2.2x", *(ec_slave[0].outputs + j));
}
printf(" I:");
for(j = 0 ; j < iloop; j++)
{
printf(" %2.2x", *(ec_slave[0].inputs + j));
}
printf(" T:%lld\r",ec_DCtime);
needlf = TRUE;
}
osal_usleep(50000);
}
inOP = FALSE;
}
else
{
printf("Not all slaves reached operational state.\n");
ec_readstate();
for(i = 1; i<=ec_slavecount ; i++)
{
if(ec_slave[i].state != EC_STATE_OPERATIONAL)
{
printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
}
}
}
/* stop RT thread */
timeKillEvent(mmResult);
printf("\nRequest init state for all slaves\n");
ec_slave[0].state = EC_STATE_INIT;
/* request INIT state for all slaves */
ec_writestate(0);
}
else
{
printf("No slaves found!\n");
}
printf("End simple test, close socket\n");
/* stop SOEM, close socket */
ec_close();
}
else
{
printf("No socket connection on %s\nExcecute as root\n",ifname);
}
}
void eboxtest(char *ifname)
{
int cnt, i;
printf("Starting E/BOX test\n");
/* initialise SOEM, bind socket to ifname */
if (ec_init(ifname))
{
printf("ec_init on %s succeeded.\n",ifname);
/* find and auto-config slaves */
if ( ec_config_init(FALSE) > 0 )
{
printf("%d slaves found and configured.\n",ec_slavecount);
// check if first slave is an E/BOX
if (( ec_slavecount >= 1 ) &&
(strcmp(ec_slave[1].name,"E/BOX") == 0))
{
// reprogram PDO mapping to set slave in stream mode
// this can only be done in pre-OP state
os=sizeof(ob2); ob2 = 0x1601;
ec_SDOwrite(1,0x1c12,01,FALSE,os,&ob2,EC_TIMEOUTRXM);
os=sizeof(ob2); ob2 = 0x1a01;
ec_SDOwrite(1,0x1c13,01,FALSE,os,&ob2,EC_TIMEOUTRXM);
}
ec_config_map(&IOmap);
ec_configdc();
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
/* configure DC options for every DC capable slave found in the list */
printf("DC capable : %d\n",ec_configdc());
/* check configuration */
if (( ec_slavecount >= 1 ) &&
(strcmp(ec_slave[1].name,"E/BOX") == 0)
)
{
printf("E/BOX found.\n");
/* connect struct pointers to slave I/O pointers */
in_EBOX = (in_EBOX_streamt*) ec_slave[1].inputs;
out_EBOX = (out_EBOX_streamt*) ec_slave[1].outputs;
/* read indevidual slave state and store in ec_slave[] */
ec_readstate();
for(cnt = 1; cnt <= ec_slavecount ; cnt++)
{
printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
}
printf("Request operational state for all slaves\n");
/* send one processdata cycle to init SM in slaves */
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
ec_slave[0].state = EC_STATE_OPERATIONAL;
/* request OP state for all slaves */
ec_writestate(0);
/* wait for all slaves to reach OP state */
ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
if (ec_slave[0].state == EC_STATE_OPERATIONAL )
{
printf("Operational state reached for all slaves.\n");
ain[0] = 0;
ain[1] = 0;
ainc = 0;
dorun = 1;
usleep(100000); // wait for linux to sync on DC
ec_dcsync0(1, TRUE, SYNC0TIME, 0); // SYNC0 on slave 1
/* acyclic loop 20ms */
for(i = 1; i <= 200; i++)
{
/* read DC difference register for slave 2 */
// ec_FPRD(ec_slave[1].configadr, ECT_REG_DCSYSDIFF, sizeof(DCdiff), &DCdiff, EC_TIMEOUTRET);
// if(DCdiff<0) { DCdiff = - (int32)((uint32)DCdiff & 0x7ffffff); }
printf("PD cycle %5d DCtime %12lld Cnt:%3d Data: %6d %6d %6d %6d %6d %6d %6d %6d \n",
cyclecount, ec_DCtime, in_EBOX->counter, in_EBOX->stream[0], in_EBOX->stream[1],
in_EBOX->stream[2], in_EBOX->stream[3], in_EBOX->stream[4], in_EBOX->stream[5],
in_EBOX->stream[98], in_EBOX->stream[99]);
usleep(20000);
}
dorun = 0;
// printf("\nCnt %d : Ain0 = %f Ain2 = %f\n", ainc, ain[0] / ainc, ain[1] / ainc);
}
else
{
printf("Not all slaves reached operational state.\n");
}
}
else
{
printf("E/BOX not found in slave configuration.\n");
}
ec_dcsync0(1, FALSE, 8000, 0); // SYNC0 off
printf("Request safe operational state for all slaves\n");
ec_slave[0].state = EC_STATE_SAFE_OP;
/* request SAFE_OP state for all slaves */
ec_writestate(0);
/* wait for all slaves to reach state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
ec_slave[0].state = EC_STATE_PRE_OP;
/* request SAFE_OP state for all slaves */
ec_writestate(0);
/* wait for all slaves to reach state */
ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE);
if (( ec_slavecount >= 1 ) &&
(strcmp(ec_slave[1].name,"E/BOX") == 0))
{
// restore PDO to standard mode
// this can only be done is pre-op state
os=sizeof(ob2); ob2 = 0x1600;
ec_SDOwrite(1,0x1c12,01,FALSE,os,&ob2,EC_TIMEOUTRXM);
os=sizeof(ob2); ob2 = 0x1a00;
ec_SDOwrite(1,0x1c13,01,FALSE,os,&ob2,EC_TIMEOUTRXM);
}
printf("Streampos %d\n", streampos);
output_cvs("stream.txt", streampos);
}
else
{
printf("No slaves found!\n");
}
printf("End E/BOX, close socket\n");
/* stop SOEM, close socket */
ec_close();
}
else
{
printf("No socket connection on %s\nExcecute as root\n",ifname);
}
}
bool EcMaster::configure() throw(EcError)
{
bool success;
if(slaveInformation)
slaveInfo();
//Configure IOmap
ec_config_map(&m_IOmap);
//Configure DC
if (m_useDC)
ec_configdc();
/* The SGDV slave need to activate the cyclic communication
* just after of activating the distributed clocks
*/
taskFinished = false;
void* period = &m_cycleTime;
if(SGDVconnected)
{
rt_task_create (task, "PDO rt_task", 8192, 99, T_JOINABLE);
rt_task_start (task, &rt_thread, period);
}
if (m_useDC)
{
usleep(100000);//wait for master to sync on DC
for (int i = 0; i < m_drivers.size(); i++)
m_drivers[i] -> setDC(true, m_cycleTime, sync0Shift);//Activate sync0 event shifted 250us (it works well)
}
if(EcatError)
throw(EcError(EcError::ECAT_ERROR));
#ifdef HRT
//Create master buffers. Added timestamp space for each slave in input buffer
outputSize = ec_slave[0].Obytes;
inputSize = ec_slave[0].Ibytes + ec_slavecount * timestampSize;
outputBuf = new char [outputSize];
inputBuf = new char[inputSize];
memset(outputBuf,0, outputSize);
memset(inputBuf,0, inputSize);
/*
*Configuring the slave buffer pointers. Each slave have a pointer that points to their data location
*in the master Buffer.
*/
int offSetInput = 0, offSetOutput = 0;
for(int i = 0; i < m_drivers.size();i++)
{
m_drivers[i] -> setPDOBuffer(inputBuf + offSetInput, outputBuf + offSetOutput);
if(i < (m_drivers.size()-1))
{
offSetOutput += ec_slave[i+1].Obytes;
offSetInput += ec_slave[i+1].Ibytes + timestampSize;
}
}
#endif
rt_printf("Request SAFE-OPERATIONAL state for all slaves \n");
success = switchState (EC_STATE_SAFE_OP);
if (!success)
throw(EcError(EcError::FAIL_SWITCHING_STATE_SAFE_OP));
rt_printf("SAFE-OPERATIONAL state reached \n");
if(EcatError)
throw(EcError(EcError::ECAT_ERROR));
rt_printf("Request OPERATIONAL state for all slaves \n");
success = switchState(EC_STATE_OPERATIONAL);
if (!success)
throw(EcError(EcError::FAIL_SWITCHING_STATE_OPERATIONAL));
rt_printf("OPERATIONAL state reached \n" );
//Starts the cyclic task (PDOs task) if no SGDV slaves connected.
if(!SGDVconnected)
{
rt_task_create (task, "PDO rt_task", 8192, 99, T_JOINABLE);
rt_task_start (task, &rt_thread, period);
}
usleep(200000);
rt_printf("Master configured!!! \n");
return true;
}
bool Drive::open(const QString& adapterName)
{
qDebug() << "drive: open " << adapterName;
bool ok = true;
if (isOpened) {
assert(false && "repeated call is not supported");
ok = false;
}
if (ok) {
strcpy_s(adapterNameBuffer, adapterName.toStdString().c_str()); // TODO уязвимость
// initialise SOEM, bind socket to adapterNameBuffer
if (ec_init(adapterNameBuffer) <= 0) {
qDebug() << "ошибка ec_init";
ok = false;
}
}
// find and auto-config slaves
if (ok) {
isOpened = true; // TODO правильное ли место?
const auto slaveCount = ec_config_init(0);
if (slaveCount != 1) {
qDebug() << "количество найденных устройств равно " << slaveCount;
if (slaveCount == EC_OTHERFRAME)
qDebug() << "Ошибка: unknown frame received";
else if (slaveCount == EC_NOFRAME)
qDebug() << "Ошибка: no frame returned";
ok = false;
}
}
if (ok) {
// проверка, что подключен именно наш мотор
auto slave = &ec_slave[1];
if (slave->eep_man != 599 || slave->eep_id != 41220) {
qDebug() << "eep_man: " << slave->eep_man << ", slave->eep_id: " << slave->eep_id;
ok = false;
}
ec_slave[1].PO2SOconfig = &myPdoSetup;
}
if (ok) {
// Map all PDOs from slaves to iomap
ec_config_map(&iomap);
// wait for all slaves to reach SAFE_OP state
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
ec_slave[0].state = EC_STATE_OPERATIONAL; // TODO правильно ли, что это здесь?
// send one valid process data to make outputs in slaves happy
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
// start I/O transfer thread as periodic multimedia timer
globalErrorFlag.store(GLOBAL_OK);
timerEventId = timeSetEvent(1, 0, ioTransferThread, 0, TIME_PERIODIC); // TODO рассмотреть переход на CreateTimerQueueTimer
if (timerEventId == NULL) {
qDebug() << "Не удалось создать поток ввода/вывода";
ok = false;
}
}
if (ok) {
// request OP state for all slaves
ec_writestate(0);
// wait for all slaves to reach OP state
int chk = 40;
do
ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
if (ec_slave[0].state != EC_STATE_OPERATIONAL) {// не удадось перевести метор в состояние OPERATIONAL
qDebug() << "не удадось перевести мотор в состояние OPERATIONAL";
ok = false;
}
while(EcatError)
qDebug("%s", ec_elist2string());
}
if (ok) {
int result = ec_SDOwrite(1, 0x4003, 0x01, FALSE, sizeof(deviceMode), &deviceMode, EC_TIMEOUTRXM); // Device Mode
if (result != 1)
qDebug() << "ошибка ec_SDOwrite: " << result;
while(EcatError)
qDebug("%s", ec_elist2string());
ok = (result == 1);
}
if (ok) {
timer.start(updatePeriod);
setPower(true);
}
else
close();
return ok;
}
