/* Try all the possible characters at the current position.
* Display those that do not generate any conflicts.
*/
void dbstryall(gwindow *dbs, key k __attribute__((unused)))
{
int col, pos;
int oldrow, oldcol;
int tchar;
ecinfo *ecbi;
dbsinfo *dbsi;
int pque_index;
pqueue_hdr pque_hdr;
pqueue_ent the_pque[MAXCHAR+1];
dbsi = ((dbsinfo *) dbs->wprivate);
ecbi = &t_ecinfo;
pque_init(&pque_hdr, 1000.0, &the_pque[0], MAXCHAR+1);
ec_init(dbsi->cbuf, dbsi->perm, ecbi);
oldrow = dbs->wcur_row;
oldcol = dbs->wcur_col;
pos = dbsrc2pos(oldrow, oldcol);
dbstrypq(ecbi, &pque_hdr, pos);
wl_setcur(dbs, dbsp2row(BLOCKSIZE), dbsp2col(BLOCKSIZE));
tchar = 0;
for (col = 1 ; col < dbs->wwidth ; col++) {
pque_index = col - 1;
if (pque_index >= pque_hdr.next_index) break;
tchar = the_pque[pque_index].value1;
plnchars(1, char2sym(tchar));
}
/* alldone: */
plnchars((dbs->wwidth) - col, ' ');
wl_setcur(dbs, oldrow, oldcol);
}
int imanager_ec_probe(void)
{
int chipid = ec_read_chipid(EC_BASE_ADDR);
memset((void *)&ec, 0, sizeof(ec));
switch (chipid) {
case EC_DEVID_IT8516:
pr_err("EC IT8516 not supported\n");
ec.dev.id = IT8516;
return -ENODEV;
case EC_DEVID_IT8518:
ec.read = ec_io18_read;
ec.write = ec_io18_write;
ec.dev.id = IT8518;
break;
case EC_DEVID_IT8528:
ec.read = ec_io28_read;
ec.write = ec_io28_write;
ec.dev.id = IT8528;
break;
default:
return -ENODEV;
}
ec.dev.addr = EC_BASE_ADDR;
return ec_init();
}
void main()
{
// initialisation of ECLiPSe
ec_set_option_ptr(EC_OPTION_DEFAULT_MODULE, "ptc_solver");
ec_init();
//quiet();
// loading of the solver
post_goal(term(EC_functor("lib", 1), "ptc_solver"));
printf("ECLiPSe and the PTC solver are being loaded ...\n");
if (EC_resume()==EC_succeed) printf("The PTC Solver has been loaded\n");
else printf("Error: The PTC Solver did not load properly\n");
// initialisation of the solver
submit_string("ptc_solver__clean_up, ptc_solver__default_declarations");
printf("The PTC Solver is initialised\n");
//output file
log = fopen("latest run.log", "w");
printf("The PTC Solver is running\n");
//calling a demonstration session
session3();
printf("The PTC Solver has finished\n");
//Unloading ECLiPSe and tidying up
ec_cleanup();
printf("ECLiPSe and the PTC Solver have been unloaded\n\n");
fclose(log);
}//main
static int
xpv_drv_init(void)
{
if (xpv_feature(XPVF_HYPERCALLS) < 0 ||
xpv_feature(XPVF_SHARED_INFO) < 0)
return (-1);
/* Set up the grant tables. */
gnttab_init();
/* Set up event channel support */
if (ec_init() != 0)
return (-1);
/* Set up xenbus */
xb_addr = vmem_alloc(heap_arena, MMU_PAGESIZE, VM_SLEEP);
xs_early_init();
xs_domu_init();
/* Set up for suspend/resume/migrate */
xen_shutdown_tq = taskq_create("shutdown_taskq", 1,
maxclsyspri - 1, 1, 1, TASKQ_PREPOPULATE);
shutdown_watch.node = "control/shutdown";
shutdown_watch.callback = xen_shutdown_handler;
if (register_xenbus_watch(&shutdown_watch))
cmn_err(CE_WARN, "Failed to set shutdown watcher");
return (0);
}
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;
}
int server_init(void)
{
dbg_err_if((g_ctx.base = event_base_new()) == NULL);
dbg_err_if((g_ctx.dns = evdns_base_new(g_ctx.base, 1)) == NULL);
dbg_err_if((g_ctx.coap = ec_init(g_ctx.base, g_ctx.dns)) == NULL);
dbg_err_if((g_ctx.fs = ec_filesys_create(g_ctx.rel_refs)) == NULL);
if (g_ctx.bsz)
dbg_err_if(ec_set_block_size(g_ctx.coap, g_ctx.bsz));
return 0;
err:
server_term();
return -1;
}
int proxy_init(void)
{
dbg_err_if ((g_ctx.base = event_base_new()) == NULL);
dbg_err_if ((g_ctx.dns = evdns_base_new(g_ctx.base, 1)) == NULL);
dbg_err_if ((g_ctx.coap = ec_init(g_ctx.base, g_ctx.dns)) == NULL);
dbg_err_if ((g_ctx.cache = ec_filesys_create(false)) == NULL);
if (g_ctx.block_sz)
dbg_err_if(ec_set_block_size(g_ctx.coap, g_ctx.block_sz));
return 0;
err:
proxy_term();
return -1;
}
int skb_init(void)
{
int n;
ec_set_option_int(EC_OPTION_IO, MEMORY_IO);
ec_set_option_ptr(EC_OPTION_ECLIPSEDIR, ECLIPSE_DIR);
ec_set_option_long(EC_OPTION_GLOBALSIZE, MEMORY_SIZE);
ec_set_option_int(EC_OPTION_IO, MEMORY_IO);
SKB_DEBUG("\ninitialize eclipse\n");
n = ec_init();
if (n != 0) {
SKB_DEBUG("\nskb_main: ec_init() failed.");
}
return (n);
}
/**
* Performance testing of ECC signature generations and verifications
*/
int main(void)
{
elem_t rho;
uint8_t b;
unsigned char hash[PLA_HASH_LENGTH + 1];
hash[PLA_HASH_LENGTH] = 0;
unsigned char id_hash[PLA_HASH_LENGTH + 1];
id_hash[PLA_HASH_LENGTH] = 0;
unsigned char c = 'x';
unsigned char c2 = 'y';
int i;
time_t initial, final;
ec_init();
/* Some hashes */
RIPEMD160(&c, 1, hash);
RIPEMD160(&c2, 1, id_hash);
/* Generate a new key */
mpz_t tmp_sigma; mpz_init(tmp_sigma);
mpz_t sigma; mpz_init(sigma);
ec_keygen(&(e.y), tmp_sigma);
ec_create_key(&rho, &b, &sigma, id_hash, &tmp_sigma);
/* Test signing */
ecselfsig_t sig;
initial = time(&initial);
for (i=0; i<COUNT; i++)
ec_sign_self(&sig, hash, sigma);
final = time(&final);
void boottest(char *ifname, uint16 slave, char *filename)
{
printf("Starting firmware update example\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);
printf("Request init state for slave %d\n", slave);
ec_slave[slave].state = EC_STATE_INIT;
ec_writestate(slave);
/* wait for slave to reach INIT state */
ec_statecheck(slave, EC_STATE_INIT, EC_TIMEOUTSTATE * 4);
printf("Slave %d state to INIT.\n", slave);
/* read BOOT mailbox data, master -> slave */
data = ec_readeeprom(slave, ECT_SII_BOOTRXMBX, EC_TIMEOUTEEP);
ec_slave[slave].SM[0].StartAddr = (uint16)LO_WORD(data);
ec_slave[slave].SM[0].SMlength = (uint16)HI_WORD(data);
/* store boot write mailbox address */
ec_slave[slave].mbx_wo = (uint16)LO_WORD(data);
/* store boot write mailbox size */
ec_slave[slave].mbx_l = (uint16)HI_WORD(data);
/* read BOOT mailbox data, slave -> master */
data = ec_readeeprom(slave, ECT_SII_BOOTTXMBX, EC_TIMEOUTEEP);
ec_slave[slave].SM[1].StartAddr = (uint16)LO_WORD(data);
ec_slave[slave].SM[1].SMlength = (uint16)HI_WORD(data);
/* store boot read mailbox address */
ec_slave[slave].mbx_ro = (uint16)LO_WORD(data);
/* store boot read mailbox size */
ec_slave[slave].mbx_rl = (uint16)HI_WORD(data);
printf(" SM0 A:%4.4x L:%4d F:%8.8x\n", ec_slave[slave].SM[0].StartAddr, ec_slave[slave].SM[0].SMlength,
(int)ec_slave[slave].SM[0].SMflags);
printf(" SM1 A:%4.4x L:%4d F:%8.8x\n", ec_slave[slave].SM[1].StartAddr, ec_slave[slave].SM[1].SMlength,
(int)ec_slave[slave].SM[1].SMflags);
/* program SM0 mailbox in for slave */
ec_FPWR (ec_slave[slave].configadr, ECT_REG_SM0, sizeof(ec_smt), &ec_slave[slave].SM[0], EC_TIMEOUTRET);
/* program SM1 mailbox out for slave */
ec_FPWR (ec_slave[slave].configadr, ECT_REG_SM1, sizeof(ec_smt), &ec_slave[slave].SM[1], EC_TIMEOUTRET);
printf("Request BOOT state for slave %d\n", slave);
ec_slave[slave].state = EC_STATE_BOOT;
ec_writestate(slave);
/* wait for slave to reach BOOT state */
if (ec_statecheck(slave, EC_STATE_BOOT, EC_TIMEOUTSTATE * 10) == EC_STATE_BOOT)
{
printf("Slave %d state to BOOT.\n", slave);
if (input_bin(filename, &filesize))
{
printf("File read OK, %d bytes.\n",filesize);
printf("FoE write....");
j = ec_FOEwrite(slave, filename, 0, filesize , &filebuffer, EC_TIMEOUTSTATE);
printf("result %d.\n",j);
printf("Request init state for slave %d\n", slave);
ec_slave[slave].state = EC_STATE_INIT;
ec_writestate(slave);
}
else
printf("File not read OK.\n");
}
}
else
{
printf("No slaves found!\n");
}
printf("End firmware update example, close socket\n");
/* stop SOEM, close socket */
ec_close();
}
else
{
printf("No socket connection on %s\nExcecute as root\n",ifname);
}
}
///establishes the ethercat connection
void EthercatMaster::initializeEthercat() {
// Bouml preserved body begin 000410F1
/* initialise SOEM, bind socket to ifname */
if (ec_init(ethernetDevice.c_str())) {
LOG(info) << "Initializing EtherCAT on " << ethernetDevice;
/* find and auto-config slaves */
if (ec_config(TRUE, &IOmap_) > 0) {
LOG(trace) << ec_slavecount << " slaves found and configured.";
/* wait for all slaves to reach Pre OP state */
/*ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE);
if (ec_slave[0].state != EC_STATE_PRE_OP ){
printf("Not all slaves reached pre operational state.\n");
ec_readstate();
//If not all slaves operational find out which one
for(int i = 1; i<=ec_slavecount ; i++){
if(ec_slave[i].state != EC_STATE_PRE_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));
}
}
}
*/
/* distributed clock is not working
//Configure distributed clock
if(!ec_configdc()){
LOG(warning) << "no distributed clock is available";
}else{
uint32 CyclTime = 4000000;
uint32 CyclShift = 0;
for (int i = 1; i <= ec_slavecount; i++) {
ec_dcsync0(i, true, CyclTime, CyclShift);
}
}
*/
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
if (ec_slave[0].state != EC_STATE_SAFE_OP) {
LOG(warning) << "Not all slaves reached safe operational state.";
ec_readstate();
//If not all slaves operational find out which one
for (int i = 1; i <= ec_slavecount; i++) {
if (ec_slave[i].state != EC_STATE_SAFE_OP) {
LOG(info) << "Slave " << i << " State=" << ec_slave[i].state << " StatusCode=" << ec_slave[i].ALstatuscode << " : " << ec_ALstatuscode2string(ec_slave[i].ALstatuscode);
}
}
}
//Read the state of all slaves
//ec_readstate();
LOG(trace) << "Request operational state for all slaves";
ec_slave[0].state = EC_STATE_OPERATIONAL;
// request OP state for all slaves
/* 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);
// wait for all slaves to reach OP state
ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
if (ec_slave[0].state == EC_STATE_OPERATIONAL) {
LOG(trace) << "Operational state reached for all slaves.";
} else {
throw std::runtime_error("Not all slaves reached operational state.");
}
} else {
throw std::runtime_error("No slaves found!");
}
} else {
throw std::runtime_error("No socket connection on " + ethernetDevice + "\nExcecute as root");
}
std::string baseJointControllerName = "TMCM-174";
std::string manipulatorJointControllerName = "TMCM-174";
std::string actualSlaveName;
nrOfSlaves = 0;
YouBotSlaveMsg emptySlaveMsg;
quantity<si::current> maxContinuousCurrentBase = 3.54 * ampere;
quantity<si::time> thermalTimeConstantWindingBase = 16.6 * second;
quantity<si::time> thermalTimeConstantMotorBase = 212 * second;
quantity<si::current> maxContinuousCurrentJoint13 = 2.36 * ampere;
quantity<si::time> thermalTimeConstantWindingJoint13 = 16.6 * second;
quantity<si::time> thermalTimeConstantMotorJoint13 = 212 * second;
quantity<si::current> maxContinuousCurrentJoint4 = 1.07 * ampere;
quantity<si::time> thermalTimeConstantWindingJoint4 = 13.2 * second;
quantity<si::time> thermalTimeConstantMotorJoint4 = 186 * second;
quantity<si::current> maxContinuousCurrentJoint5 = 0.49 * ampere;
quantity<si::time> thermalTimeConstantWindingJoint5 = 8.1 * second;
quantity<si::time> thermalTimeConstantMotorJoint5 = 108 * second;
int manipulatorNo = 0;
configfile->readInto(baseJointControllerName, "BaseJointControllerName");
configfile->readInto(manipulatorJointControllerName, "ManipulatorJointControllerName");
//reserve memory for all slave with a input/output buffer
for (int 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);
ethercatSlaveInfo.push_back(ec_slave[cnt]);
actualSlaveName = ec_slave[cnt].name;
if ((actualSlaveName == baseJointControllerName || actualSlaveName == manipulatorJointControllerName) && ec_slave[cnt].Obits > 0 && ec_slave[cnt].Ibits > 0) {
nrOfSlaves++;
// joints.push_back(YouBotJoint(nrOfSlaves));
firstBufferVector.push_back(emptySlaveMsg);
secondBufferVector.push_back(emptySlaveMsg);
ethercatOutputBufferVector.push_back((SlaveMessageOutput*) (ec_slave[cnt].outputs));
ethercatInputBufferVector.push_back((SlaveMessageInput*) (ec_slave[cnt].inputs));
YouBotSlaveMailboxMsg emptyMailboxSlaveMsg(cnt);
firstMailboxBufferVector.push_back(emptyMailboxSlaveMsg);
secondMailboxBufferVector.push_back(emptyMailboxSlaveMsg);
newOutputDataFlagOne.push_back(false);
newOutputDataFlagTwo.push_back(false);
newMailboxDataFlagOne.push_back(false);
newMailboxDataFlagTwo.push_back(false);
newMailboxInputDataFlagOne.push_back(false);
newMailboxInputDataFlagTwo.push_back(false);
pendingMailboxMsgsReply.push_back(false);
int i = 0;
bool b = false;
upperLimit.push_back(i);
lowerLimit.push_back(i);
limitActive.push_back(b);
jointLimitReached.push_back(b);
inverseMovementDirection.push_back(b);
if (actualSlaveName == baseJointControllerName) {
motorProtections.push_back(MotorProtection(maxContinuousCurrentBase,
thermalTimeConstantWindingBase,
thermalTimeConstantMotorBase));
}
if (actualSlaveName == manipulatorJointControllerName) {
manipulatorNo++;
if (manipulatorNo >= 1 && manipulatorNo <= 3) {
motorProtections.push_back(MotorProtection(maxContinuousCurrentJoint13,
thermalTimeConstantWindingJoint13,
thermalTimeConstantMotorJoint13));
}
if (manipulatorNo == 4) {
motorProtections.push_back(MotorProtection(maxContinuousCurrentJoint4,
thermalTimeConstantWindingJoint4,
thermalTimeConstantMotorJoint4));
}
if (manipulatorNo == 5) {
motorProtections.push_back(MotorProtection(maxContinuousCurrentJoint5,
thermalTimeConstantWindingJoint5,
thermalTimeConstantMotorJoint5));
manipulatorNo = 0;
}
}
}
}
if (nrOfSlaves != motorProtections.size()) {
throw std::runtime_error("Insufficient motor protections loaded");
}
if (nrOfSlaves > 0) {
LOG(info) << nrOfSlaves << " EtherCAT slaves found" ;
} else {
throw std::runtime_error("No EtherCAT slave could be found");
return;
}
stopThread = false;
threads.create_thread(boost::bind(&EthercatMaster::updateSensorActorValues, this));
SLEEP_MILLISEC(10); //needed to start up thread and EtherCAT communication
return;
// Bouml preserved body end 000410F1
}
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);
}
}
static int youbot_init(ubx_block_t *b)
{
int ret=-1, i, cur_slave;
unsigned int len;
char* interface;
uint32_t *conf_nr_arms;
struct youbot_info *inf;
interface = (char *) ubx_config_get_data_ptr(b, "ethernet_if", &len);
conf_nr_arms = (uint32_t *) ubx_config_get_data_ptr(b, "nr_arms", &len);
if(strncmp(interface, "", len)==0) {
ERR("config ethernet_if unset");
goto out;
}
/* allocate driver data */
if((inf=calloc(1, sizeof(struct youbot_info)))==NULL) {
ERR("failed to alloc youbot_info");
goto out;
}
b->private_data=inf;
if(ec_init(interface) <= 0) {
ERR("ec_init failed (IF=%s), sufficient rights, correct interface?", interface);
goto out_free;
}
if(ec_config(TRUE, &inf->io_map) <= 0) {
ERR("ec_config failed (IF=%s)", interface);
goto out_free;
} else {
DBG("detected %d slaves", ec_slavecount);
}
/* Let's see what we have...
* check for base, base is mandatory for a youbot */
if(ec_slavecount >= YOUBOT_NR_OF_BASE_SLAVES && validate_base_slaves(1) == 0) {
DBG("detected youbot base");
inf->base.detected=1;
for(i=0, cur_slave=2; i<YOUBOT_NR_OF_WHEELS; i++, cur_slave++)
inf->base.wheel_inf[i].slave_idx=cur_slave;
} else {
ERR("no base detected");
goto out_free;
}
/* check for first arm */
if(ec_slavecount >= (YOUBOT_NR_OF_BASE_SLAVES + YOUBOT_NR_OF_ARM_SLAVES) &&
validate_arm_slaves(1+YOUBOT_NR_OF_BASE_SLAVES)==0) {
DBG("detected youbot arm #1 (first slave=%d)", cur_slave);
inf->arm1.detected=1;
*conf_nr_arms=1;
cur_slave++; /* skip the power board */
for(i=0; i<YOUBOT_NR_OF_JOINTS; i++, cur_slave++)
inf->arm1.jnt_inf[i].slave_idx=cur_slave;
}
/* check for second arm */
if(ec_slavecount >= (YOUBOT_NR_OF_BASE_SLAVES + (2 * YOUBOT_NR_OF_ARM_SLAVES)) &&
validate_arm_slaves(1+YOUBOT_NR_OF_BASE_SLAVES+YOUBOT_NR_OF_ARM_SLAVES)==0) {
DBG("detected youbot arm #2 (first slave=%d)", cur_slave);
inf->arm2.detected=1;
*conf_nr_arms=2;
cur_slave++; /* skip the power board */
for(i=0; i<YOUBOT_NR_OF_JOINTS; i++, cur_slave++)
inf->arm2.jnt_inf[i].slave_idx=cur_slave;
}
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
if(ec_slave[0].state != EC_STATE_SAFE_OP) {
ERR("not all slaves reached state safe_operational:");
for (i=0; i<=ec_slavecount; i++) {
if (ec_slave[i].state != EC_STATE_SAFE_OP) {
ERR("\tslave %d (%s) is in state=0x%x, ALstatuscode=0x%x",
i, ec_slave[i].name, ec_slave[i].state, ec_slave[i].ALstatuscode);
}
}
goto out_free;
}
/* send and receive bogus process data to get things going */
if(ec_send_processdata() <= 0) {
ERR("failed to send bootstrap process data");
goto out_free;
}
if(ec_receive_processdata(EC_TIMEOUTRET) == 0) {
ERR("failed to receive bootstrap process data");
goto out_free;
}
if(inf->base.detected) base_config_params(&inf->base);
if(inf->arm1.detected) arm_config_params(&inf->arm1);
if(inf->arm2.detected) arm_config_params(&inf->arm2);
#if 0
int tmp
/* find firmware version */
if(send_mbx(0, FIRMWARE_VERSION, 1, 0, 0, &tmp)) {
ERR("Failed to read firmware version");
goto out;
}
else
DBG("youbot firmware version %d", tmp);
#endif
ret=0;
goto out;
out_free:
free(b->private_data);
out:
return ret;
}
void redtest(char *ifname, char *ifname2)
{
int cnt, i, j, oloop, iloop;
printf("Starting Redundant test\n");
/* initialise SOEM, bind socket to ifname */
// if (ec_init_redundant(ifname, ifname2))
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);
/* 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 */
ec_configdc();
/* 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(" Out:%8.8x,%4d In:%8.8x,%4d\n",
(int)ec_slave[cnt].outputs, ec_slave[cnt].Obytes, (int)ec_slave[cnt].inputs, ec_slave[cnt].Ibytes);
/* check for EL2004 or EL2008 */
if( !digout && ((ec_slave[cnt].eep_id == 0x0af83052) || (ec_slave[cnt].eep_id == 0x07d83052)))
{
digout = ec_slave[cnt].outputs;
}
}
expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
printf("Calculated workcounter %d\n", expectedWKC);
printf("Request operational state for all slaves\n");
ec_slave[0].state = EC_STATE_OPERATIONAL;
/* request OP state for all slaves */
ec_writestate(0);
/* activate cyclic process data */
dorun = 1;
/* wait for all slaves to reach OP state */
ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
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;
if (ec_slave[0].state == EC_STATE_OPERATIONAL )
{
printf("Operational state reached for all slaves.\n");
inOP = TRUE;
/* acyclic loop 5000 x 20ms = 10s */
for(i = 1; i <= 5000; i++)
{
printf("Processdata cycle %5d , Wck %3d, DCtime %12lld, dt %12lld, O:",
dorun, wkc , ec_DCtime, gl_delta);
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("\r");
fflush(stdout);
osal_usleep(20000);
}
dorun = 0;
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("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);
}
else
{
printf("No slaves found!\n");
}
printf("End redundant test, close socket\n");
/* stop SOEM, close socket */
ec_close();
}
else
{
printf("No socket connection on %s\nExcecute as root\n",ifname);
}
}
/*
* We spread data strips of req along with its parity strips onto replica for
* write operation. For read we only need to prepare data strip buffers.
*/
static struct req_iter *prepare_erasure_requests(struct request *req, int *nr)
{
uint32_t len = req->rq.data_length;
uint64_t off = req->rq.obj.offset;
int opcode = req->rq.opcode;
int start = off / SD_EC_DATA_STRIPE_SIZE;
int end = DIV_ROUND_UP(off + len, SD_EC_DATA_STRIPE_SIZE), i, j;
int nr_stripe = end - start;
struct fec *ctx;
int strip_size, nr_to_send;
struct req_iter *reqs;
char *p, *buf = NULL;
uint8_t policy = req->rq.obj.copy_policy ?:
get_vdi_copy_policy(oid_to_vid(req->rq.obj.oid));
int ed = 0, ep = 0, edp;
edp = ec_policy_to_dp(policy, &ed, &ep);
ctx = ec_init(ed, edp);
*nr = nr_to_send = (opcode == SD_OP_READ_OBJ) ? ed : edp;
strip_size = SD_EC_DATA_STRIPE_SIZE / ed;
reqs = xzalloc(sizeof(*reqs) * nr_to_send);
sd_debug("start %d, end %d, send %d, off %"PRIu64 ", len %"PRIu32,
start, end, nr_to_send, off, len);
for (i = 0; i < nr_to_send; i++) {
int l = strip_size * nr_stripe;
reqs[i].buf = xmalloc(l);
reqs[i].dlen = l;
reqs[i].off = start * strip_size;
switch (opcode) {
case SD_OP_CREATE_AND_WRITE_OBJ:
case SD_OP_WRITE_OBJ:
reqs[i].wlen = l;
break;
default:
break;
}
}
if (opcode != SD_OP_WRITE_OBJ && opcode != SD_OP_CREATE_AND_WRITE_OBJ)
goto out; /* Read and remove operation */
p = buf = init_erasure_buffer(req, SD_EC_DATA_STRIPE_SIZE * nr_stripe);
if (!buf) {
sd_err("failed to init erasure buffer %"PRIx64,
req->rq.obj.oid);
for (i = 0; i < nr_to_send; i++)
free(reqs[i].buf);
free(reqs);
reqs = NULL;
goto out;
}
for (i = 0; i < nr_stripe; i++) {
const uint8_t *ds[ed];
uint8_t *ps[ep];
for (j = 0; j < ed; j++)
ds[j] = reqs[j].buf + strip_size * i;
for (j = 0; j < ep; j++)
ps[j] = reqs[ed + j].buf + strip_size * i;
for (j = 0; j < ed; j++)
memcpy((uint8_t *)ds[j], p + j * strip_size,
strip_size);
ec_encode(ctx, ds, ps);
p += SD_EC_DATA_STRIPE_SIZE;
}
out:
ec_destroy(ctx);
free(buf);
return reqs;
}
void init_stuff()
{
int i;
int seed;
chdir(DATA_DIR);
#ifndef WINDOWS
setlocale(LC_NUMERIC,"en_US");
#endif
init_translatables();
//create_error_mutex();
init_globals();
init_crc_tables();
init_zip_archives();
cache_system_init(MAX_CACHE_SYSTEM);
init_texture_cache();
init_vars();
read_config();
file_check_datadir();
#ifdef LOAD_XML
//Well, the current version of the map editor doesn't support having a datadir - will add that later ;-)
load_translatables();
#endif
#ifdef LINUX
#ifdef GTK2
init_filters();
#else
file_selector = create_fileselection();
#endif
#endif //LINUX
init_gl();
window_resize();
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LESS);
// glDepthFunc(GL_LEQUAL);
glEnable(GL_TEXTURE_2D);
glShadeModel(GL_SMOOTH);
glFrontFace(GL_CCW);
glCullFace(GL_BACK);
glEnable(GL_NORMALIZE);
glClearColor( 0.0, 0.0, 0.0, 0.0 );
glClearStencil(0);
seed = time (NULL);
srand (seed);
init_texture_cache();
init_particles ();
init_e3d_cache();
init_2d_obj_cache();
for(i=0; i<256; i++)
tile_list[i]=0;
for (i = 0; i < MAX_LIGHTS; i++)
lights_list[i] = NULL;
new_map(256,256);
load_all_tiles();
//lights setup
build_global_light_table();
build_sun_pos_table();
reset_material();
init_lights();
//disable_local_lights();
//clear_error_log();
// Setup the new eye candy system
#ifdef EYE_CANDY
ec_init();
#endif //EYE_CANDY
init_gl_extensions();
if(have_multitexture)
#ifdef NEW_TEXTURES
ground_detail_text = load_texture_cached("./textures/ground_detail.bmp", tt_mesh);
#else /* NEW_TEXTURES */
ground_detail_text = load_texture_cache ("./textures/ground_detail.bmp",255);
#endif /* NEW_TEXTURES */
//load the fonts texture
init_fonts();
#ifdef NEW_TEXTURES
icons_text=load_texture_cached("./textures/gamebuttons.bmp", tt_gui);
buttons_text=load_texture_cached("./textures/buttons.bmp", tt_gui);
#else /* NEW_TEXTURES */
icons_text=load_texture_cache("./textures/gamebuttons.bmp",0);
buttons_text=load_texture_cache("./textures/buttons.bmp",0);
#endif /* NEW_TEXTURES */
//get the application home dir
have_multitexture=0;//debug only
#ifndef LINUX
GetCurrentDirectory(sizeof(exec_path),exec_path);
#else
exec_path[0]='.';exec_path[1]='/';exec_path[2]=0;
#endif
init_browser();
if(SDL_InitSubSystem(SDL_INIT_TIMER)<0)
{
char str[120];
snprintf(str, sizeof(str), "Couldn't initialize the timer: %s\n", SDL_GetError());
log_error(__FILE__, __LINE__, str);
SDL_Quit();
exit(1);
}
SDL_SetTimer (1000/(18*4), my_timer);
SDL_EnableUNICODE(1);
//we might want to do this later.
// creating windows
display_browser();
toggle_window(browser_win);
display_o3dow();
toggle_window(o3dow_win);
display_replace_window();
toggle_window(replace_window_win);
display_edit_window();
toggle_window(edit_window_win);
create_particles_window ();
}
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);
}
}
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;
}
main(int argc, char **argv)
{
char * eclipsedir = (char *) 0;
int c, new_argc, err;
int init_flags = INIT_SHARED|INIT_PRIVATE|INIT_ENGINE|INIT_PROCESS;
char * session, * nsrv_hostname;
unsigned nsrv_port_number;
uword size;
/*
* If stdio is not a tty, get rid of the console window. This is not ideal
* since the window flashes up briefly, but no better solution yet.
* (The correct way would be not to build eclipse.exe as a "console
* application" and have a WinMain() instead of main(). But then we have
* to do all the setup of stdin/out/err, argc/argv, environment etc
* ourselves)
*/
if (!isatty(_fileno(stdin))
&& !isatty(_fileno(stdout))
&& !isatty(_fileno(stderr)))
{
FreeConsole();
}
/*
* collect information from the command line
* remove some internally used arguments from the command line
*/
for (c = new_argc = 1; c < argc; )
{
if (argv[c][0] == '-' && argv[c][2] == 0) /* single char opt */
{
switch (argv[c][1])
{
case 'a': /* -a <worker> <session>
<nsrv_hostname> <nsrv_port_no> */
if (++c + 4 > argc) usage(argv[c-1]);
ec_set_option_int(EC_OPTION_PARALLEL_WORKER, atoi(argv[c++]));
session = argv[c++];
nsrv_hostname = argv[c++];
nsrv_port_number = atoi(argv[c++]);
break;
case 'c': /* -c <shared_map_file> */
if (++c + 1 > argc) usage(argv[c-1]);
ec_set_option_ptr(EC_OPTION_MAPFILE, argv[c++]);
ec_set_option_int(EC_OPTION_ALLOCATION, ALLOC_FIXED);
init_flags &= ~INIT_SHARED;
break;
case 'm': /* -m <shared_map_file> */
if (++c + 1 > argc) usage(argv[c-1]);
ec_set_option_ptr(EC_OPTION_MAPFILE, argv[c++]);
ec_set_option_int(EC_OPTION_ALLOCATION, ALLOC_FIXED);
break;
case 'b': /* -b <bootfile> */
argv[new_argc++] = argv[c]; /* shift */
if (++c + 1 > argc) usage(argv[c-1]);
argv[new_argc++] = argv[c++]; /* shift */
break;
case 'e': /* -e <goal> */
argv[new_argc++] = argv[c]; /* shift */
if (++c + 1 > argc) usage(argv[c-1]);
argv[new_argc++] = argv[c++]; /* shift */
break;
case 'g': /* -g <size> */
argv[new_argc++] = argv[c]; /* shift */
if (++c + 1 > argc) usage(argv[c-1]);
argv[new_argc++] = argv[c]; /* shift */
size = sizearg(argv[c++]);
ec_set_option_long(EC_OPTION_GLOBALSIZE, size);
if (size < MIN_GLOBAL) {
fprintf(stderr,"Global stack size out of range\n");
exit(-1);
}
break;
case 'd': /* -d <n> */
/* delay worker startup by <n> seconds */
if (++c + 1 > argc) usage(argv[c-1]);
Sleep(1000 * atoi(argv[c++]));
break;
case 'D': /* -D <eclipsedir> */
if (++c + 1 > argc) usage(argv[c-1]);
eclipsedir = argv[c++];
break;
case 'l': /* -l <size> */
argv[new_argc++] = argv[c]; /* shift */
if (++c + 1 > argc) usage(argv[c-1]);
argv[new_argc++] = argv[c]; /* shift */
size = sizearg(argv[c++]);
ec_set_option_long(EC_OPTION_LOCALSIZE, size);
if (size < MIN_LOCAL) {
fprintf(stderr,"Local stack size out of range\n");
exit(-1);
}
break;
case 'h': /* -h <size> */
argv[new_argc++] = argv[c]; /* shift */
if (++c + 1 > argc) usage(argv[c-1]);
argv[new_argc++] = argv[c]; /* shift */
size = sizearg(argv[c++]);
ec_set_option_long(EC_OPTION_PRIVATESIZE, size);
if (size < MIN_PRIVATE) {
fprintf(stderr,"Private heap size out of range\n");
exit(-1);
}
break;
case 's': /* -s <size> */
argv[new_argc++] = argv[c]; /* shift */
if (++c + 1 > argc) usage(argv[c-1]);
argv[new_argc++] = argv[c]; /* shift */
size = sizearg(argv[c++]);
ec_set_option_long(EC_OPTION_SHAREDSIZE, size);
if (size < MIN_SHARED) {
fprintf(stderr,"Shared heap size out of range\n");
exit(-1);
}
break;
case 'o': /* enable oracles */
c += 1;
/* vm_options = ORACLES_ENABLED; */
break;
case '-': /* -- give the rest to Prolog */
for (; c < argc; )
argv[new_argc++] = argv[c++];
break;
default: /* unknown: error */
usage(argv[c]);
break;
}
}
else if (!strcmp(argv[c], "-debug_level"))
{
if (++c + 1 > argc) usage(argv[c-1]);
ec_set_option_int(EC_OPTION_DEBUG_LEVEL, atoi(argv[c++]));
}
else /* raise error unless preceeded by a -- option */
{
usage(argv[c]);
}
}
/*----------------------------------------------------------------
* Entry point after longjmp(reset)
*----------------------------------------------------------------*/
switch (setjmp(reset))
{
case 0: /* raw boot or -r from above */
break;
case 3: /* restore program state */
case 2:
init_flags = REINIT_SHARED|INIT_ENGINE|INIT_PRIVATE;
break;
case 4: /* restore execution state */
init_flags = REINIT_SHARED|INIT_PRIVATE;
break;
case 1: /* reset after fatal error */
default:
init_flags = INIT_ENGINE;
switch (memory_corrupted++)
{
case 0:
break;
case 1:
/* try to print a message */
memory_corrupted = 2;
fprintf(stderr,"\n*** SEPIA Fatal error: memory corrupted\n");
/* fall to */
case 2:
/* we couldn't even print the message */
exit(-1);
}
break;
}
/*
* set up our own panic function which longjumps back to reset
*/
ec_set_option_ptr(EC_OPTION_PANIC, main_panic);
ec_set_option_int(EC_OPTION_INIT, init_flags);
ec_set_option_int(EC_OPTION_ARGC, new_argc);
ec_set_option_ptr(EC_OPTION_ARGV, argv);
if (eclipsedir)
ec_set_option_ptr(EC_OPTION_ECLIPSEDIR, eclipsedir);
ec_init();
ec_post_goal(ec_term(ec_did(":",2), ec_atom(ec_did("sepia_kernel",0)),
ec_atom(ec_did("standalone_toplevel",0))));
do {
err = ec_resume();
} while (err == PYIELD);
ec_cleanup();
return err;
}
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);
}
}
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);
}
}
