block_beckhoff_el5101.c

/*
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2009, 2013
	Copyright Stichting C.A. Muller Radioastronomiestation, 2009
	Copyright (c) 2010, 2012,
	     Daan Vreeken <Daan @ Vitsch . nl> - Vitsch Electronics
	     All rights reserved.


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

#include <controller/controller_block.h>

#include "block_beckhoff_el5101.h"
#include "ec.h"
#include "esc.h"
#include "esc_esi.h"
#include "esc_coe.h"
#include "esc_id.h"
#include "log.h"


#define EL5101_OBJ_SM_OUTPUT 			0x1c32
#define EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE		1
#define EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE_SYNC0		2


struct controller_block_private {
	float	 converted_value;
	int16_t  enc;
	int32_t value;
	
	double 	gain;
	double 	offset;
	
	unsigned char *rx_buffer;
	unsigned char *tx_buffer;

	struct esc_device *dev;
	struct canopen_dev *can_dev;
};

static void calculate(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	struct {
		uint8_t	 status;
		uint16_t value;
		uint16_t latch;
	} __packed *rxdata;
	struct {
		uint8_t ctrl;
		uint16_t value;
	} __packed *txdata;
	uint16_t enc;
	int32_t value = priv->value;
	
	rxdata = (void*)priv->rx_buffer;
	txdata = (void*)priv->tx_buffer;
	
	ec_rx_pdo(controller_samplenr);

	enc = le16toh(rxdata->value);
	value += (int16_t)(enc - priv->enc);
	priv->enc = enc;
	priv->value = value;
	priv->converted_value = (value * priv->gain) + priv->offset;

	txdata->ctrl = 0;
	txdata->value = 0;

	ec_tx_pdo(controller_samplenr);
}


static struct controller_block_outterm_list outterms[] = {
	{ "converted_value",  CONTROLLER_BLOCK_TERM_FLOAT, 
	    offsetof(struct controller_block_private, converted_value)  },
	{ "value",  CONTROLLER_BLOCK_TERM_UINT16, 
	    offsetof(struct controller_block_private, enc)  },
	{ NULL },
};

struct controller_block * block_beckhoff_el5101_create(char *name, va_list ap)
{
	struct controller_block *block;
	struct ec_dgram_addr addr;
	struct canopen_dev *can_dev;
	int devno;
	struct timespec timeout = { 1, 0 };
	uint32_t vendor, product, leval32;
	uint16_t leval16;		

	devno = va_arg(ap, int);
	
	ec_addr_set_auto_inc_nr(&addr, devno);
	vendor = esc_esi_vendorid_get(&addr);
	product = esc_esi_productcode_get(&addr);

	if (vendor != ESC_ESI_VENDORID_BECKHOFF ||
	    product != ESC_ESI_PRODUCTCODE_BECKHOFF_EL5101) {
		log_send(LOG_T_ERROR, "%s: Device not found\n", name);
		return NULL;
	}
	
	block = controller_block_alloc("beckhoff_el5101", name,
	    sizeof(struct controller_block_private));
	if (!block)
		return NULL;

	block->private->dev = esc_device_create(&addr, name);
	esc_esi_device_fill(block->private->dev);

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

	esc_device_initialize_pre_operational(block->private->dev);


	if (block->private->dev->sm[block->private->dev->tx_pdo_sm].len != 3) {
		log_send(LOG_T_ERROR, "TX PDO length mismatch");
	}
	if (block->private->dev->sm[block->private->dev->rx_pdo_sm].len != 5) {
		log_send(LOG_T_ERROR, "RX PDO length mismatch");
	}

	block->private->tx_buffer = block->private->dev->tx_data;
	block->private->rx_buffer = block->private->dev->rx_data;

	can_dev = esc_coe_create(block->private->dev->mailbox);
	block->private->can_dev = can_dev;

	canopen_read_param(can_dev, 
	    EL5101_OBJ_SM_OUTPUT, EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE, 
	    &leval16, sizeof(leval16));
	log_send(LOG_T_DEBUG, "mode: %d", le16toh(leval16));

	canopen_read_param(can_dev, 
	    EL5101_OBJ_SM_OUTPUT, 3, 
	    &leval32, sizeof(leval32));
	log_send(LOG_T_DEBUG, "cycle time: %d ns", le32toh(leval32));
	leval32 = htole32(block->private->dev->dc_sync.sync0_cycle);
	log_send(LOG_T_DEBUG, "cycle time: %d ns", le32toh(leval32));
	canopen_write_param(can_dev, 
	    EL5101_OBJ_SM_OUTPUT, 3, 
	    &leval32, sizeof(leval32));

	canopen_read_param(can_dev, 
	    EL5101_OBJ_SM_OUTPUT, 4, 
	    &leval16, sizeof(leval16));
	log_send(LOG_T_DEBUG, "available modes: 0x%02x", le16toh(leval16));

	/* switch device to dc synchronised mode */
	leval16 = htole16(EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE_SYNC0);
	canopen_write_param(can_dev, 
	    EL5101_OBJ_SM_OUTPUT, EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE, 
	    &leval16, sizeof(leval16));

	canopen_read_param(can_dev, 
	    EL5101_OBJ_SM_OUTPUT, EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE, 
	    &leval16, sizeof(leval16));
	log_send(LOG_T_DEBUG, "mode: %d", le16toh(leval16));

	if (esc_al_state_set(&addr, ESC_AL_STATE_SAFE_OPERATIONAL, &timeout) < 0)
		log_send(LOG_T_ERROR, 
		    "%s: Could not go to state safe operational", name);
	if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
		log_send(LOG_T_ERROR, 
		    "%s: Could not go to state operational", name);

	/* Inputs on the hardware are outputs on this software block */
	if (controller_block_outterm_list_init(block, outterms))
		goto err_outputs;	
	
	block->calculate = calculate;
	
	block->private->enc = 0;
	block->private->gain = 1.0;
	block->private->offset = 0.0;
	
	controller_block_add(block);
	return block;
	
err_outputs:
	controller_block_free(block);
	return NULL;
}