Commit 2ee125b1 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Status na een avondje rommelen met de drive

Hiermee komt de drive in operational mode (mogelijk na een paar keer
aandringen).
De encoder is echter nog niet uit te lezen, en er gaat ook nog geen
stroom naar de motor
parent 2369d2ac
......@@ -3,3 +3,4 @@ status.cgi
trace.cgi
shell.cgi
trace.cgi
console_version.js
......@@ -254,7 +254,7 @@ static void *sample_monitor(void *arg)
log_send(LOG_T_DEBUG, linebuf);
sample_timing_snprintf(&st_sample, linebuf, 200);
log_send(LOG_T_DEBUG, linebuf);
sleep(1);
sleep(60);
}
return NULL;
}
......
......@@ -263,6 +263,11 @@ struct controller_block * block_beckhoff_ax5xxx_create(char *name, va_list ap)
block->private->dev = esc_device_create(&addr, name);
esc_esi_device_fill(block->private->dev);
block->private->dev->tx_pdo_sm = 2; //0x1100 0x24
// block->private->dev->sm[2].start = 0x1100;
block->private->dev->rx_pdo_sm = 3; //0x1180 0x20
// block->private->dev->sm[3].start = 0x1180;
esc_device_initialize_pre_operational(block->private->dev);
bus = esc_soe_create(name, block->private->dev);
......
......@@ -240,6 +240,8 @@ int ec_tx_pdo_add(struct ec_dgram_addr *addr, unsigned char **data, size_t len)
dgram->cmd = EC_DGRAM_HDR_CMD_APWR;
dgram->addr.position.pos = addr->addr.position.pos;
dgram->addr.position.off = addr->addr.position.off;
log_send(LOG_T_DEBUG, "Address: %x.%x",
addr->addr.position.pos, addr->addr.position.off);
break;
default:
printf("Addressing mode not implemented!\n");
......
......@@ -113,8 +113,8 @@ int esc_al_state_set(struct ec_dgram_addr *addr, enum esc_al_state newstate,
"Could not read status");
return -1;
} else if (status != 0) {
log_send(LOG_T_DEBUG,
"status code: 0x%x %s", status,
log_send(LOG_T_ERROR,
"Failed to go to state, status code: 0x%x %s", status,
esc_al_status2str(status));
esc_al_error_ack(addr);
......@@ -252,7 +252,7 @@ int esc_pdo_tx_set(struct ec_dgram_addr *addr,
ESC_SYNCMANAGER_CONTROL_INT_PDI;
if (watchdog)
val8 |= ESC_SYNCMANAGER_CONTROL_WATCHDOG_ENABLE;
log_send(LOG_T_DEBUG, "PDO tx Control byte: 0x%02x", val8);
log_send(LOG_T_DEBUG, "PDO tx Control byte: 0x%02x, SM%d", val8, sm->sm);
ec_datagram_write(addr, &val8, 1);
......@@ -297,7 +297,8 @@ int esc_pdo_rx_set(struct ec_dgram_addr *addr,
addr->addr.position.off = ESC_ADDR_MAP_SYNCMANAGER_NR(sm->sm) |
ESC_ADDR_MAP_SYNCMANAGER_CONTROL;
val8 = ESC_SYNCMANAGER_CONTROL_INT_PDI;
log_send(LOG_T_DEBUG, "PDO rx Control byte: 0x%02x", val8);
val8 |= 0x02;
log_send(LOG_T_DEBUG, "PDO rx Control byte: 0x%02x, SM%d", val8, sm->sm);
ec_datagram_write(addr, &val8, 1);
if (sm->len) {
......
......@@ -133,6 +133,7 @@ int esc_dc_init(struct ec_dgram_addr *addr, struct esc_dc_sync *sync)
if (sync->sync_out_active) {
log_send(LOG_T_DEBUG, "\tEnable Sync Out");
}
sync->sync0_cycle = 250000;
if (sync->sync0_active) {
log_send(LOG_T_DEBUG, "\tEnable SYNC0 with %d ns cycle and %d ns shift",
sync->sync0_cycle, sync->sync0_shift);
......@@ -162,6 +163,7 @@ int esc_dc_init(struct ec_dgram_addr *addr, struct esc_dc_sync *sync)
}
if (sync->sync1_active) {
sync->sync1_shift = 1750000;
log_send(LOG_T_DEBUG, "\tEnable SYNC1 with %d ns shift",
sync->sync1_shift);
addr->addr.position.off = ESC_ADDR_MAP_DC_SYNC1_CYCLE_TIME;
......
......@@ -55,6 +55,13 @@ struct soe_cmd {
unsigned char data[200];
} __packed;
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 ssize_t soe_idn_read(struct sercos_bus *bus,
unsigned char devnr, uint8_t element, uint16_t idn, void *res, size_t len,
struct timespec *timeout)
......@@ -196,9 +203,10 @@ static ssize_t soe_idn_write(struct sercos_bus *bus,
if (cmd.cmd_inc_err_drive & CMD_FLAG_ERR) {
// oops. error received :-s
errnr = cmd.data[0] + (cmd.data[1] << 8);
log_send(LOG_T_DEBUG, "%s: soe_idn_write: drive "
log_send(LOG_T_DEBUG, "%s: soe_idn_write(0x%04x): drive "
"responded with error flag set (err code=%04x, %s)",
bus->name, errnr, bus->errstr(errnr));
bus->name, idn, errnr, bus->errstr(errnr));
debug_print_idn(idn);
return -1;
}
if ((cmd.cmd_inc_err_drive & CMD_MASK) != ESC_SOE_CMD_WRITE_RESP) {
......@@ -243,14 +251,8 @@ static int soe_phase_get(struct sercos_bus *bus, unsigned char devnr)
return bus->private->phase;
}
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)
void dump_idn_list(uint8_t *buf, ssize_t len)
{
uint16_t act_len;
uint16_t idn_nr;
......@@ -299,10 +301,25 @@ static int soe_phase_request(struct sercos_bus *bus,
if (esc_al_state_set(&priv->esc->addr, state, &timeout) < 0) {
ssize_t result;
uint8_t buf[2048];
uint32_t val32;
log_send(LOG_T_ERROR, "%s: Could not go to phase %d",
bus->name, phase);
result = bus->idn_read(bus, 0, SERCOS_IDN_EL_DATA,
SERCOS_IDN_DIAGNOSTIC_NUMBER, &val32, sizeof(val32),
&timeout);
if (result == sizeof(val32)) {
log_send(LOG_T_ERROR, "%s: Diagnostic number: 0x%04x",
bus->name, le32toh(val32));
}
result = sercos_idn_read_str(bus, 0, SERCOS_IDN_EL_DATA,
SERCOS_IDN_DIAG_MSG, (char *)buf, sizeof(buf), &timeout);
if (result > 0) {
log_send(LOG_T_ERROR, "%s: Diag message: %s",
bus->name, buf);
}
switch(phase) {
case 4:
// get list of all IDNs that prevent the drive from
......@@ -333,6 +350,7 @@ static int soe_phase_request(struct sercos_bus *bus,
} else {
log_send(LOG_T_DEBUG, " list is empty..");
}
break;
case 2:
default:
......@@ -357,8 +375,10 @@ static ssize_t soe_at_len(struct sercos_bus *bus, unsigned char devnr)
result = bus->idn_read(bus, devnr, SERCOS_IDN_EL_DATA,
SERCOS_IDN_TELEGRAM_TYPE, &val16, sizeof(val16), &timeout);
if (result < 0)
if (result < 0) {
log_send(LOG_T_ERROR, "Could not get telegram type");
return -1;
}
type = le16toh(val16);
type &= SERCOS_TELEGRAM_TYPE_MASK;
......@@ -375,7 +395,7 @@ static ssize_t soe_at_len(struct sercos_bus *bus, unsigned char devnr)
return -1;
items = result / 2;
for (i = 0; i < items; i++) {
for (i = 2; i < items; i++) {
result = sercos_idn_len_get(bus, devnr,
list[i], &timeout);
if (result < 0)
......@@ -393,7 +413,7 @@ static ssize_t soe_at_len(struct sercos_bus *bus, unsigned char devnr)
break;
}
return result;
return result + 2;
}
static ssize_t soe_mdt_len(struct sercos_bus *bus, unsigned char devnr)
......@@ -423,7 +443,7 @@ static ssize_t soe_mdt_len(struct sercos_bus *bus, unsigned char devnr)
return -1;
items = result / 2;
for (i = 0; i < items; i++) {
for (i = 2; i < items; i++) {
result = sercos_idn_len_get(bus, devnr,
list[i], &timeout);
if (result < 0)
......@@ -441,7 +461,7 @@ static ssize_t soe_mdt_len(struct sercos_bus *bus, unsigned char devnr)
break;
}
return result;
return result + 2;
}
static void *soe_at_allocate(struct sercos_bus *bus,
......@@ -454,12 +474,18 @@ static void *soe_at_allocate(struct sercos_bus *bus,
offset = 0;
for (i = 1; i < devnr; i++) {
for (i = 0; i < devnr; i++) {
size = soe_at_len(bus, i);
if (size < 0)
if (size < 0) {
log_send(LOG_T_ERROR,
"%s: Device %d has a zero length AT",
bus->name, i);
return NULL;
}
offset += size;
}
log_send(LOG_T_DEBUG, "%s: dev %d AT @ %p + %zx",
bus->name, devnr, data, offset);
return data + offset;
}
......@@ -470,16 +496,22 @@ static void *soe_mdt_allocate(struct sercos_bus *bus,
struct sercos_bus_private *priv = bus->private;
unsigned char *data = priv->esc->tx_data;
int i;
ssize_t size, offset;
ssize_t size, offset = 0;
priv->devs++;
for (i = 1; i < devnr; i++) {
for (i = 0; i < devnr; i++) {
size = soe_mdt_len(bus, i);
if (size < 0)
if (size < 0) {
log_send(LOG_T_ERROR,
"%s: Device %d has a zero length MDT",
bus->name, i);
return NULL;
}
offset += size;
}
log_send(LOG_T_DEBUG, "%s: dev %d MDT @ %p + %zx",
bus->name, devnr, data, offset);
return data + offset;
}
......
......@@ -56,6 +56,8 @@ struct controller_block_private {
struct at_format *at;
struct mdt_format *mdt;
int32_t prevpos;
uint16_t prevstat;
float *position_in;
bool *power;
......@@ -110,6 +112,10 @@ static void calculate_at(struct controller_block *rx)
bus->at_rx(bus, controller_samplenr);
status = le16toh(priv->at->drive_status);
if (status != priv->prevstat) {
printf("%s: status %x -> %x\n", rx->name, priv->prevstat, status);
priv->prevstat = status;
}
priv->ready = status & 0xc000;
priv->active = status & 0x8000;
......@@ -129,6 +135,12 @@ static void calculate_at(struct controller_block *rx)
priv->position_out = position_s32 * priv->pos_scale;
priv->following_distance = following_distance_s32 * priv->pos_scale;
if (position_s32 != priv->prevpos) {
printf("%s: pos %x -> %x (%g)\n", rx->name, priv->prevpos, position_s32,
priv->position_out);
priv->prevpos = position_s32;
}
}
static void calculate_mdt(struct controller_block *tx)
......@@ -223,8 +235,7 @@ static void get_scaling(struct controller_block_private *priv)
goto err_read;
resolution = le32toh(val32);
priv->pos_scale *=
2 * M_PI / (float)resolution;
priv->pos_scale = 2 * M_PI / (float)resolution;
break;
}
default:
......@@ -312,7 +323,7 @@ struct controller_block * block_sercos_position_create(char *name, va_list ap)
goto err_block_at;
block_at->private = block->private;
sprintf(name_at, "%s_mdt", name);
sprintf(name_mdt, "%s_mdt", name);
block_mdt = controller_block_alloc("sercos_position_mdt", name_mdt, 0);
if (!block_mdt)
goto err_block_mdt;
......@@ -345,21 +356,40 @@ struct controller_block * block_sercos_position_create(char *name, va_list ap)
if (result < 0)
goto err_idn;
result = bus->idn_read(bus, 0, SERCOS_IDN_EL_DATA,
SERCOS_IDN_AT_CYCLIC_DATA, val16_list, sizeof(uint16_t)*20,
&timeout);
dump_idn_list(val16_list, result);
// result = bus->idn_read(bus, 0, SERCOS_IDN_EL_DATA,
// 187, val16_list, sizeof(uint16_t)*20,
// &timeout);
// dump_idn_list(val16_list, result);
/* program AT */
val16_list[0] = htole16(SERCOS_IDN_DRIVE_STATUS);
val16_list[1] = htole16(SERCOS_IDN_POS_FB1);
val16_list[2] = htole16(SERCOS_IDN_FOLLOWING_DISTANCE);
val16_list[0] = htole16(2*2);
val16_list[1] = htole16(2*2);
// val16_list[2] = htole16(SERCOS_IDN_DRIVE_STATUS);
val16_list[2] = htole16(SERCOS_IDN_POS_FB1);
val16_list[3] = htole16(SERCOS_IDN_FOLLOWING_DISTANCE);
result = bus->idn_write(bus, busdevnr, SERCOS_IDN_EL_DATA,
SERCOS_IDN_AT_CYCLIC_DATA, val16_list, sizeof(uint16_t)*3,
SERCOS_IDN_AT_CYCLIC_DATA, val16_list, sizeof(uint16_t)*4,
&timeout);
if (result < 0)
goto err_idn;
result = bus->idn_read(bus, 0, SERCOS_IDN_EL_DATA,
SERCOS_IDN_MDT_CYCLIC_DATA, val16_list, sizeof(uint16_t)*20,
&timeout);
dump_idn_list(val16_list, result);
/* program MDT */
val16_list[0] = htole16(SERCOS_IDN_DRIVE_CTRL);
val16_list[1] = htole16(SERCOS_IDN_POS_SETPOINT);
val16_list[0] = htole16(1*2);
val16_list[1] = htole16(1*2);
// val16_list[2] = htole16(SERCOS_IDN_DRIVE_CTRL);
val16_list[2] = htole16(SERCOS_IDN_POS_SETPOINT);
result = bus->idn_write(bus, busdevnr, SERCOS_IDN_EL_DATA,
SERCOS_IDN_MDT_CYCLIC_DATA, val16_list, sizeof(uint16_t)*2,
SERCOS_IDN_MDT_CYCLIC_DATA, val16_list, sizeof(uint16_t)*3,
&timeout);
if (result < 0)
goto err_idn;
......@@ -373,7 +403,12 @@ struct controller_block * block_sercos_position_create(char *name, va_list ap)
goto err_inputs;
block->outputs = block_at->outputs;
block->output = block_at->output;
block->output = calloc(block->outputs, sizeof(struct controller_block_outterm));
memcpy(block->output, block_at->output,
block->outputs*sizeof(struct controller_block_outterm));
for (i = 0; i < block->outputs; i++) {
block->output[i].sourceterm = &block_at->output[i];
}
block->inputs = block_mdt->inputs;
block->input = calloc(block->inputs,
......
params{
{ "axis1", "phase_request", 4 }
}
......@@ -97,6 +97,7 @@ int sercos_exec_proc(struct sercos_bus *bus, unsigned char devnr,
#define ERR_SVC_CHAN_NOT_OPEN 0x0001
#define ERR_IDN_NOT_SUPPORTED 0x1001
#define ERR_OPP_DATA_TOO_SHORT 0x7002
#define ERR_OPP_DATA_PROTECTED 0x7005
#define ERR_INVALID_OPP_DATA 0x7008
char *sercos_errstr(uint16_t errnr)
......@@ -111,6 +112,8 @@ char *sercos_errstr(uint16_t errnr)
return "IDN not supported";
case ERR_OPP_DATA_TOO_SHORT:
return "operation data transmission too short";
case ERR_OPP_DATA_PROTECTED:
return "operation data is write protected at this time";
case ERR_INVALID_OPP_DATA:
return "invalid operation data";
default:
......
......@@ -126,6 +126,47 @@ ssize_t sercos_idn_write_str(struct sercos_bus *bus, uint8_t drive_nr,
return result;
}
ssize_t sercos_idn_read_str(struct sercos_bus *bus, uint8_t drive_nr,
uint8_t element, uint16_t idn, char *str, size_t slen, struct timespec *timeout)
{
ssize_t result;
int len, len2;
char buf[slen+4];
result = bus->idn_read(bus, drive_nr, element, idn, buf, 4, timeout);
if (result != 4) {
log_send(LOG_T_ERROR,
"Could not read string length of IDN, res=%zd", result);
return result;
}
len = buf[0] | buf[1] * 256;
if (len >= slen) {
log_send(LOG_T_ERROR,
"String length %d is larger than given str buffer %zd",
len, slen);
len = slen-1;
}
result = bus->idn_read(bus, drive_nr, element, idn, buf, 4 + len, timeout);
if (result != 4 + len) {
log_send(LOG_T_ERROR,
"Could not read string, res=%zd", result);
return result;
}
len2 = buf[0] | buf[1] * 256;
if (len2 != len) {
log_send(LOG_T_WARNING,
"Error length changed while reading");
}
memcpy(str, buf + 4, len);
str[len] = 0;
return result;
}
int sercos_idn_set_param(struct sercos_bus *bus, unsigned char devnr,
va_list val)
{
......
......@@ -34,6 +34,8 @@ int sercos_idn_exec_proc_param(struct sercos_bus *bus, unsigned char devnr,
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 sercos_idn_read_str(struct sercos_bus *bus, uint8_t drive_nr,
uint8_t element, uint16_t idn, char *str, size_t slen, struct timespec *timeout);
enum sercos_idn_type {
SERCOS_IDN_TYPE_UINT16,
......@@ -75,7 +77,7 @@ ssize_t sercos_idn_len_get(struct sercos_bus *bus,
#define SERCOS_IDN_CP2_OPP_DATA_LIST sercos_idn(SERCOS_IDN_S, 0, 18) // page 56
#define SERCOS_IDN_CP2_INVALID_LIST sercos_idn(SERCOS_IDN_S, 0, 21) // page 57
#define SERCOS_IDN_CP3_INVALID_LIST sercos_idn(SERCOS_IDN_S, 0, 22) // page 58
#define SERCOS_IDN_MDT_CYCLIC_DATA 14
#define SERCOS_IDN_MDT_CYCLIC_DATA sercos_idn(SERCOS_IDN_S, 0, 24)
// http://infosys.beckhoff.com/italiano.php?content=../content/1040/ax5000_idn-description/html/s-0-0032.htm&id=8446
#define SERCOS_IDN_PRIMARY_OPP_MODE sercos_idn(SERCOS_IDN_S, 0, 32) // page 62
#define SERCOS_OPP_MODE_NONE 0
......@@ -106,6 +108,7 @@ ssize_t sercos_idn_len_get(struct sercos_bus *bus,
#define SERCOS_IDN_DRIVE_CTRL sercos_idn(SERCOS_IDN_S, 0, 134) // page 124
#define SERCOS_IDN_DRIVE_STATUS sercos_idn(SERCOS_IDN_S, 0, 135) // page 128
#define SERCOS_IDN_FOLLOWING_DISTANCE sercos_idn(SERCOS_IDN_S, 0, 189)
#define SERCOS_IDN_DIAGNOSTIC_NUMBER sercos_idn(SERCOS_IDN_S, 0, 390)
// activate parametrization
#define SERCOS_IDN_PROC_ACTIVATE_PL sercos_idn(SERCOS_IDN_S, 0, 420) // page 328
#define SERCOS_IDN_COMM_CYCLE_TIME sercos_idn(SERCOS_IDN_S, 0, 1002) // page 479
......
frequency 1000
frequency 500
blocks {
{ "ec", "ethercat", "eth0", 1, 1 }
{ "beckhoff_ax5xxx", "soe_bus", "AX5203", 0 }
{ "sercos_position", "axis1", "soe_bus", 1 }
{ "sercos_position", "axis2", "soe_bus", 2 }
{ "sercos_position", "axis1", "soe_bus", 0 }
{ "sercos_position", "axis2", "soe_bus", 1 }
{ "value", "pos1" }
{ "value_bool", "true" }
......@@ -41,10 +41,30 @@ params {
{ "axis1", "idn_set", "P-0-51", 4 }
# Motor EMF (mV/rpm)
{ "axis1", "idn_set", "P-0-55", 58 }
{ "axis1", "idn_set", "P-0-92", 6000 }
{ "axis1", "idn_set", "P-0-93", 3000 }
{ "axis1", "idn_set", "S-0-109", 19200 }
{ "axis1", "idn_set", "S-0-111", 3500 }
{ "axis1", "idn_set", "S-0-1", 2000 }
{ "axis1", "idn_set", "S-0-2", 2000 }
{ "axis1", "idn_set", "P-0-57", 27000 }
# { "axis1", "idn_set", "P-0-150", 2 }
{ "axis2", "idn_set", "P-0-53", "AM8033-0F00-0000" }
{ "axis2", "idn_set", "P-0-50", 0 }
{ "axis2", "idn_set", "S-0-113", 6500 }
{ "axis2", "idn_set", "P-0-51", 4 }
{ "axis2", "idn_set", "P-0-55", 58 }
{ "axis2", "idn_set", "P-0-92", 6000 }
{ "axis2", "idn_set", "P-0-93", 3000 }
{ "axis2", "idn_set", "S-0-109", 19200 }
{ "axis2", "idn_set", "S-0-111", 3500 }
{ "axis2", "idn_set", "S-0-1", 2000 }
{ "axis2", "idn_set", "S-0-2", 2000 }
{ "axis1", "phase_request", 2 }
{ "axis1", "exec_proc", "S-0-0127"
#{ "axis1", "exec_proc", "S-0-0127" }
{ "axis1", "phase_request", 3 }
......
params {
# { "axis1", "idn_set", "P-0-156", }
# { "axis1", "idn_set", "P-0-150", }
{ "axis1", "exec_proc", "P-0-0160" }
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment