Commit 2d8d46db authored by Jeroen Vreeken's avatar Jeroen Vreeken Committed by Michel Roelofs
Browse files

Ethercat updates

parent f2fb2315
......@@ -13,7 +13,8 @@ BLOCKSRCS= \
block_beckhoff_el3xxx.c \
block_beckhoff_el4xxx.c \
block_beckhoff_el5001.c \
block_beckhoff_el5101.c
block_beckhoff_el5101.c \
block_stoeber.c
BLOCKS=$(BLOCKSRCS:.c=.o)
SRCS=$(BLOCKSRCS) $(LIBSRCS)
......
This source diff could not be displayed because it is too large. You can view the blob instead.
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2012
Copyright Stichting C.A. Muller Radioastronomiestation, 2012
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
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <string.h>
#include <stdio.h>
#include "controller_block.h"
#include "esc.h"
#include "ec_stoeber.h"
struct controller_block_private {
struct ec_stoeber stbr;
bool enabled;
float output_position;
float output_speed;
float output_torque;
bool output_be1;
bool output_be2;
bool output_be3;
bool output_be4;
bool output_be5;
float output_ae1;
float *input_speed;
float *input_torque;
bool *input_enable;
};
static void calculate_tx(struct controller_block *tx)
{
struct controller_block_private *private = tx->private;
struct ec_stoeber *stbr = &private->stbr;
bool enable;
bool enabled = private->enabled;
float sp;
float torque;
int ret;
enable = *private->input_enable;
if (enable && !enabled) {
esc_al_state_set(&stbr->addr, ESC_AL_STATE_OPERATIONAL);
enabled = true;
}
*stbr->tx_A180 = enabled;
*stbr->tx_F210 = 0;
*stbr->tx_I210 = htole16(le16toh(*stbr->tx_I210) ^ 1);
if (enabled)
sp = *private->input_speed * 16384.0 / RPM2RADS(3000.0);
else
sp = 0;
if (sp > 32767)
sp = 32767;
if (sp < -32768)
sp = -32768;
*stbr->tx_I215 = htole16((int16_t)sp);
torque = *private->input_torque / stbr->standstill_torque * 16384;
*stbr->tx_C230 = htole16((int16_t)torque);
ret = ec_tx_pdo(controller_samplenr);
if (ret && enabled) {
enabled = false;
}
private->enabled = enabled;
}
static void calculate_rx(struct controller_block *rx)
{
struct controller_block_private *private = rx->private;
struct ec_stoeber *stbr = &private->stbr;
int ret;
float speed, torque, position, ae1;
bool enabled, be1, be2, be3, be4, be5;
uint16_t E19;
enabled = private->enabled;
ret = ec_rx_pdo(controller_samplenr);
if (ret == 0) {
position = (int32_t)le32toh(*stbr->rx_I80);
position *= 2.0 * M_PI / 16384;
speed = (int32_t)le32toh(*stbr->rx_E91);
speed *= 1.0/16384;
speed = RPM2RADS(speed);
torque = (int16_t)le16toh(*stbr->rx_E90);
torque *= 1.0 / 1200;
ae1 = (int16_t)le16toh(*stbr->rx_E10);
ae1 *= 20.0 / 32767.0;
E19 = le16toh(*stbr->rx_E19);
be1 = E19 & 0x01;
be2 = E19 & 0x02;
be3 = E19 & 0x04;
be4 = E19 & 0x08;
be5 = E19 & 0x10;
} else {
enabled = false;
speed = 0;
torque = 0;
position = 0;
be1 = false;
be2 = false;
be3 = false;
be4 = false;
be5 = false;
ae1 = 0;
}
private->output_position = position;
private->output_speed = speed;
private->output_torque = torque;
private->enabled = enabled;
private->output_be1 = be1;
private->output_be2 = be2;
private->output_be3 = be3;
private->output_be4 = be4;
private->output_be5 = be5;
private->output_ae1 = ae1;
}
struct controller_block *block_stoeber_create(char *name, va_list ap)
{
struct controller_block *stoeber;
struct controller_block *stoeber_tx;
struct controller_block *stoeber_rx;
struct controller_block_private *private;
int ec_pos;
int i;
double max_torque, brake_resistor, brake_power;
ec_pos = va_arg(ap, int);
max_torque = va_arg(ap, double);
brake_resistor = va_arg(ap, double);
brake_power = va_arg(ap, double);
stoeber = malloc(sizeof(struct controller_block));
if (!stoeber)
goto err_malloc;
stoeber_tx = malloc(sizeof(struct controller_block));
if (!stoeber_tx)
goto err_malloc_tx;
stoeber_rx = malloc(sizeof(struct controller_block));
if (!stoeber_rx)
goto err_malloc_rx;
stoeber->type = "stoeber";
stoeber_tx->type = "stoeber_tx";
stoeber_rx->type = "stoeber_rx";
stoeber->name = strdup(name);
if (!stoeber->name)
goto err_name;
stoeber_rx->name = malloc(strlen(name)+4);
if (!stoeber_rx->name)
goto err_stoeber_rx_name;
sprintf(stoeber_rx->name, "%s_rx", name);
stoeber_tx->name = malloc(strlen(name)+4);
if (!stoeber_tx->name)
goto err_stoeber_tx_name;
sprintf(stoeber_tx->name, "%s_tx", name);
private = calloc(1, sizeof(struct controller_block_private));
if (!private)
goto err_private;
stoeber->private = private;
stoeber_rx->private = private;
stoeber_tx->private = private;
ec_addr_set_auto_inc_nr(&private->stbr.addr, ec_pos);
private->stbr.max_speed = RPM2RADS(3000);
private->stbr.max_accel = 3000 * 360 / 60 * 10;
private->stbr.max_torque = max_torque;
private->stbr.brake_resistor = brake_resistor;
private->stbr.brake_power = brake_power;
if (ec_stoeber_init(&private->stbr))
goto err_stoeber_init;
stoeber_rx->outputs = 10;
stoeber_rx->output = calloc(stoeber_rx->outputs, sizeof(struct controller_block_outterm));
if (!stoeber_rx->output)
goto err_rx_output;
stoeber_rx->output[0].name = "position";
stoeber_rx->output[0].value.f = &private->output_position;
stoeber_rx->output[0].type = CONTROLLER_BLOCK_TERM_FLOAT;
stoeber_rx->output[0].source = stoeber_rx;
stoeber_rx->output[0].sourceterm = &stoeber_rx->output[0];
stoeber_rx->output[1].name = "speed";
stoeber_rx->output[1].value.f = &private->output_speed;
stoeber_rx->output[1].type = CONTROLLER_BLOCK_TERM_FLOAT;
stoeber_rx->output[1].source = stoeber_rx;
stoeber_rx->output[1].sourceterm = &stoeber_rx->output[1];
stoeber_rx->output[2].name = "torque";
stoeber_rx->output[2].value.f = &private->output_torque;
stoeber_rx->output[2].type = CONTROLLER_BLOCK_TERM_FLOAT;
stoeber_rx->output[2].source = stoeber_rx;
stoeber_rx->output[2].sourceterm = &stoeber_rx->output[2];
stoeber_rx->output[3].name = "enabled";
stoeber_rx->output[3].value.b = &private->enabled;
stoeber_rx->output[3].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber_rx->output[3].source = stoeber_rx;
stoeber_rx->output[3].sourceterm = &stoeber_rx->output[3];
stoeber_rx->output[4].name = "be1";
stoeber_rx->output[4].value.b = &private->output_be1;
stoeber_rx->output[4].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber_rx->output[4].source = stoeber_rx;
stoeber_rx->output[4].sourceterm = &stoeber_rx->output[4];
stoeber_rx->output[5].name = "be2";
stoeber_rx->output[5].value.b = &private->output_be2;
stoeber_rx->output[5].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber_rx->output[5].source = stoeber_rx;
stoeber_rx->output[5].sourceterm = &stoeber_rx->output[5];
stoeber_rx->output[6].name = "be3";
stoeber_rx->output[6].value.b = &private->output_be3;
stoeber_rx->output[6].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber_rx->output[6].source = stoeber_rx;
stoeber_rx->output[6].sourceterm = &stoeber_rx->output[6];
stoeber_rx->output[7].name = "be4";
stoeber_rx->output[7].value.b = &private->output_be4;
stoeber_rx->output[7].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber_rx->output[7].source = stoeber_rx;
stoeber_rx->output[7].sourceterm = &stoeber_rx->output[7];
stoeber_rx->output[8].name = "be5";
stoeber_rx->output[8].value.b = &private->output_be5;
stoeber_rx->output[8].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber_rx->output[8].source = stoeber_rx;
stoeber_rx->output[8].sourceterm = &stoeber_rx->output[8];
stoeber_rx->output[9].name = "ae1";
stoeber_rx->output[9].value.f = &private->output_ae1;
stoeber_rx->output[9].type = CONTROLLER_BLOCK_TERM_FLOAT;
stoeber_rx->output[9].source = stoeber_rx;
stoeber_rx->output[9].sourceterm = &stoeber_rx->output[9];
/* stoeber also exports these outputs we will re-use the same struct
this is possible as both source and sourceterm point to the
stoeber_tx structs alread. */
stoeber->outputs = stoeber_rx->outputs;
stoeber->output = stoeber_rx->output;
stoeber_tx->outputs = 0;
stoeber_tx->output = NULL;
stoeber_tx->inputs = 3;
stoeber_tx->input = calloc(stoeber_tx->inputs, sizeof(struct controller_block_interm));
if (!stoeber_tx->input)
goto err_tx_input;
stoeber_tx->input[0].name = "speed";
stoeber_tx->input[0].type = CONTROLLER_BLOCK_TERM_FLOAT;
stoeber_tx->input[0].value.f = &private->input_speed;
stoeber_tx->input[0].ghostof = NULL;
stoeber_tx->input[1].name = "torque";
stoeber_tx->input[1].type = CONTROLLER_BLOCK_TERM_FLOAT;
stoeber_tx->input[1].value.f = &private->input_torque;
stoeber_tx->input[1].ghostof = NULL;
stoeber_tx->input[2].name = "enable";
stoeber_tx->input[2].type = CONTROLLER_BLOCK_TERM_BOOL;
stoeber_tx->input[2].value.b = &private->input_enable;
stoeber_tx->input[2].ghostof = NULL;
stoeber->inputs = stoeber_tx->inputs;
stoeber->input = calloc(stoeber->inputs, sizeof(struct controller_block_interm));
if (!stoeber->input)
goto err_input;
for (i = 0; i < stoeber->inputs; i++) {
stoeber->input[i].name = stoeber_tx->input[i].name;
stoeber->input[i].type = stoeber_tx->input[i].type;
stoeber->input[i].value = stoeber_tx->input[i].value;
stoeber->input[i].ghostof = &stoeber_tx->input[i];
}
stoeber->calculate = NULL;
stoeber_tx->calculate = calculate_tx;
stoeber_rx->calculate = calculate_rx;
controller_block_add(stoeber_tx);
controller_block_add(stoeber_rx);
controller_block_add(stoeber);
return stoeber;
err_input:
free(stoeber_tx->input);
err_tx_input:
free(stoeber->output);
err_rx_output:
/* no way to un-init the device? */
err_stoeber_init:
free(private);
err_private:
free(stoeber_tx->name);
err_stoeber_tx_name:
free(stoeber_rx->name);
err_stoeber_rx_name:
free(stoeber->name);
err_name:
free(stoeber_rx);
err_malloc_rx:
free(stoeber_tx);
err_malloc_tx:
free(stoeber);
err_malloc:
return NULL;
}
......@@ -123,6 +123,19 @@ int ec_rx_pdo_add(struct ec_dgram_addr *addr, unsigned char **data, size_t len)
struct ec_dgram_hdr *prev;
uint16_t *wkc;
for (prev = (struct ec_dgram_hdr *)(rx_pdo.tx_frame + sizeof(struct ec_frame_hdr));
(size_t)prev < (size_t)rx_pdo.tx_frame + rx_pdo.flen;
prev = ((void*)prev) + EC_DGRAM_HDR_LEN(prev) + sizeof(struct ec_dgram_hdr) + 2) {
if (addr->type == EC_DGRAM_ADDR_AUTO_INC &&
prev->cmd == EC_DGRAM_HDR_CMD_APRD &&
prev->addr.position.pos == addr->addr.position.pos &&
prev->addr.position.off == addr->addr.position.off) {
printf("Double registration for rx PDO");
*data = (unsigned char *)prev + sizeof(struct ec_dgram_hdr);
return 0;
}
}
dgram = (struct ec_dgram_hdr *)(rx_pdo.tx_frame + rx_pdo.flen);
*data = rx_pdo.rx_frame + rx_pdo.flen + sizeof(struct ec_dgram_hdr);
......@@ -170,6 +183,19 @@ int ec_tx_pdo_add(struct ec_dgram_addr *addr, unsigned char **data, size_t len)
struct ec_dgram_hdr *dgram;
struct ec_dgram_hdr *prev;
uint16_t *wkc;
for (prev = (struct ec_dgram_hdr *)(tx_pdo->tx_frame + sizeof(struct ec_frame_hdr));
(size_t)prev < (size_t)tx_pdo->tx_frame + tx_pdo->flen;
prev = ((void*)prev) + EC_DGRAM_HDR_LEN(prev) + sizeof(struct ec_dgram_hdr) + 2) {
if (addr->type == EC_DGRAM_ADDR_AUTO_INC &&
prev->cmd == EC_DGRAM_HDR_CMD_APWR &&
prev->addr.position.pos == addr->addr.position.pos &&
prev->addr.position.off == addr->addr.position.off) {
printf("Double registration for tx PDO");
*data = (unsigned char *)prev + sizeof(struct ec_dgram_hdr);
return 0;
}
}
dgram = (struct ec_dgram_hdr *)(tx_pdo->tx_frame + tx_pdo->flen);
*data = tx_pdo->tx_frame + tx_pdo->flen + sizeof(struct ec_dgram_hdr);
......
......@@ -399,7 +399,7 @@ while(1) {
// }
// if (le32toh(*(uint32_t*)&buffer[1]) != prev) {
// prev = le32toh(*(uint32_t*)&buffer[1]);
fprintf(stderr, "%d %d\n", speed, le32toh(*(uint32_t*)&buffer[1]));
// fprintf(stderr, "%d %d\n", speed, le32toh(*(uint32_t*)&buffer[1]));
// i++;
// }
// printf(" %d\n", ret);
......
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