Commit 5815b469 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

work-in-progress-daan-2013-06-06-op-naar-cp2-v2.diff

parent bab5bbe6
......@@ -71,6 +71,8 @@ struct soe_cmd {
#define ERR_NO_ERROR 0x0000
#define ERR_SVC_CHAN_NOT_OPEN 0x0001
#define ERR_IDN_NOT_SUPPORTED 0x1001
#define ERR_OPP_DATA_TOO_SHORT 0x7002
#define ERR_INVALID_OPP_DATA 0x7008
//TODO: move to generic sercos file
static char *sercos_errstr(uint16_t errnr)
......@@ -83,6 +85,10 @@ static char *sercos_errstr(uint16_t errnr)
return "service channel not open";
case ERR_IDN_NOT_SUPPORTED:
return "IDN not supported";
case ERR_OPP_DATA_TOO_SHORT:
return "operation data transmission too short";
case ERR_INVALID_OPP_DATA:
return "invalid operation data";
default:
return "unknown";
}
......@@ -133,8 +139,8 @@ static ssize_t ax5xxx_sercos_idn_write(struct sercos_bus *bus,
ret = esc_mailbox_write(bpriv->dev->mailbox, ESC_MAILBOX_TYPE_SOE,
&cmd, len + 4, timeout);
log_send(LOG_T_DEBUG, "ax5xxx_sercos_idn_write: mailbox write ret=%zo",
ret);
log_send(LOG_T_DEBUG, "ax5xxx_sercos_idn_write: mailbox write "
"ret=%zo", ret);
if (ret != len + 4) {
log_send(LOG_T_DEBUG, "ax5xxx_sercos_idn_write: failed to "
"write %zu bytes to mailbox. (ret=%zo)", len + 4, ret);
......@@ -142,7 +148,7 @@ static ssize_t ax5xxx_sercos_idn_write(struct sercos_bus *bus,
}
ret = esc_mailbox_read(bpriv->dev->mailbox, &type,
&cmd, 4, timeout);
&cmd, 6, timeout);
// we should at least receive a header
if (ret < 0) {
......@@ -337,7 +343,7 @@ static int ax5xxx_sercos_phase_request(struct sercos_bus *bus,
}
if (esc_al_state_set(&bpriv->dev->addr, state, &timeout) < 0) {
log_send(LOG_T_ERROR, "Could not go to phase %d\n", phase);
log_send(LOG_T_ERROR, "Could not go to phase %d", phase);
return -1;
}
......@@ -349,6 +355,7 @@ static struct beckhoff_ax5xxx_type devlist[] = {
};
#include <stdio.h>
#include <ctype.h>
// todo: move to sercos specific file
// see http://support1.motioneng.com/soft/zSercos/procedures.htm
......@@ -365,25 +372,99 @@ static struct beckhoff_ax5xxx_type devlist[] = {
#define PROC_NOT_DONE (1 << 2)
#define PROC_DONE (0 << 2)
#define IDN_S false
#define IDN_P true
// todo: move to sercos specific file
#define IDN_C1DIAG sercos_idn(false, 0, 11) // page 46
#define IDN_TELEGRAM_TYPE sercos_idn(false, 0, 15) // page 53
#define IDN_ALL_OPP_DATA_LIST sercos_idn(false, 0, 17) // page 56
#define IDN_CP2_INVALID_LIST sercos_idn(false, 0, 21) // page 57
#define IDN_CP3_INVALID_LIST sercos_idn(false, 0, 22) // page 58
#define IDN_DIAG_MSG sercos_idn(false, 0, 95) // page 99
#define IDN_RESET_PROC sercos_idn(false, 0, 99) // page 99
#define IDN_CP3_TRANS_CHECK sercos_idn(false, 0, 127) // page 118
#define IDN_DRIVE_CTRL sercos_idn(false, 0, 134) // page 124
#define IDN_DRIVE_STATUS sercos_idn(false, 0, 135) // page 128
#define IDN_C1DIAG sercos_idn(IDN_S, 0, 11) // page 46
#define IDN_TELEGRAM_TYPE sercos_idn(IDN_S, 0, 15) // page 53
#define IDN_ALL_OPP_DATA_LIST sercos_idn(IDN_S, 0, 17) // page 56
#define IDN_CP2_OPP_DATA_LIST sercos_idn(IDN_S, 0, 18) // page 56
#define IDN_CP2_INVALID_LIST sercos_idn(IDN_S, 0, 21) // page 57
#define IDN_CP3_INVALID_LIST sercos_idn(IDN_S, 0, 22) // page 58
#define IDN_DIAG_MSG sercos_idn(IDN_S, 0, 95) // page 99
#define IDN_RESET_PROC sercos_idn(IDN_S, 0, 99) // page 99
#define IDN_CP3_TRANS_CHECK sercos_idn(IDN_S, 0, 127) // page 118
#define IDN_DRIVE_CTRL sercos_idn(IDN_S, 0, 134) // page 124
#define IDN_DRIVE_STATUS sercos_idn(IDN_S, 0, 135) // page 128
// activate parametrization
#define IDN_PROC_ACTIVATE_PL sercos_idn(false, 0, 420) // page 328
#define IDN_COMM_CYCLE_TIME sercos_idn(false, 0, 1002) // page 479
#define IDN_DEVICE_STATUS sercos_idn(false, 0, 1045) // page 517
#define IDN_PROC_ACTIVATE_PL sercos_idn(IDN_S, 0, 420) // page 328
#define IDN_COMM_CYCLE_TIME sercos_idn(IDN_S, 0, 1002) // page 479
#define IDN_DEVICE_STATUS sercos_idn(IDN_S, 0, 1045) // page 517
// http://infosys.beckhoff.com/italiano.php?content=../content/1040/ax5000_idn-description/html/s-0-0032.htm&id=8446
#define IDN_PRIMARY_OPP_MODE sercos_idn(IDN_S, 0, 32) // page 62
#define OPP_MODE_NONE 0
#define OPP_MODE_TORQUE 1
#define OPP_MODE_VELO 2
#define OPP_MODE_POS_FB1 3
#define OPP_MODE_POS_FB2 4
#define OPP_MODE_POS_FB1_LAG_LESS1 11
#define OPP_MODE_POS_FB2_LAG_LESS 12
enum idn_type {
UINT16 = 0,
UINT32,
STRING,
};
union idn_value {
uint16_t v_uint16;
uint32_t v_uint32;
char *v_str;
};
struct idn_setting {
uint16_t idn;
enum idn_type type;
union idn_value val;
};
#define IDN_BH_CONFIGURED_MOTOR sercos_idn(IDN_P, 0, 53)
struct idn_setting settings_list_cp2[] = {
{ IDN_PRIMARY_OPP_MODE, UINT16, .val.v_uint16 = OPP_MODE_POS_FB1 },
// P-0-0050, Motor construction type
// P-0-0051, Number of pole pairs
// P-0-0052, Time limitation for peak current
// P-0-0053, Configured motor type
// see: ftp://ftp.beckhoff.com.cn/training_cd/%D6%D0%BC%B6%C5%E0%D1%B5/Drive%20Technology/AX5000%CB%C5%B7%FE%C7%FD%B6%AF%C6%F7%B4%F8AM8000%B5%E7%BB%FA%B5%C4%CB%B5%C3%F7/2012_02_09_Motorfiles/2012_02_09_Motorfiles/BECKHOFF%20AM8033-xFxx.xml
// test motor= AM8033-0F10-0000, Io=4.10A, Nn=6000 1/min, Mo=3.22Nm
// { IDN_BH_CONFIGURED_MOTOR, STRING, .val.v_str = "AM8033-0F10-0000" },
{ IDN_BH_CONFIGURED_MOTOR, STRING, .val.v_str = "AM8033-0F00-0000" },
// zie 'BECKHOFF AM8033-xFxx.xml'
// ElectricParameterset voor AM8033-xFxx
{ sercos_idn(IDN_P, 0, 50), UINT16, .val.v_uint16 = 0 },
// Maximum motor speed (rpm)
{ sercos_idn(IDN_S, 0, 113), UINT32, .val.v_uint32 = 6500 },
// Number of pole pairs
{ sercos_idn(IDN_P, 0, 51), UINT16, .val.v_uint16 = 4 },
// Motor EMF (mV/rpm)
{ sercos_idn(IDN_P, 0, 55), UINT16, .val.v_uint16 = 58 },
// P-0-0054, Configured drive type
// P-0-0055, Motor EMF
// P-0-0056, Max motor speed with max torque
// P-0-0059, Motor brake current monitoring level
// P-0-0060, Motor brake
// P-0-0061, Motor temperature sensor type
// P-0-0062, Thermal motor model
// P-0-0066, Electric motor model
// P-0-0067, Motor winding: Dielectric strength
// P-0-0068, Thermal overload factor (motor winding)
// P-0-0069, Commutation monitoring
// P-0-0070, Motor continuous stall torque
// P-0-0071, Mechanical motor data
// P-0-0072, Motor brake data
// P-0-0073, Motor peak torque
{ 0, 0 },
};
// for device specific parameters:
// see http://infosys.beckhoff.com/italiano.php?content=../content/1040/ax5000_idn-description/html/p-1-0093-ax5203.htm&id=8164
// for device specific parameters
// todo: move to sercos specific file
......@@ -446,39 +527,48 @@ static void debug_show_drive_status(uint16_t status)
{
// decodes S-0-0135 (drive status word) in human readable format
fprintf(stderr, "DAAN: drive %s command values\n",
log_send(LOG_T_DEBUG, "DAAN: drive %s command values",
(status & (1 << 3)) ? "follows" : "ignores");
fprintf(stderr, "DAAN: operating mode=%x\n", (status >> 8) & 0x07);
log_send(LOG_T_DEBUG, "DAAN: operating mode=%x",
(status >> 8) & 0x07);
switch (status >> 14) {
case 0:
fprintf(stderr, "drive not ready for power-up. internal "
"checks not yet concluded successfully\n");
log_send(LOG_T_DEBUG, "drive not ready for power-up. "
"internal checks not yet concluded successfully");
break;
case 1:
fprintf(stderr, "drive logic ready for main power on\n");
log_send(LOG_T_DEBUG, "drive logic ready for main power "
"on");
break;
case 2:
fprintf(stderr, "drive ready and main power on. drive is "
"free of torque\n");
log_send(LOG_T_DEBUG, "drive ready and main power on. drive "
"is free of torque");
break;
case 3:
fprintf(stderr, "power stage active. drive on\n");
log_send(LOG_T_DEBUG, "power stage active. drive on");
break;
}
}
static void debug_print_idn(uint16_t idn_nr)
{
log_send(LOG_T_DEBUG, " %c-%d-%04d", idn_nr & 0x8000 ? 'P' : 'S',
(idn_nr >> 12) & 0x07, idn_nr & 0x0fff);
}
static void dump_idn_list(uint8_t *buf, ssize_t len)
{
uint16_t act_len;
uint16_t idn_nr;
fprintf(stderr, "IDN list:\n");
log_send(LOG_T_DEBUG, "IDN list:");
act_len = buf[0] + (buf[1] << 8);
//buf[2..3] = max_len
if (act_len > len - 4) {
fprintf(stderr, " (list is truncated. %d != %d)\n", act_len,
len);
log_send(LOG_T_DEBUG, " (list is truncated. %d != %zd)",
act_len, len);
}
len -= 4;
......@@ -487,10 +577,234 @@ static void dump_idn_list(uint8_t *buf, ssize_t len)
idn_nr = buf[0] + (buf[1] << 8);
len -= 2;
buf += 2;
debug_print_idn(idn_nr);
}
}
static void dump_idn_string(struct sercos_bus *bus, uint8_t drive_nr,
uint8_t element, uint16_t idn, struct timespec *timeout)
{
ssize_t result;
uint16_t size;
int total_len, cnt;
char *buf;
// get 'actual len' part of IDN first
result = ax5xxx_sercos_idn_read(bus, drive_nr, element, idn,
&size, sizeof(size), timeout);
if (result < 0)
return;
// allocate needed amount of memory
size = le16toh(size);
total_len = 2 * 2 + size;
buf = malloc(total_len + 1);
if (buf == NULL)
return;
// request entire string. (note: there's no way to guarantee that
// it's size didn't change in the mean time..)
result = ax5xxx_sercos_idn_read(bus, drive_nr, element, idn, buf,
total_len, timeout);
if (result < 0)
goto err_out;
size = buf[0] + (buf[1] << 8);
if (size + 4 < total_len)
total_len = size + 4;
buf[total_len] = '\0';
for (cnt = 4; cnt < total_len; cnt++) {
if (! isprint(buf[cnt]))
buf[cnt] = '.';
}
log_send(LOG_T_DEBUG, "idn_string: (len=%d) '%s'", size, &buf[4]);
err_out:
if (buf != NULL)
free(buf);
}
static const char *idn_types[] = {
"binary number",
"unsigned integer",
"signed integer",
"unsigned hex.",
"UFT8",
"IDN",
"floating point",
"time",
};
static const char *idn_len[] = {
"reserved",
"2 bytes",
"4 bytes",
"8 bytes",
"var. 1 byte",
"var. 2-byte",
"var. 4-byte",
"var. 8-byte",
};
static void dump_idn(struct sercos_bus *bus, uint8_t drive_nr, uint16_t idn,
struct timespec *timeout)
{
uint32_t attribute;
ssize_t result;
int type, len;
char *type_str;
int printed;
uint16_t val16;
uint32_t val32;
// see 'Parameters' PDF, page 20
debug_print_idn(idn);
dump_idn_string(bus, drive_nr, IDN_EL_NAME, idn, timeout);
result = ax5xxx_sercos_idn_read(bus, drive_nr, IDN_EL_ATTRIBUTE, idn,
&attribute, sizeof(attribute), timeout);
if (result < 0) {
log_send(LOG_T_ERROR, "failed to read idn attribute!");
return;
}
attribute = le32toh(attribute);
if (attribute & (1 << 19)) {
log_send(LOG_T_DEBUG, "IDN type=procedure command");
return;
}
type = (attribute >> 20) & 0x07;
len = (attribute >> 16) & 0x07;
log_send(LOG_T_DEBUG, "IDN type=%s, len=%s", idn_types[type],
idn_len[len]);
printed = 0;
if ((type < 4) && (len == 1)) {
// 2 bytes signed, unsigned or hex
result = ax5xxx_sercos_idn_read(bus, drive_nr, IDN_EL_DATA,
idn, &val16, sizeof(val16), timeout);
if (result < 0) {
log_send(LOG_T_ERROR, "failed to read idn data!");
return;
}
val16 = le16toh(val16);
log_send(LOG_T_DEBUG, " data: %d (0x%x)", val16, val16);
printed = 1;
}
if ((type < 4) && (len == 2)) {
// 4 bytes signed, unsigned or hex
result = ax5xxx_sercos_idn_read(bus, drive_nr, IDN_EL_DATA,
idn, &val32, sizeof(val32), timeout);
if (result < 0) {
log_send(LOG_T_ERROR, "failed to read idn data!");
return;
}
val32 = le32toh(val32);
log_send(LOG_T_DEBUG, " data: %d (0x%x)", val32, val32);
printed = 1;
}
if ((type == 4) && (len == 4)) {
// UTF8, variable length. (string)
dump_idn_string(bus, drive_nr, IDN_EL_DATA, idn, timeout);
printed = 1;
}
if (! printed) {
log_send(LOG_T_DEBUG, "don't known how to print this type");
}
}
ssize_t sercos_idn_write_str(struct sercos_bus *bus, uint8_t drive_nr,
uint8_t element, uint16_t idn, char *str, struct timespec *timeout)
{
ssize_t result;
int len;
char *buf;
len = strlen(str);
buf = malloc(len + 4);
if (buf == NULL)
return -1;
// see sercos 'Parameters' PDF, page 25, chapter 1.8
buf[0] = len & 0xff;
buf[1] = len >> 8;
// 'during write, the slave shall ignore index 3 and 4'
buf[2] = 0;
buf[3] = 0;
memcpy(buf + 4, str, len);
result = ax5xxx_sercos_idn_write(bus, drive_nr, element, idn, buf,
len + 4, timeout);
log_send(LOG_T_DEBUG, "trying to write string (%d bytes req.). "
"result=%zd", len + 4, result);
free(buf);
return result;
}
static ssize_t ax5xxx_send_idn_list(struct sercos_bus *bus, uint8_t drive_nr,
struct idn_setting *list, struct timespec *timeo)
{
ssize_t result;
uint16_t val16;
uint32_t val32;
result = 0;
while (list->idn != 0) {
debug_print_idn(list->idn);
dump_idn(bus, drive_nr, list->idn, timeo);
switch (list->type) {
case UINT16:
val16 = htole16(list->val.v_uint16);
result = ax5xxx_sercos_idn_write(bus, drive_nr,
IDN_EL_DATA, list->idn, &val16, sizeof(val16),
timeo);
result = ax5xxx_sercos_idn_read(bus, drive_nr,
IDN_EL_DATA, list->idn, &val16, sizeof(val16),
timeo);
val16 = le16toh(val16);
log_send(LOG_T_DEBUG, "DAAN: read back: res:%zd, "
"val=%d (0x%x)", result, val16, val16);
break;
case UINT32:
val32 = htole32(list->val.v_uint32);
result = ax5xxx_sercos_idn_write(bus, drive_nr,
IDN_EL_DATA, list->idn, &val32, sizeof(val32),
timeo);
result = ax5xxx_sercos_idn_read(bus, drive_nr,
IDN_EL_DATA, list->idn, &val32, sizeof(val32),
timeo);
val32 = le32toh(val32);
log_send(LOG_T_DEBUG, "DAAN: read back: res:%zd, "
"val=%d (0x%x)", result, val32, val32);
break;
case STRING:
dump_idn_string(bus, drive_nr, IDN_EL_DATA, list->idn,
timeo);
fprintf(stderr, " %c-%d-%04d\n", idn_nr & 0x8000 ? 'P' : 'S',
(idn_nr >> 12) & 0x07, idn_nr & 0x0fff);
result = sercos_idn_write_str(bus, drive_nr,
IDN_EL_DATA, list->idn, list->val.v_str, timeo);
dump_idn_string(bus, drive_nr, IDN_EL_DATA, list->idn,
timeo);
break;
}
log_send(LOG_T_DEBUG, "DAAN: setting IDN. result=%zd",
result);
if (result < 0)
return result;
list++;
}
return 0;
}
struct controller_block * block_beckhoff_ax5xxx_create(char *name, va_list ap)
......@@ -501,9 +815,11 @@ struct controller_block * block_beckhoff_ax5xxx_create(char *name, va_list ap)
int dev = -1;
char *axtype;
int devno;
struct sercos_bus *bus;
uint32_t vendor, product;
ssize_t result;
struct timespec timeo;
uint16_t val;
timeo.tv_sec = 2;
timeo.tv_nsec = 0;
......@@ -519,7 +835,7 @@ struct controller_block * block_beckhoff_ax5xxx_create(char *name, va_list ap)
}
if (dev < 0) {
log_send(LOG_T_ERROR, "%s: Device type '%s' is not supported "
"by this block\n", name, axtype);
"by this block", name, axtype);
goto err_ethercat;
}
......@@ -529,8 +845,8 @@ struct controller_block * block_beckhoff_ax5xxx_create(char *name, va_list ap)
if (vendor != ESC_ESI_VENDORID_BECKHOFF ||
product != devlist[dev].productcode) {
log_send(LOG_T_ERROR, "%s: Device '%s' not found. (%x != %x)"
"\n", name, axtype, product, devlist[dev].productcode);
log_send(LOG_T_ERROR, "%s: Device '%s' not found. (%x != %x)",
name, axtype, product, devlist[dev].productcode);
goto err_ethercat;
}
......@@ -547,19 +863,18 @@ struct controller_block * block_beckhoff_ax5xxx_create(char *name, va_list ap)
// exit(1);
block->private->bus = sercos_bus_alloc(name,
sizeof(struct sercos_bus_private));
if (!block->private->bus)
bus = sercos_bus_alloc(name, sizeof(struct sercos_bus_private));
if (bus == NULL)
goto err_bus;
block->private->bus->private->block = block;
block->private->bus->phase_get = ax5xxx_sercos_phase_get;
block->private->bus->phase_request = ax5xxx_sercos_phase_request;
block->private->bus->idn_write = ax5xxx_sercos_idn_write;
block->private->bus->idn_read = ax5xxx_sercos_idn_read;
block->private->bus->at_allocate = ax5xxx_sercos_at_allocate;
block->private->bus->mdt_allocate = ax5xxx_sercos_mdt_allocate;
block->private->bus = bus;
bus->private->block = block;
bus->phase_get = ax5xxx_sercos_phase_get;
bus->phase_request = ax5xxx_sercos_phase_request;
bus->idn_write = ax5xxx_sercos_idn_write;
bus->idn_read = ax5xxx_sercos_idn_read;
bus->at_allocate = ax5xxx_sercos_at_allocate;
bus->mdt_allocate = ax5xxx_sercos_mdt_allocate;
block->private->phase = 2;
......@@ -567,10 +882,15 @@ struct controller_block * block_beckhoff_ax5xxx_create(char *name, va_list ap)
controller_block_add(block);
sercos_bus_add(block->private->bus);
sercos_bus_add(bus);
fprintf(stderr, "DAAN: test 1\n");
log_send(LOG_T_DEBUG, "DAAN: test 1");
// try to set all IDNs in the settings list that are needed to
// transition to CP2
result = ax5xxx_send_idn_list(bus, 0, settings_list_cp2, &timeo);
// try to go to CP2 (which maps to Ethercat state 'Safe-operational')
result = esc_al_state_set(&addr, ESC_AL_STATE_SAFE_OPERATIONAL,
&timeo);
if (result < 0)
......@@ -581,51 +901,65 @@ struct controller_block * block_beckhoff_ax5xxx_create(char *name, va_list ap)
// log_send(LOG_T_ERROR, "%s: Could not go to state operational",
// name);
fprintf(stderr, "DAAN: test 1\n");
log_send(LOG_T_DEBUG, "DAAN: test 1");
// exit(1);
if (1) {
uint32_t val32;
uint16_t val;
uint8_t buf[2048];
int cnt;
#if 0
result = sercos_exec_proc(block->private->bus, 0, IDN_RESET_PROC,
dump_idn_string(bus, 0, IDN_EL_DATA, sercos_idn(IDN_S, 0, 432),
&timeo);
fprintf(stderr, "DAAN: reset proc result=%zd\n", result);
result = sercos_exec_proc(block->private->bus, 0,
IDN_PROC_ACTIVATE_PL, &timeo);
fprintf(stderr, "DAAN: activate parametrization result=%zd\n",
#if 0
result = sercos_exec_proc(bus, 0, IDN_RESET_PROC, &timeo);
log_send(LOG_T_DEBUG, "DAAN: reset proc result=%zd", result);
result = sercos_exec_proc(bus, 0, IDN_PROC_ACTIVATE_PL, &timeo);
log_send(LOG_T_DEBUG, "DAAN: activate parametrization result=%zd",
result);
#endif
#if 0
// get list of all operation IDNs
result = ax5xxx_sercos_idn_read(bus, 0, IDN_EL_DATA,
IDN_ALL_OPP_DATA_LIST, buf, sizeof(buf), &timeo);
log_send(LOG_T_DEBUG, "DAAN: All operational parameters IDN-list "
"result=%zd, data:", result);
if (result > 2) {
dump_idn_list(buf, result);
}
#endif
// get list of IDNs that should be sent for CP2
for (cnt = 0; cnt < sizeof(buf); cnt++) {
buf[cnt] = 0x55;
}
result = ax5xxx_sercos_idn_read(block->private->bus, 0, IDN_EL_DATA,
IDN_ALL_OPP_DATA_LIST, buf, sizeof(buf), &timeo);
fprintf(stderr, "DAAN: All operational parameters IDN-list "
"result=%zd, data:\n", result);
result = ax5xxx_sercos_idn_read(bus, 0, IDN_EL_DATA,
IDN_CP2_OPP_DATA_LIST, buf, sizeof(buf), &timeo);
log_send(LOG_T_DEBUG, "DAAN: operational parameters for CP2: "
"result=%zd, data:", result);
if (result > 2) {
dump_idn_list(buf, result);
}
// get list of all IDNs that prevent the drive from going to state CP2
result = ax5xxx_sercos_idn_read(block->private->bus, 0, IDN_EL_DATA,
result = ax5xxx_sercos_idn_read(bus, 0, IDN_EL_DATA,
IDN_CP2_INVALID_LIST, buf, sizeof(buf), &timeo);
fprintf(stderr, "DAAN: IDNs that keep us from going to CP2 list: "
"result=%zd\n", result);
log_send(LOG_T_DEBUG, "DAAN: IDNs that keep us from going to CP2 "
"list: result=%zd", result);