Commit 1ebd504b authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

canopen emergency support

parent b093ebb6
......@@ -251,6 +251,8 @@ struct controller_block *block_stoeber_create(char *name, va_list ap)
stoeber_rx->private = private;
stoeber_tx->private = private;
memset(&private->stbr, 0, sizeof(private->stbr));
ec_addr_set_auto_inc_nr(&private->stbr.addr, ec_pos);
private->stbr.max_speed = RPM2RADS(3000);
......@@ -263,6 +265,8 @@ struct controller_block *block_stoeber_create(char *name, va_list ap)
if (ec_stoeber_init(&private->stbr))
goto err_stoeber_init;
private->stbr.canopen_dev->logname = stoeber->name;
printf("Drive serial number: %d\n", private->stbr.serial_number);
if (serial_number && serial_number != private->stbr.serial_number) {
printf("Serial number of drive (%d) does not match requested number (%d)\n",
......
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007
Copyright Stichting C.A. Muller Radioastronomiestation, 2007
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2013
Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2013
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
......@@ -19,13 +19,16 @@
#include <string.h>
#include <stdio.h>
#include <pthread.h>
#include "canopen.h"
#include "log.h"
#define CANOPEN_CSS_MASK 0xe0
#define CANOPEN_CSS_INITIATE_DOMAIN_DOWNLOAD (0x1 << 5)
#define CANOPEN_CSS_INITIATE_DOMAIN_UPLOAD (0x2 << 5)
#define CANOPEN_CSS_ABORT (0x4 << 5)
#define CANOPEN_CSS_INITIATE_DOMAIN_DOWNLOAD (0x1 << 5) // 0x20
#define CANOPEN_CSS_INITIATE_DOMAIN_UPLOAD (0x2 << 5) // 0x40
#define CANOPEN_CSS_ABORT (0x4 << 5) // 0x80
#define CANOPEN_N_MASK 0x0c
#define CANOPEN_N_SHIFT 2
#define CANOPEN_E 0x02
......@@ -47,10 +50,14 @@ ssize_t canopen_write_param(struct canopen_dev *dev,
cmd[2] = index >> 8;
cmd[3] = subindex & 0xff;
pthread_mutex_lock(&dev->lock);
ret = dev->send_request(dev, cmd, 8);
if (ret != 8)
return -1;
ret = dev->recv_response(dev, cmd, 8);
if (ret == 8)
ret = dev->recv_response(dev, cmd, 8);
pthread_mutex_unlock(&dev->lock);
if (ret != 8)
return -1;
......@@ -74,10 +81,15 @@ ssize_t canopen_read_param(struct canopen_dev *dev,
cmd[2] = index >> 8;
cmd[3] = subindex & 0xff;
pthread_mutex_lock(&dev->lock);
ret = dev->send_request(dev, cmd, 8);
if (ret != 8)
return -1;
ret = dev->recv_response(dev, cmd, 8);
if (ret == 8)
ret = dev->recv_response(dev, cmd, 8);
pthread_mutex_unlock(&dev->lock);
if (ret != 8)
return -1;
......@@ -96,3 +108,91 @@ ssize_t canopen_read_param(struct canopen_dev *dev,
return ret;
}
void canopen_emergency_handler(struct canopen_dev *dev, void *msg, size_t len)
{
uint16_t code_le, code;
uint8_t reg;
uint8_t vendor[5];
if (len != 8) {
log_send(LOG_T_ERROR,
"CANopen emergency received with wrong length: %zd",
len);
} else {
int log_type = LOG_T_ERROR;
char *category = "unknown";
char *cause = "unknown";
memcpy(&code_le, msg, 2);
code = le16toh(code_le);
memcpy(&reg, msg + 2, 1);
memcpy(vendor, msg + 3, 5);
if (reg & 0x01)
cause = "Generic Error";
else if (reg & 0x02)
cause = "Current";
else if (reg & 0x04)
cause = "Voltage";
else if (reg & 0x08)
cause = "Temperature";
else if (reg & 0x10)
cause = "Communication Error";
else if (reg & 0x20)
cause = "Device Profile Specific";
else if (reg & 0x40)
cause = "Reserved";
else if (reg & 0x80)
cause = "Manufacturer Specific";
if (!(code & 0xff00)) {
category = "Error reset";
log_type = LOG_T_WARNING;
} else if ((code & 0xff00) == 0x1000) {
category = "Generic Error";
} else if ((code & 0xf000) == 0x2000) {
category = "Current";
} else if ((code & 0xf000) == 0x3000) {
category = "Voltage";
} else if ((code & 0xf000) == 0x4000) {
category = "Temperature";
} else if ((code & 0xff00) == 0x5000) {
category = "Device Hardware";
} else if ((code & 0xff00) == 0x6000) {
category = "Device Software";
} else if ((code & 0xff00) == 0x7000) {
category = "Additional Modules";
} else if ((code & 0xff00) == 0x8000) {
category = "Monitoring";
} else if ((code & 0xff00) == 0x9000) {
category = "External Error";
} else if ((code & 0xff00) == 0xf000) {
category = "Additional Functions";
} else if ((code & 0xff00) == 0xff00) {
category = "Device Specific";
}
log_send(log_type,
"%s: CANopen emergency message: "
"errorcode=0x%04x (%s), register=0x%02x (%s), "
"vendor=0x%02x%02x%02x%02x%02x",
dev->logname, code, category, reg, cause,
vendor[0], vendor[1], vendor[2], vendor[3], vendor[4]);
if (dev->emergency_handler)
dev->emergency_handler(dev, code, reg, vendor);
}
}
void canopen_emergency_handler_attach(struct canopen_dev *dev,
void (*func)(struct canopen_dev *dev, uint16_t code, uint8_t register, uint8_t vendor[5]))
{
dev->emergency_handler = func;
}
int canopen_init(struct canopen_dev *dev)
{
return pthread_mutex_init(&dev->lock, NULL);
}
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007
Copyright Stichting C.A. Muller Radioastronomiestation, 2007
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2013
Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2013
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
......@@ -21,10 +21,17 @@
#define _INCLUDE_CANOPEN_H_
#include <unistd.h>
#include <stdint.h>
struct canopen_dev {
ssize_t (*send_request)(struct canopen_dev *dev, void *req, size_t len);
ssize_t (*recv_response)(struct canopen_dev *dev, void *res, size_t len);
void (*emergency_handler)(struct canopen_dev *dev, uint16_t code, uint8_t reg, uint8_t vendor[5]);
char *logname;
pthread_mutex_t lock;
struct canopen_dev_priv *priv;
};
......@@ -35,4 +42,11 @@ ssize_t canopen_read_param(struct canopen_dev *dev,
unsigned index, unsigned subindex,
void *data, ssize_t bytes);
void canopen_emergency_handler_attach(struct canopen_dev *dev,
void (*func)(struct canopen_dev *dev, uint16_t code, uint8_t register, uint8_t vendor[5]));
int canopen_init(struct canopen_dev *dev);
void canopen_emergency_handler(struct canopen_dev *dev, void *msg, size_t len);
#endif /* _INCLUDE_CANOPEN_H_ */
......@@ -23,6 +23,70 @@
#include "esc.h"
#include "ec_stoeber.h"
#include "log.h"
static void ec_stoeber_emergency_handler(struct canopen_dev *dev,
uint16_t code, uint8_t reg, uint8_t vendor[5])
{
char *msg = "unknown";
if (code == 0x0000) {
msg = "no_error";
} else if (code == 0x2110) {
msg = "short circuit earth";
} else if (code == 0x2230) {
msg = "intern short circuit earth";
} else if (code == 0x2310) {
msg = "continuous overcurrent";
} else if (code == 0x3110) {
msg = "mains overvoltage";
} else if (code == 0x3120) {
msg = "mains undervoltage";
} else if (code == 0x4210) {
msg = "temperature device";
} else if (code == 0x4280) {
msg = "temperature device i2t";
} else if (code == 0x4310) {
msg = "temperature drive";
} else if (code == 0x4310) {
msg = "temperature drive i2t";
} else if (code == 0x5000) {
msg = "device hardware";
} else if (code == 0x5200) {
msg = "device hardware control";
} else if (code == 0x6010) {
msg = "software reset";
} else if (code == 0x6100) {
msg = "internal software";
} else if (code == 0x6200) {
msg = "user software";
} else if (code == 0x6300) {
msg = "data record";
} else if (code == 0x6310) {
msg = "loss of parameters";
} else if (code == 0x7110) {
msg = "brake chopper";
} else if (code == 0x7120) {
msg = "motor";
} else if (code == 0x7303) {
msg = "resolver 1 fault";
} else if (code == 0x8100) {
msg = "communication";
} else if (code == 0x8400) {
msg = "velocity speed control";
} else if (code == 0x8311) {
msg = "excess torque";
} else if (code == 0x9000) {
msg = "external error";
} else if (code == 0xa000) {
msg = "Transition from Pre-Operational to Safe-Operational was not successful";
} else if (code == 0xa001) {
msg = "Transition from Safe-Operational to Pre-Operational was not successful";
}
log_send(LOG_T_WARNING, "%s: %s",
dev->logname, msg);
}
int ec_stoeber_init(struct ec_stoeber *stbr)
{
......@@ -77,8 +141,18 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
mb_wr.len = 0x0040;
mb_wr.sm = 0;
mb = esc_mailbox_create(&stbr->addr, &mb_rd, &mb_wr);
canopen_dev = esc_coe_create(mb);
if (stbr->canopen_dev) {
esc_mailbox_reinit(&stbr->addr, &mb_rd, &mb_wr);
canopen_dev = stbr->canopen_dev;
} else {
mb = esc_mailbox_create(&stbr->addr, &mb_rd, &mb_wr);
canopen_dev = esc_coe_create(mb);
canopen_emergency_handler_attach(canopen_dev, ec_stoeber_emergency_handler);
stbr->canopen_dev = canopen_dev;
}
printf("Going to state pre-operational\n");
......@@ -279,9 +353,6 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
leval32 = STOEBER_PDO_MAP('C', 230, 0, 16);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x5, &leval32, 4);
esc_coe_destroy(canopen_dev);
esc_mailbox_destroy(mb);
printf("Setup PDOs\n");
......
......@@ -54,6 +54,8 @@ struct ec_stoeber {
uint32_t serial_number;
struct canopen_dev *canopen_dev;
/* rx pdo */
unsigned char *rx_buffer; /* 14 */
uint32_t *rx_I80;
......
......@@ -20,6 +20,8 @@
#include <string.h>
#include <stdio.h>
#include <time.h>
#include <pthread.h>
#include <limits.h>
#include "esc.h"
#include "ec.h"
......@@ -35,6 +37,8 @@ struct esc_mailbox {
int len_wr;
unsigned char *buffer_rd;
unsigned char *buffer_wr;
void (*error_handler)(void *msg, size_t len);
};
......@@ -53,6 +57,7 @@ struct esc_mailbox_hdr {
#define ESC_MAILBOX_TYPE_SOE 0x5
#define ESC_MAILBOX_TYPE_VOE 0xf
#define ESC_COE_TIMEOUT 2
ssize_t esc_esi_eeprom_read(struct ec_dgram_addr *ec_addr,
void *buffer, size_t offset, size_t size)
......@@ -353,6 +358,18 @@ int esc_syncmanager_claim(struct ec_dgram_addr *addr)
return nr;
}
static void esc_error_handler_default(void *msg, size_t len)
{
unsigned char *msgb = msg;
int i;
printf("Mailbox error message:");
for (i = 0; i < len; i++) {
printf(" 0x%02x", msgb[i]);
}
printf("\n");
}
struct esc_mailbox *esc_mailbox_create(struct ec_dgram_addr *node,
struct esc_syncmanager_info *rd, struct esc_syncmanager_info *wr)
{
......@@ -509,6 +526,7 @@ printf("Read status byte: 0x%02x\n", val8);
if (!mailbox->buffer_wr)
goto err_buffer_wr;
}
mailbox->error_handler = esc_error_handler_default;
}
return mailbox;
......@@ -521,6 +539,149 @@ err_buffer_rd:
return NULL;
}
void esc_mailbox_reinit(struct ec_dgram_addr *node,
struct esc_syncmanager_info *rd, struct esc_syncmanager_info *wr)
{
int sync_manager_write, sync_manager_read;
size_t start_write, start_read;
size_t len_write, len_read;
struct esc_mailbox mb;
uint8_t val8;
uint16_t leval16;
/* init write sync manager */
if (!wr) {
sync_manager_write = esc_syncmanager_claim(node);
if (sync_manager_write < 0)
return;
start_write = 0x1000;
len_write = 0x0100;
} else {
sync_manager_write = wr->sm;
start_write = wr->start;
len_write = wr->len;
}
printf("Claiming syncmanager %d for mailbox write\n", sync_manager_write);
/* mailbox write (master->slave) */
node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) |
ESC_ADDR_MAP_SYNCMANAGER_ACTIVATE;
val8 = 0x00;
ec_datagram_write(node, &val8, 1);
node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) |
ESC_ADDR_MAP_SYNCMANAGER_PHYSICAL_START;
leval16 = htole16(start_write);
ec_datagram_write(node, &leval16, 2);
printf("write address: %04zx\n", start_write);
node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) |
ESC_ADDR_MAP_SYNCMANAGER_LENGTH;
leval16 = htole16(len_write);
ec_datagram_write(node, &leval16, 2);
node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) |
ESC_ADDR_MAP_SYNCMANAGER_CONTROL;
val8 =
ESC_SYNCMANAGER_CONTROL_MAILBOX |
ESC_SYNCMANAGER_CONTROL_WRITE |
ESC_SYNCMANAGER_CONTROL_INT_PDI;
printf("Write Control byte: 0x%02x\n", val8);
ec_datagram_write(node, &val8, 1);
node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) |
ESC_ADDR_MAP_SYNCMANAGER_STATUS;
ec_datagram_read(node, &val8, 1);
printf("Write status byte: 0x%02x\n", val8);
node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) |
ESC_ADDR_MAP_SYNCMANAGER_ACTIVATE;
val8 = ESC_SYNCMANAGER_ACTIVATE_ENABLE;
ec_datagram_write(node, &val8, 1);
/* init read sync manager */
if (!rd) {
sync_manager_read = esc_syncmanager_claim(node);
if (sync_manager_read < 0)
return;
start_read = 0x1100;
len_read = 0x0100;
} else {
sync_manager_read = rd->sm;
start_read = rd->start;
len_read = rd->len;
}
printf("Claiming syncmanager %d for mailbox read\n", sync_manager_read);
/* mailbox read (slave->master) */
node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) |
ESC_ADDR_MAP_SYNCMANAGER_ACTIVATE;
val8 = 0x00;
ec_datagram_write(node, &val8, 1);
node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) |
ESC_ADDR_MAP_SYNCMANAGER_PHYSICAL_START;
leval16 = htole16(start_read);
ec_datagram_write(node, &leval16, 2);
printf("read address: %04zx\n", start_read);
node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) |
ESC_ADDR_MAP_SYNCMANAGER_LENGTH;
leval16 = htole16(len_read);
ec_datagram_write(node, &leval16, 2);
node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) |
ESC_ADDR_MAP_SYNCMANAGER_CONTROL;
val8 =
ESC_SYNCMANAGER_CONTROL_MAILBOX |
ESC_SYNCMANAGER_CONTROL_INT_PDI;
printf("Read Control byte: 0x%02x\n", val8);
ec_datagram_write(node, &val8, 1);
node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) |
ESC_ADDR_MAP_SYNCMANAGER_STATUS;
ec_datagram_read(node, &val8, 1);
printf("Read status byte: 0x%02x\n", val8);
node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) |
ESC_ADDR_MAP_SYNCMANAGER_ACTIVATE;
val8 = ESC_SYNCMANAGER_ACTIVATE_ENABLE;
ec_datagram_write(node, &val8, 1);
memcpy(&mb.status_rd, node, sizeof(struct ec_dgram_addr));
mb.status_rd.addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) |
ESC_ADDR_MAP_SYNCMANAGER_STATUS;
memcpy(&mb.status_wr, node, sizeof(struct ec_dgram_addr));
mb.status_wr.addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) |
ESC_ADDR_MAP_SYNCMANAGER_STATUS;
mb.len_rd = len_read;
memcpy(&mb.data_rd, node, sizeof(struct ec_dgram_addr));
mb.data_rd.addr.position.off = start_read;
mb.len_wr = len_write;
memcpy(&mb.data_wr, node, sizeof(struct ec_dgram_addr));
mb.data_wr.addr.position.off = start_write;
}
void esc_mailbox_destroy(struct esc_mailbox *mailbox)
{
/* todo: deactivate sync managers */
......@@ -536,6 +697,7 @@ ssize_t esc_mailbox_write(struct esc_mailbox *mailbox, int type,
struct esc_mailbox_hdr *hdr;
uint8_t val8;
int ret;
time_t start = time(NULL);
if (mailbox->len_wr == 0) {
printf("len_wr == 0\n");
......@@ -557,22 +719,22 @@ ssize_t esc_mailbox_write(struct esc_mailbox *mailbox, int type,
mailbox->ctr++;
/* wait till mailbox is empty */
while (1) {
do {
ret = ec_datagram_read(&mailbox->status_wr, &val8,
sizeof(val8));
if (ret != sizeof(val8)) {
printf("could not read mailbox status\n");
return -1;
}
} while ((val8 & 0x08) && (time(NULL) - start < ESC_COE_TIMEOUT));
if (!(val8 & 0x08)) {
/* write mailbox contents */
ret = ec_datagram_write(&mailbox->data_wr, mailbox->buffer_wr,
mailbox->len_wr);
if (ret > 0) {
break;
if (ret != mailbox->len_wr) {
return -1;
}
/* wait while mailbox full */
do {
ret = ec_datagram_read(&mailbox->status_wr, &val8,
sizeof(val8));
if (ret != sizeof(val8)) {
printf("could not read mailbox status\n");
return -1;
}
} while (val8 & 0x08);
}
return len;
......@@ -585,6 +747,7 @@ ssize_t esc_mailbox_read(struct esc_mailbox *mailbox, int *type,
int ret;
size_t clen;
uint8_t val8;
time_t start = time(NULL);
if (mailbox->len_rd == 0) {
printf("len_rd == 0\n");
......@@ -598,22 +761,20 @@ ssize_t esc_mailbox_read(struct esc_mailbox *mailbox, int *type,
hdr = (struct esc_mailbox_hdr *)mailbox->buffer_rd;
/* wait till mailbox is full */
while (1/*todo add timeout*/) {
/* wait while mailbox full */
do {
ret = ec_datagram_read(&mailbox->status_rd, &val8, sizeof(val8));
if (ret != sizeof(val8)) {
printf("Could not read mailbox status\n");
return -1;
}
} while (val8 & 0x08);
/* read mailbox contents */
ret = ec_datagram_read(&mailbox->data_rd, mailbox->buffer_rd,
mailbox->len_rd);
if (ret > 0) {
break;
/* wait untill mailbox full */
do {
ret = ec_datagram_read(&mailbox->status_rd, &val8, sizeof(val8));
if (ret != sizeof(val8)) {
printf("Could not read mailbox status\n");
return -1;
}
} while (!(val8 & 0x08) && (time(NULL) - start < ESC_COE_TIMEOUT));
/* read mailbox contents */
ret = ec_datagram_read(&mailbox->data_rd, mailbox->buffer_rd,
mailbox->len_rd);
if (ret != mailbox->len_rd) {
return -1;
}
clen = le16toh(hdr->length);
......@@ -622,6 +783,12 @@ ssize_t esc_mailbox_read(struct esc_mailbox *mailbox, int *type,
memcpy(data, mailbox->buffer_rd + sizeof(struct esc_mailbox_hdr), clen);
*type = hdr->type_ctr & 0x0f;
if (*type == ESC_MAILBOX_TYPE_ERROR) {
mailbox->error_handler(
mailbox->buffer_rd + sizeof(struct esc_mailbox_hdr),
hdr->length);
}
return clen;
}
......@@ -629,16 +796,18 @@ ssize_t esc_mailbox_read(struct esc_mailbox *mailbox, int *type,