block_beckhoff_el4xxx.c

/*
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2009, 2013
	Copyright Stichting C.A. Muller Radioastronomiestation, 2009

	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 <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <controller/controller_block.h>

#include <ec/ec.h>
#include <ec/esc.h>
#include <ec/esc_esi.h>
#include <ec/esc_coe.h>
#include <ec/esc_id.h>
#include <log/log.h>

struct controller_block_private {
	float	*output[8];

	float gain;
	float offset;
	
	unsigned char *tx_buffer;

	struct esc_device *dev;
	struct canopen_dev *candev;
};

static void calculate_1u(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	float value1;
	struct {
		uint16_t value1;
	} __packed *out;
	
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
	if (value1 > 65535)
		value1 = 65535;
	
	out = (void*)priv->tx_buffer;
	
	out->value1 = htole16(value1);
}

static void calculate_1s(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	float value1;
	struct {
		int16_t value1;
	} __packed *out;
	
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
	if (value1 > 32767)
		value1 = 32767;
	if (value1 < -32768)
		value1 = -32768;
	
	out = (void*)priv->tx_buffer;
	
	out->value1 = htole16((int16_t)value1);
}

static void calculate_2u(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	float value1, value2;
	struct {
		uint16_t value1;
		uint16_t value2;
	} __packed *out;
	
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
	if (value1 > 65535)
		value1 = 65535;
	value2 = (priv->offset + *priv->output[1]) * priv->gain;
	if (value2 > 65535)
		value2 = 65535;
	
	out = (void*)priv->tx_buffer;
	
	out->value1 = htole16(value1);
	out->value2 = htole16(value2);
}

static void calculate_2s(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	float value1, value2;
	struct {
		int16_t value1;
		int16_t value2;
	} __packed *out;
	
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
	value2 = (priv->offset + *priv->output[1]) * priv->gain;
	if (value1 > 32767)
		value1 = 32767;
	if (value1 < -32768)
		value1 = -32768;
	if (value2 > 32767)
		value2 = 32767;
	if (value2 < -32768)
		value2 = -32768;
	
	out = (void*)priv->tx_buffer;
	
	out->value1 = htole16((int16_t)value1);
	out->value2 = htole16((int16_t)value2);
}

static void calculate_4u(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	float value1, value2, value3, value4;
	struct {
		uint16_t value1;
		uint16_t value2;
		uint16_t value3;
		uint16_t value4;
	} __packed *out;
	
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
	if (value1 > 65535)
		value1 = 65535;
	value2 = (priv->offset + *priv->output[1]) * priv->gain;
	if (value2 > 65535)
		value2 = 65535;
	value3 = (priv->offset + *priv->output[2]) * priv->gain;
	if (value3 > 65535)
		value3 = 65535;
	value4 = (priv->offset + *priv->output[3]) * priv->gain;
	if (value4 > 65535)
		value4 = 65535;
	
	out = (void*)priv->tx_buffer;
	
	out->value1 = htole16(value1);
	out->value2 = htole16(value2);
	out->value3 = htole16(value3);
	out->value4 = htole16(value4);
}

static void calculate_4s(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	float value1, value2, value3, value4;
	struct {
		int16_t value1;
		int16_t value2;
		int16_t value3;
		int16_t value4;
	} __packed *out;
	
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
	value2 = (priv->offset + *priv->output[1]) * priv->gain;
	value3 = (priv->offset + *priv->output[2]) * priv->gain;
	value4 = (priv->offset + *priv->output[3]) * priv->gain;
	if (value1 > 32767)
		value1 = 32767;
	if (value1 < -32768)
		value1 = -32768;
	if (value2 > 32767)
		value2 = 32767;
	if (value2 < -32768)
		value2 = -32768;
	if (value3 > 32767)
		value3 = 32767;
	if (value3 < -32768)
		value3 = -32768;
	if (value4 > 32767)
		value4 = 32767;
	if (value4 < -32768)
		value4 = -32768;
	
	out = (void*)priv->tx_buffer;
	
	out->value1 = htole16((int16_t)value1);
	out->value2 = htole16((int16_t)value2);
	out->value3 = htole16((int16_t)value3);
	out->value4 = htole16((int16_t)value4);
}

static void calculate_8u(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	float value1, value2, value3, value4;
	float value5, value6, value7, value8;
	struct {
		uint16_t value1;
		uint16_t value2;
		uint16_t value3;
		uint16_t value4;
		uint16_t value5;
		uint16_t value6;
		uint16_t value7;
		uint16_t value8;
	} __packed *out;
	
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
	if (value1 > 65535)
		value1 = 65535;
	value2 = (priv->offset + *priv->output[1]) * priv->gain;
	if (value2 > 65535)
		value2 = 65535;
	value3 = (priv->offset + *priv->output[2]) * priv->gain;
	if (value3 > 65535)
		value3 = 65535;
	value4 = (priv->offset + *priv->output[3]) * priv->gain;
	if (value4 > 65535)
		value4 = 65535;
	value5 = (priv->offset + *priv->output[4]) * priv->gain;
	if (value5 > 65535)
		value5 = 65535;
	value6 = (priv->offset + *priv->output[5]) * priv->gain;
	if (value6 > 65535)
		value6 = 65535;
	value7 = (priv->offset + *priv->output[6]) * priv->gain;
	if (value7 > 65535)
		value7 = 65535;
	value8 = (priv->offset + *priv->output[7]) * priv->gain;
	if (value8 > 65535)
		value8 = 65535;
	
	out = (void*)priv->tx_buffer;
	
	out->value1 = htole16(value1);
	out->value2 = htole16(value2);
	out->value3 = htole16(value3);
	out->value4 = htole16(value4);
	out->value5 = htole16(value5);
	out->value6 = htole16(value6);
	out->value7 = htole16(value7);
	out->value8 = htole16(value8);
}

static void calculate_8s(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	float value1, value2, value3, value4;
	float value5, value6, value7, value8;
	struct {
		int16_t value1;
		int16_t value2;
		int16_t value3;
		int16_t value4;
		int16_t value5;
		int16_t value6;
		int16_t value7;
		int16_t value8;
	} __packed *out;
	
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
	value2 = (priv->offset + *priv->output[1]) * priv->gain;
	value3 = (priv->offset + *priv->output[2]) * priv->gain;
	value4 = (priv->offset + *priv->output[3]) * priv->gain;
	value5 = (priv->offset + *priv->output[4]) * priv->gain;
	value6 = (priv->offset + *priv->output[5]) * priv->gain;
	value7 = (priv->offset + *priv->output[6]) * priv->gain;
	value8 = (priv->offset + *priv->output[7]) * priv->gain;
	if (value1 > 32767)
		value1 = 32767;
	if (value1 < -32768)
		value1 = -32768;
	if (value2 > 32767)
		value2 = 32767;
	if (value2 < -32768)
		value2 = -32768;
	if (value3 > 32767)
		value3 = 32767;
	if (value3 < -32768)
		value3 = -32768;
	if (value4 > 32767)
		value4 = 32767;
	if (value4 < -32768)
		value4 = -32768;
	if (value5 > 32767)
		value5 = 32767;
	if (value5 < -32768)
		value5 = -32768;
	if (value6 > 32767)
		value6 = 32767;
	if (value6 < -32768)
		value6 = -32768;
	if (value7 > 32767)
		value7 = 32767;
	if (value7 < -32768)
		value7 = -32768;
	if (value8 > 32767)
		value8 = 32767;
	if (value8 < -32768)
		value8 = -32768;
	
	out = (void*)priv->tx_buffer;
	
	out->value1 = htole16((int16_t)value1);
	out->value2 = htole16((int16_t)value2);
	out->value3 = htole16((int16_t)value3);
	out->value4 = htole16((int16_t)value4);
	out->value5 = htole16((int16_t)value5);
	out->value6 = htole16((int16_t)value6);
	out->value7 = htole16((int16_t)value7);
	out->value8 = htole16((int16_t)value8);
}

static void callback_tx(struct controller_block *block, void *data)
{
	block->private->tx_buffer = data;
}


static struct controller_block_interm_list interms_1[] = {
	{ "output1", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[0])  },
	{ NULL },
};

static struct controller_block_interm_list interms_2[] = {
	{ "output1", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[0])  },
	{ "output2", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[1])  },
	{ NULL },
};

static struct controller_block_interm_list interms_4[] = {
	{ "output1", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[0])  },
	{ "output2", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[1])  },
	{ "output3", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[2])  },
	{ "output4", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[3])  },
	{ NULL },
};

static struct controller_block_interm_list interms_8[] = {
	{ "output1", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[0])  },
	{ "output2", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[1])  },
	{ "output3", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[2])  },
	{ "output4", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[3])  },
	{ "output5", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[4])  },
	{ "output6", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[5])  },
	{ "output7", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[6])  },
	{ "output8", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, output[7])  },
	{ NULL },
};

struct beckhoff_el4xxx_type {
	char *name;
	uint32_t productcode;
	int outputs;
	float gain;
	float offset;
	bool sign;
	bool driver_verified;
	struct controller_block_interm_list *interms;
};

static struct beckhoff_el4xxx_type devlist[] = {
	// 0-10 Volt blocks. Values are verified to match with:
	//  http://infosys.beckhoff.com/espanol.php?content=../content/1034/el40xx/html/bt_el40xx_functionality.htm&id=5960
	{ "EL4001", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4001, 1, 32767 / 10.0,  0.0, true, false, interms_1 },
	{ "EL4002", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4002, 2, 32767 / 10.0,  0.0, true, false, interms_2 },
	{ "EL4004", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4004, 4, 32767 / 10.0,  0.0, true, true, interms_4 },
	{ "EL4008", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4008, 8, 32767 / 10.0,  0.0, true, true, interms_8 },
	// 0-20 mA blocks. Values are verified to match with:
	// http://infosys.beckhoff.com/espanol.php?content=../content/1034/el40xx/html/bt_el40xx_functionality.htm&id=5960
	{ "EL4011", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4011, 1, 32767 / 0.020,  0.0,  true, false, interms_1 },
	{ "EL4012", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4012, 2, 32767 / 0.020,  0.0,  true, false, interms_2 },
	{ "EL4014", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4014, 4, 32767 / 0.020,  0.0,  true, false, interms_4 },
	{ "EL4018", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4018, 8, 32767 / 0.020,  0.0,  true, false, interms_8 },
	// 4-20 mA blocks. Values are verified to match with:
	// http://infosys.beckhoff.com/espanol.php?content=../content/1034/el40xx/html/bt_el40xx_functionality.htm&id=5960	{ "EL4021", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4021, 1, 4096/0.016, -0.004, false, false, interms_1 },
	{ "EL4022", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4022, 2, 32767 / 0.016, -0.004, true, false, interms_2 },
	{ "EL4024", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4024, 4, 32767 / 0.016, -0.004, true, false, interms_4 },
	{ "EL4028", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4028, 8, 32767 / 0.016, -0.004, true, false, interms_8 },

	// -10 - +10 Volt blocks. Values range from -32767 to +32767
	// see http://infosys.beckhoff.com/espanol.php?content=../content/1034/el40xx/html/bt_el40xx_functionality.htm&id=5960	{ "EL4021", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4021, 1, 4096/0.016, -0.004, false, false, interms_1 },
	// TODO: implement the EL403X series of blocks!

	// see http://infosys.beckhoff.com/espanol.php?content=../content/1034/el41xx/html/bt_el41x2_functionality_a.htm&id=6017
	// 0-10 Volt, 16-bits. limits verified with docs.
	{ "EL4102", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4102, 2, 32767 / 10.0, 0.0,   true, false, interms_2 },
	// 0-20 mA, 16-bits. limits verified with docs.
	// WARNING! There's also a EL4112-0010! Fix detection of both before
	// switching on support for the EL4112.
	{ "EL4112", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4112, 2, 32767 / 0.020, 0.0,   true, false, interms_2 },
	// TODO: implement the EL4112-0010 (-10/+10 mA, -32768 .. +32767)
	// Beckhof says this block accepts -32768! Test first, I'll believe it
	// when I've seen it work :)
	//
	// 4-20mA, 16-bits. limits verified with docs.
	{ "EL4122", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4122, 2, 32767 / 0.016, 0.0,  true, false, interms_2 },
	// -10 - +10 Volt, 16-bits. Beckhoff says this block accepts -32768!
	// Test this first, I'll believe it when I see it.
	{ "EL4132", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4132, 2, 32768 / 10.0, 0.0,   true, true, interms_2  },

	// see http://infosys.beckhoff.com/espanol.php?content=../content/1034/el41xx/html/bt_el41x4_functionality.htm&id=6025
	// HAHAHA :)
	// Beckhoff has copied the page for the EL41x2 here and forgot to
	// update the content. No definitive info on this block...
	{ "EL4114", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4114, 4, 65536 / 0.020, 0.0,   false, false, interms_4 },
	// TODO: There also is an EL4124. Implement.
	// No idea what this thing's PDO does:
	{ "EL4134", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4134, 4, 32768 / 10.0, 0.0,   true, false, interms_4  },

	// see http://infosys.beckhoff.com/content/1034/el47xx/html/bt_el47xx_procdata.htm?id=6060
	// There is also a EL4712: 0 - 20 mA. 
	// There is also a EL4732: -10 - +10 Volt. lower limit: -32769?!?!!
	//  Hahaha :)
	
};

static int pre_operational_hook(struct esc_device *esc)
{
	struct controller_block *block = esc->block;

	esc_sm_set(block->private->dev, 3);

	if (!block->private->candev)
		block->private->candev = esc_coe_create(esc);

	return 0;
}

static struct controller_block * block_beckhoff_el4xxx_create(char *name, int argc, va_list ap)
{
	struct controller_block *block;
	int i;
	int dev = -1;
	char *eltype;
	char *busname;
	int devno;
	struct esc_device *esc;
		
	uint32_t vendor, product;

	busname = va_arg(ap, char *);
	devno = va_arg(ap, int);
	eltype = va_arg(ap, char *);
	
	for (i = 0; i < sizeof(devlist)/sizeof(struct beckhoff_el4xxx_type); i++) {
		if (!strcmp(eltype, devlist[i].name)) {
			dev = i;
			break;
		}
	}
	if (dev < 0) {
		log_send(LOG_T_ERROR, "Unsupported device: '%s'", eltype);
		return NULL;
	}
	if (! devlist[i].driver_verified) {
		log_send(LOG_T_ERROR, "This driver hasn't been verified to "
		    "work with the '%s'. Please implement it first!", eltype);
		return NULL;
	}

	esc = esc_device_create(busname, devno, name);
	
	vendor = esc_esi_vendorid_get(esc);
	product = esc_esi_productcode_get(esc);

	if (vendor != ESC_ESI_VENDORID_BECKHOFF ||
	    product != devlist[i].productcode) {
		log_send(LOG_T_ERROR, "%s: Device not found\n", name);
		return NULL;
	}

	
	block = controller_block_alloc("beckhoff_el4xxx", name, 
	    sizeof(struct controller_block_private));
	if (!block)
		return NULL;

	block->private->dev = esc;
	esc->block = block;
	esc->block_tx = block;
	esc_esi_device_fill(esc);
	esc->callback_tx = callback_tx;
	esc->pre_operational_hook = pre_operational_hook;

	block->private->dev->dc_sync.sync0_active = false;
	block->private->dev->dc_sync.sync1_active = false;

	esc_device_initialize_operational(esc);
	
	if (esc->sm[esc->tx_pdo_sm].len !=
	    0x2 * devlist[dev].outputs) {
		log_send(LOG_T_ERROR, "TX PDO length mismatch");
	}
	
//	esc_dc_init(&esc->addr);
//	esc_dc_sync0_cycle_time(&esc->addr, 1000*1000);

/*
	addr.addr.position.off = ESC_ADDR_MAP_DC_ACTIVATION_REGISTER;
	val8 = 0x00;
	ec_datagram_write(&addr, &val8, 1);


	addr.addr.position.off = ESC_ADDR_MAP_WATCHDOG_TIME_PD;
	leval16 = 0x0000;
	ec_datagram_write(&addr, &leval16, 2);


	addr.addr.position.off = ESC_ADDR_MAP_WATCHDOG_COUNTER_PD;
	val8 = 0;
	ec_datagram_write(&addr, &val8, 1);
}
*/

	/* Inputs on the hardware are outputs on this software block */
	if (controller_block_interm_list_init(block, devlist[dev].interms))
		return NULL;

	switch (devlist[dev].outputs) {
		case 1:
			if (devlist[dev].sign)
				block->calculate = calculate_1s;
			else
				block->calculate = calculate_1u;
			break;
		case 2:
			if (devlist[dev].sign)
				block->calculate = calculate_2s;
			else
				block->calculate = calculate_2u;
			break;
		case 4:
			if (devlist[dev].sign)
				block->calculate = calculate_4s;
			else
				block->calculate = calculate_4u;
			break;
		case 8:
			if (devlist[dev].sign)
				block->calculate = calculate_8s;
			else
				block->calculate = calculate_8u;
			break;
	}
	block->private->gain = devlist[dev].gain;
	block->private->offset = devlist[dev].offset;
	
	if (controller_block_add(block))
		goto err_add;
		
	return block;

err_add:
	controller_block_free(block);
	return NULL;
}

BLOCK_CREATE(beckhoff_el4xxx) = {
	.create = block_beckhoff_el4xxx_create,
	.args = { "char*,int,char*", NULL },
};