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; }