int EL7031setup(uint16 slave) { int retval; uint16 u16val; // map velocity uint16 map_1c12[4] = {0x0003, 0x1601, 0x1602, 0x1604}; uint16 map_1c13[3] = {0x0002, 0x1a01, 0x1a03}; retval = 0; // Set PDO mapping using Complete Access // Strange, writing CA works, reading CA doesn't // This is a protocol error of the slave. retval += ec_SDOwrite(slave, 0x1c12, 0x00, TRUE, sizeof(map_1c12), &map_1c12, EC_TIMEOUTSAFE); retval += ec_SDOwrite(slave, 0x1c13, 0x00, TRUE, sizeof(map_1c13), &map_1c13, EC_TIMEOUTSAFE); // bug in EL7031 old firmware, CompleteAccess for reading is not supported even if the slave says it is. ec_slave[slave].CoEdetails &= ~ECT_COEDET_SDOCA; // set some motor parameters, just as example u16val = 1200; // max motor current in mA // retval += ec_SDOwrite(slave, 0x8010, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTSAFE); u16val = 150; // motor coil resistance in 0.01ohm // retval += ec_SDOwrite(slave, 0x8010, 0x04, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTSAFE); // set other nescessary parameters as needed // ..... while(EcatError) printf("%s", ec_elist2string()); printf("EL7031 slave %d set, retval = %d\n", slave, retval); return 1; }
static int myPdoSetup(uint16 slave) { int retval = 0; uint8 u8val; uint16 u16val; u8val = 0; retval += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); u16val = 0x1A00; retval += ec_SDOwrite(slave, 0x1c13, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM); u16val = 0x1A03; retval += ec_SDOwrite(slave, 0x1c13, 0x02, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM); u16val = 0x1A04; retval += ec_SDOwrite(slave, 0x1c13, 0x03, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM); u16val = 0x1A06; retval += ec_SDOwrite(slave, 0x1c13, 0x04, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM); u8val = 4; retval += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); while(EcatError) qDebug("%s", ec_elist2string()); qDebug("AEP slave %d set, retval = %d\n", slave, retval); return 1; }
// slots void Drive::update() { if (!isOpened) return; PdoData pdoData; memcpy(&pdoData, ec_slave[1].inputs, sizeof(pdoData)); //qDebug() << "statusword: " << pdoData.statusword; //qDebug() << "velocity actual value: " << pdoData.velocityActualValue; //qDebug() << "measurement velocity rpm: " << pdoData.measurementVelRpm; //qDebug() << "current actual filtered value: " << pdoData.currentActualValue; //qDebug() << "ue: " << pdoData.ue; //qDebug() << "up: " << pdoData.up; emit rpmChanged(pdoData.velocityActualValue); //emit currentChanged(pdoData.currentActualValue); bool ok = !EcatError; while (EcatError) qDebug("%s", ec_elist2string()); //emit stateChanged(ok); ok = true; // // qDebug("Slave State=0x%2.2x StatusCode=0x%4.4x : %s", // ec_slave[1].state, ec_slave[1].ALstatuscode, ec_ALstatuscode2string(ec_slave[1].ALstatuscode)); if (globalErrorFlag.fetchAndStoreRelaxed(GLOBAL_OK) == GLOBAL_ERROR) { qDebug("ERROR from iothread"); isWorking = false; emit error(); } }
bool SoemEL3104::configure() { #if 0 for (unsigned int i = 0; i < params.size(); i++) { while (EcatError) log(RTT::Error) << ec_elist2string() << RTT::endlog(); //assigning parameters ec_SDOwrite(((m_datap->configadr) & 0x0F), params[i].index, params[i].subindex, FALSE, params[i].size, &val, EC_TIMEOUTRXM); } #endif return true; }
int AEPsetup(uint16 slave) { int retval; uint8 u8val; uint16 u16val; retval = 0; u8val = 0; retval += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); u16val = 0x1603; retval += ec_SDOwrite(slave, 0x1c12, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM); u8val = 1; retval += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); u8val = 0; retval += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); u16val = 0x1a03; retval += ec_SDOwrite(slave, 0x1c13, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM); u8val = 1; retval += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); u8val = 8; retval += ec_SDOwrite(slave, 0x6060, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); // set some motor parameters, just as example u16val = 1200; // max motor current in mA // retval += ec_SDOwrite(slave, 0x8010, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTSAFE); u16val = 150; // motor coil resistance in 0.01ohm // retval += ec_SDOwrite(slave, 0x8010, 0x04, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTSAFE); // set other nescessary parameters as needed // ..... while(EcatError) printf("%s", ec_elist2string()); printf("AEP slave %d set, retval = %d\n", slave, retval); return 1; }
char* SDO2string(uint16 slave, uint16 index, uint8 subidx, uint16 dtype) { int l = sizeof(usdo) - 1, i; uint8 *u8; int8 *i8; uint16 *u16; int16 *i16; uint32 *u32; int32 *i32; uint64 *u64; int64 *i64; float *sr; double *dr; char es[32]; memset(&usdo, 0, 128); ec_SDOread(slave, index, subidx, FALSE, &l, &usdo, EC_TIMEOUTRXM); if (EcatError) { return ec_elist2string(); } else { switch(dtype) { case ECT_BOOLEAN: u8 = (uint8*) &usdo[0]; if (*u8) sprintf(hstr, "TRUE"); else sprintf(hstr, "FALSE"); break; case ECT_INTEGER8: i8 = (int8*) &usdo[0]; sprintf(hstr, "0x%2.2x %d", *i8, *i8); break; case ECT_INTEGER16: i16 = (int16*) &usdo[0]; sprintf(hstr, "0x%4.4x %d", *i16, *i16); break; case ECT_INTEGER32: case ECT_INTEGER24: i32 = (int32*) &usdo[0]; sprintf(hstr, "0x%8.8x %d", *i32, *i32); break; case ECT_INTEGER64: i64 = (int64*) &usdo[0]; sprintf(hstr, "0x%16.16llx %lld", *i64, *i64); break; case ECT_UNSIGNED8: u8 = (uint8*) &usdo[0]; sprintf(hstr, "0x%2.2x %u", *u8, *u8); break; case ECT_UNSIGNED16: u16 = (uint16*) &usdo[0]; sprintf(hstr, "0x%4.4x %u", *u16, *u16); break; case ECT_UNSIGNED32: case ECT_UNSIGNED24: u32 = (uint32*) &usdo[0]; sprintf(hstr, "0x%8.8x %u", *u32, *u32); break; case ECT_UNSIGNED64: u64 = (uint64*) &usdo[0]; sprintf(hstr, "0x%16.16llx %llu", *u64, *u64); break; case ECT_REAL32: sr = (float*) &usdo[0]; sprintf(hstr, "%f", *sr); break; case ECT_REAL64: dr = (double*) &usdo[0]; sprintf(hstr, "%f", *dr); break; case ECT_BIT1: case ECT_BIT2: case ECT_BIT3: case ECT_BIT4: case ECT_BIT5: case ECT_BIT6: case ECT_BIT7: case ECT_BIT8: u8 = (uint8*) &usdo[0]; sprintf(hstr, "0x%x", *u8); break; case ECT_VISIBLE_STRING: strcpy(hstr, usdo); break; case ECT_OCTET_STRING: hstr[0] = 0x00; for (i = 0 ; i < l ; i++) { sprintf(es, "0x%2.2x ", usdo[i]); strcat( hstr, es); } break; default: sprintf(hstr, "Unknown type"); } return hstr; } }
void slaveinfo(char *ifname) { int cnt, i, j, nSM; uint16 ssigen, dtype; printf("Starting slaveinfo\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(FALSE, &IOmap) > 0 ) { printf("%d slaves found and configured.\n",ec_slavecount); printf("Calculated workcounter %d\n",ec_group[0].expectedWKC); /* wait for all slaves to reach SAFE_OP state */ ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 3); if (ec_slave[0].state != EC_STATE_SAFE_OP ) { printf("Not all slaves reached safe operational state.\n"); ec_readstate(); for(i = 1; i<=ec_slavecount ; i++) { if(ec_slave[i].state != EC_STATE_SAFE_OP) { printf("Slave %d State=%2x StatusCode=%4x : %s\n", i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); } } } ec_configdc(); ec_readstate(); for( cnt = 1 ; cnt <= ec_slavecount ; cnt++) { printf("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %d[ns]\n Has DC: %d\n", cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, ec_slave[cnt].state, ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); if (ec_slave[cnt].hasdc) printf(" DCParentport:%d\n", ec_slave[cnt].parentport); printf(" Activeports:%d.%d.%d.%d\n", (ec_slave[cnt].activeports & 0x01) > 0 , (ec_slave[cnt].activeports & 0x02) > 0 , (ec_slave[cnt].activeports & 0x04) > 0 , (ec_slave[cnt].activeports & 0x08) > 0 ); printf(" Configured address: %4.4x\n", ec_slave[cnt].configadr); printf(" Man: %8.8x ID: %8.8x Rev: %8.8x\n", (int)ec_slave[cnt].eep_man, (int)ec_slave[cnt].eep_id, (int)ec_slave[cnt].eep_rev); for(nSM = 0 ; nSM < EC_MAXSM ; nSM++) { if(ec_slave[cnt].SM[nSM].StartAddr > 0) printf(" SM%1d A:%4.4x L:%4d F:%8.8x Type:%d\n",nSM, ec_slave[cnt].SM[nSM].StartAddr, ec_slave[cnt].SM[nSM].SMlength, (int)ec_slave[cnt].SM[nSM].SMflags, ec_slave[cnt].SMtype[nSM]); } for(j = 0 ; j < ec_slave[cnt].FMMUunused ; j++) { printf(" FMMU%1d Ls:%8.8x Ll:%4d Lsb:%d Leb:%d Ps:%4.4x Psb:%d Ty:%2.2x Act:%2.2x\n", j, (int)ec_slave[cnt].FMMU[j].LogStart, ec_slave[cnt].FMMU[j].LogLength, ec_slave[cnt].FMMU[j].LogStartbit, ec_slave[cnt].FMMU[j].LogEndbit, ec_slave[cnt].FMMU[j].PhysStart, ec_slave[cnt].FMMU[j].PhysStartBit, ec_slave[cnt].FMMU[j].FMMUtype, ec_slave[cnt].FMMU[j].FMMUactive); } printf(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n", ec_slave[cnt].FMMU0func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU3func); printf(" MBX length wr: %d rd: %d MBX protocols : %2.2x\n", ec_slave[cnt].mbx_l, ec_slave[cnt].mbx_rl, ec_slave[cnt].mbx_proto); ssigen = ec_siifind(cnt, ECT_SII_GENERAL); /* SII general section */ if (ssigen) { ec_slave[cnt].CoEdetails = ec_siigetbyte(cnt, ssigen + 0x07); ec_slave[cnt].FoEdetails = ec_siigetbyte(cnt, ssigen + 0x08); ec_slave[cnt].EoEdetails = ec_siigetbyte(cnt, ssigen + 0x09); ec_slave[cnt].SoEdetails = ec_siigetbyte(cnt, ssigen + 0x0a); if((ec_siigetbyte(cnt, ssigen + 0x0d) & 0x02) > 0) { ec_slave[cnt].blockLRW = 1; ec_slave[0].blockLRW++; } ec_slave[cnt].Ebuscurrent = ec_siigetbyte(cnt, ssigen + 0x0e); ec_slave[cnt].Ebuscurrent += ec_siigetbyte(cnt, ssigen + 0x0f) << 8; ec_slave[0].Ebuscurrent += ec_slave[cnt].Ebuscurrent; } printf(" CoE details: %2.2x FoE details: %2.2x EoE details: %2.2x SoE details: %2.2x\n", ec_slave[cnt].CoEdetails, ec_slave[cnt].FoEdetails, ec_slave[cnt].EoEdetails, ec_slave[cnt].SoEdetails); printf(" Ebus current: %d[mA]\n only LRD/LWR:%d\n", ec_slave[cnt].Ebuscurrent, ec_slave[cnt].blockLRW); if ((ec_slave[cnt].mbx_proto & 0x04) && printSDO) { ODlist.Entries = 0; memset(&ODlist, 0, sizeof(ODlist)); if( ec_readODlist(cnt, &ODlist)) { printf(" CoE Object Description found, %d entries.\n",ODlist.Entries); for( i = 0 ; i < ODlist.Entries ; i++) { ec_readODdescription(i, &ODlist); while(EcatError) { printf("%s", ec_elist2string()); } printf(" Index: %4.4x Datatype: %4.4x Objectcode: %2.2x Name: %s\n", ODlist.Index[i], ODlist.DataType[i], ODlist.ObjectCode[i], ODlist.Name[i]); memset(&OElist, 0, sizeof(OElist)); ec_readOE(i, &ODlist, &OElist); while(EcatError) { printf("%s", ec_elist2string()); } for( j = 0 ; j < ODlist.MaxSub[i]+1 ; j++) { if ((OElist.DataType[j] > 0) && (OElist.BitLength[j] > 0)) { printf(" Sub: %2.2x Datatype: %4.4x Bitlength: %4.4x Obj.access: %4.4x Name: %s\n", j, OElist.DataType[j], OElist.BitLength[j], OElist.ObjAccess[j], OElist.Name[j]); if ((OElist.ObjAccess[j] & 0x0007)) { dtype = OElist.DataType[j]; printf(" Value :%s\n", SDO2string(cnt, ODlist.Index[i], j, OElist.DataType[j])); } } } } } else { while(EcatError) { printf("%s", ec_elist2string()); } } } } } else { printf("No slaves found!\n"); } printf("End slaveinfo, close socket\n"); /* stop SOEM, close socket */ ec_close(); } else { printf("No socket connection on %s\nExcecute as root\n",ifname); } }
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; }