Ethercat updates

ec/Makefile

@@ -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)

ec/block_stoeber.c

+/*
+	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;
+}

ec/ec.c

@@ -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);

ec/ec_test.c

@@ -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);