221 lines
5.7 KiB
C
221 lines
5.7 KiB
C
#include <ethercat.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#include "ecmbind.h"
|
|
|
|
int TIMEOUT_PROCESSDATA = 2000;
|
|
|
|
char ecd_iomap[4096];
|
|
int ecd_iomap_size;
|
|
int ecd_expected_wkc_size;
|
|
|
|
int ecmbind_version(char *versionString)
|
|
{
|
|
strcpy(versionString, "ecmbind/0.1.0");
|
|
return 0x00010000;
|
|
}
|
|
|
|
int ecmbind_initialize(char *ifname)
|
|
{
|
|
return ec_init(ifname);
|
|
}
|
|
|
|
int ecmbind_config_init()
|
|
{
|
|
ec_config_init(FALSE);
|
|
return ec_slavecount;
|
|
}
|
|
|
|
void* ecmbind_get_iomap() { return ecd_iomap; }
|
|
int ecmbind_get_expected_wkc_size() { return ecd_expected_wkc_size; }
|
|
|
|
int ecmbind_config_map()
|
|
{
|
|
ecd_pdo_map_length = 0;
|
|
|
|
ecd_iomap_size = ec_config_map(&ecd_iomap);
|
|
//ec_configdc();
|
|
ecd_expected_wkc_size = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
|
|
|
|
for (int slave=1;slave<=ec_slavecount;slave++)
|
|
ecd_read_pdo_map(slave);
|
|
|
|
return ecd_iomap_size;
|
|
}
|
|
|
|
uint16 ecmbind_read_state()
|
|
{
|
|
return ec_readstate();
|
|
}
|
|
|
|
uint16 ecmbind_get_slave_state(int slave)
|
|
{
|
|
return ec_slave[slave].state;
|
|
}
|
|
|
|
|
|
int ecmbind_write_slave_state(int slave, uint16 state)
|
|
{
|
|
ec_slave[slave].state = state;
|
|
return ec_writestate(slave);
|
|
}
|
|
|
|
uint16 ecmbind_request_state(int slave, uint16 reqState, int timeout)
|
|
{
|
|
return ec_statecheck(slave, reqState, timeout * 1000);
|
|
}
|
|
|
|
int ecmbind_processdata()
|
|
{
|
|
ec_send_processdata();
|
|
int wkc = ec_receive_processdata(EC_TIMEOUTRET);
|
|
return wkc;
|
|
}
|
|
|
|
int ecmbind_processdata2(char *managed_iomap, int size)
|
|
{
|
|
if (size != ecd_iomap_size)
|
|
return -1;
|
|
|
|
memcpy( ecd_iomap, managed_iomap, ecd_iomap_size);
|
|
|
|
ec_send_processdata();
|
|
int wkc = ec_receive_processdata(EC_TIMEOUTRET);
|
|
|
|
memcpy( managed_iomap, ecd_iomap, ecd_iomap_size);
|
|
|
|
return wkc;
|
|
}
|
|
|
|
int ecmbind_recover()
|
|
{
|
|
int slave = 0;
|
|
ec_group[0].docheckstate = FALSE;
|
|
ec_readstate();
|
|
for (slave = 1; slave <= ec_slavecount; slave++)
|
|
{
|
|
if ((ec_slave[slave].group == 0) && (ec_slave[slave].state != EC_STATE_OPERATIONAL))
|
|
{
|
|
ec_group[0].docheckstate = TRUE;
|
|
if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
|
|
{
|
|
printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave);
|
|
ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
|
|
ec_writestate(slave);
|
|
}
|
|
else if(ec_slave[slave].state == EC_STATE_SAFE_OP)
|
|
{
|
|
printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave);
|
|
ec_slave[slave].state = EC_STATE_OPERATIONAL;
|
|
ec_writestate(slave);
|
|
}
|
|
else if(ec_slave[slave].state > EC_STATE_NONE)
|
|
{
|
|
if (ec_reconfig_slave(slave, TIMEOUT_PROCESSDATA))
|
|
{
|
|
ec_slave[slave].islost = FALSE;
|
|
printf("MESSAGE : slave %d reconfigured\n",slave);
|
|
}
|
|
}
|
|
else if(!ec_slave[slave].islost)
|
|
{
|
|
/* re-check state */
|
|
ec_statecheck(slave, EC_STATE_OPERATIONAL, TIMEOUT_PROCESSDATA);
|
|
if (ec_slave[slave].state == EC_STATE_NONE)
|
|
{
|
|
ec_slave[slave].islost = TRUE;
|
|
printf("ERROR : slave %d lost\n",slave);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (ec_slave[slave].islost)
|
|
{
|
|
if(ec_slave[slave].state == EC_STATE_NONE)
|
|
{
|
|
if (ec_recover_slave(slave, TIMEOUT_PROCESSDATA))
|
|
{
|
|
ec_slave[slave].islost = FALSE;
|
|
printf("MESSAGE : slave %d recovered\n",slave);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
ec_slave[slave].islost = FALSE;
|
|
printf("MESSAGE : slave %d found\n",slave);
|
|
}
|
|
}
|
|
}
|
|
|
|
if(!ec_group[0].docheckstate)
|
|
{
|
|
printf("OK : all slaves resumed OPERATIONAL.\n");
|
|
ecd_expected_wkc_size = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
|
|
return ecd_expected_wkc_size;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
int ecmbind_get_pdo_entries_length() { return ecd_pdo_map_length; }
|
|
int ecmbind_get_pdo_entries(ecd_pdo_entry_t* table, int length)
|
|
{
|
|
if (length < ecd_pdo_map_length)
|
|
return -1;
|
|
|
|
int tableSize = (sizeof(ecd_pdo_entry_t)* ecd_pdo_map_length);
|
|
memcpy(table, ecd_pdo_map, tableSize);
|
|
|
|
return ecd_pdo_map_length;
|
|
}
|
|
|
|
int ecmbind_pdo_read(int slave, int index, int subindex, char* buffer, int size)
|
|
{
|
|
return -1;
|
|
}
|
|
int ecmbind_pdo_write(int slave, int index, int subindex, char* buffer, int size)
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
int ecmbind_sdo_read(int slave, int index, int subindex, char* buffer, int size)
|
|
{
|
|
int wkc = ec_SDOread(slave, index, subindex, FALSE, &size, buffer, 100000);
|
|
if (wkc <= 0)
|
|
return -1;
|
|
return size;
|
|
}
|
|
int ecmbind_sdo_write(int slave, int index, int subindex, char* buffer, int size)
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
/*
|
|
typedef void (*cb_enum_pdo)(int slave, int index, int subindex, int addr_offset, int addr_bit, int bitlength);
|
|
*/
|
|
int ecmbind_pdo_enumerate(cb_enum_pdo cb)
|
|
{
|
|
printf("libecmbind: ecmbind_pdo_enumerate(): enumerating %d PDOs\n", ecd_pdo_map_length);
|
|
for (int n=0;n<ecd_pdo_map_length;n++)
|
|
{
|
|
cb(ecd_pdo_map[n].slave, ecd_pdo_map[n].index, ecd_pdo_map[n].subindex, ecd_pdo_map[n].addr_offset, ecd_pdo_map[n].addr_bit, ecd_pdo_map[n].bitlength);
|
|
}
|
|
}
|
|
|
|
int ecmbind_iomap_get(int offset, char* buffer, int length)
|
|
{
|
|
if (offset + length > ecd_iomap_size)
|
|
return -1;
|
|
memcpy(buffer, &ecd_iomap[offset], length);
|
|
return length;
|
|
}
|
|
int ecmbind_iomap_set(int offset, char* buffer, int length)
|
|
{
|
|
if (offset + length > ecd_iomap_size)
|
|
return -1;
|
|
|
|
memcpy(&ecd_iomap[offset], buffer, length);
|
|
}
|
|
|
|
|