block_beckhoff_el5101.c 5.59 KB
Newer Older
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1
/*
Jeroen Vreeken's avatar
Jeroen Vreeken committed
2
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2009, 2013
Jeroen Vreeken's avatar
Jeroen Vreeken committed
3
	Copyright Stichting C.A. Muller Radioastronomiestation, 2009
4
5
6
7
	Copyright (c) 2010, 2012,
	     Daan Vreeken <Daan @ Vitsch . nl> - Vitsch Electronics
	     All rights reserved.

Jeroen Vreeken's avatar
Jeroen Vreeken committed
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

	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"
Jeroen Vreeken's avatar
Jeroen Vreeken committed
34
35
36
#include "esc_esi.h"
#include "esc_coe.h"
#include "esc_id.h"
37
#include "log.h"
Jeroen Vreeken's avatar
Jeroen Vreeken committed
38

Jeroen Vreeken's avatar
Jeroen Vreeken committed
39
40
41
42
43
44

#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


Jeroen Vreeken's avatar
Jeroen Vreeken committed
45
46
47
48
49
50
51
52
53
54
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;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
55
56
57

	struct esc_device *dev;
	struct canopen_dev *can_dev;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
58
59
60
61
62
63
64
65
66
};

static void calculate(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	struct {
		uint8_t	 status;
		uint16_t value;
		uint16_t latch;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
67
	} __packed *rxdata;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
68
69
70
	struct {
		uint8_t ctrl;
		uint16_t value;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
71
	} __packed *txdata;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
	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);
}


93
94
95
96
97
98
99
100
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 },
};

Jeroen Vreeken's avatar
Jeroen Vreeken committed
101
102
103
104
struct controller_block * block_beckhoff_el5101_create(char *name, va_list ap)
{
	struct controller_block *block;
	struct ec_dgram_addr addr;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
105
	struct canopen_dev *can_dev;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
106
	int devno;
107
	struct timespec timeout = { 1, 0 };
Jeroen Vreeken's avatar
Jeroen Vreeken committed
108
109
	uint32_t vendor, product, leval32;
	uint16_t leval16;		
Jeroen Vreeken's avatar
Jeroen Vreeken committed
110
111
112

	devno = va_arg(ap, int);
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
113
114
115
116
117
118
	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) {
119
		log_send(LOG_T_ERROR, "%s: Device not found\n", name);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
120
121
122
		return NULL;
	}
	
123
124
	block = controller_block_alloc("beckhoff_el5101", name,
	    sizeof(struct controller_block_private));
Jeroen Vreeken's avatar
Jeroen Vreeken committed
125
126
127
	if (!block)
		return NULL;

Jeroen Vreeken's avatar
Jeroen Vreeken committed
128
129
	block->private->dev = esc_device_create(&addr, name);
	esc_esi_device_fill(block->private->dev);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
130

Jeroen Vreeken's avatar
Jeroen Vreeken committed
131
	block->private->dev->dc_sync.sync0_active = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
132

Jeroen Vreeken's avatar
Jeroen Vreeken committed
133
	esc_device_initialize_pre_operational(block->private->dev);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
134
135


Jeroen Vreeken's avatar
Jeroen Vreeken committed
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
	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));
Jeroen Vreeken's avatar
Jeroen Vreeken committed
179

180
	if (esc_al_state_set(&addr, ESC_AL_STATE_SAFE_OPERATIONAL, &timeout) < 0)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
181
182
		log_send(LOG_T_ERROR, 
		    "%s: Could not go to state safe operational", name);
183
	if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
184
185
		log_send(LOG_T_ERROR, 
		    "%s: Could not go to state operational", name);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
186
187

	/* Inputs on the hardware are outputs on this software block */
188
189
	if (controller_block_outterm_list_init(block, outterms))
		goto err_outputs;	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
190
191
192
193
194
195
196
197
198
	
	block->calculate = calculate;
	
	block->private->enc = 0;
	block->private->gain = 1.0;
	block->private->offset = 0.0;
	
	controller_block_add(block);
	return block;
199
200
201
202
	
err_outputs:
	controller_block_free(block);
	return NULL;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
203
}