block_beckhoff_el1xxx.c 7.19 KB
Newer Older
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1
/*
2
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2009, 2011, 2013
Jeroen Vreeken's avatar
Jeroen Vreeken committed
3
	Copyright Stichting C.A. Muller Radioastronomiestation, 2009, 2011
4
	Copyright Daan Vreeken <Daan@Vitsch.nl> - Vitsch Electronics, 2010
Jeroen Vreeken's avatar
Jeroen Vreeken committed
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

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

#include <controller/controller_block.h>

#include "block_beckhoff_el1xxx.h"
#include "ec.h"
#include "esc.h"
29
#include "log.h"
Jeroen Vreeken's avatar
Jeroen Vreeken committed
30
31
32
33
34
35
36
37
38
39
40
41

struct controller_block_private {
	bool	input1;
	bool	input2;
	bool	input3;
	bool	input4;
	bool	input5;
	bool	input6;
	bool	input7;
	bool	input8;
	
	unsigned char *rx_buffer;
42
43
	
	struct esc_device *dev;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
44
45
46
47
48
49
50
51
52
};

static void calculate_8(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	
	ec_rx_pdo(controller_samplenr);
	
	priv->input1 = (priv->rx_buffer[0] & 1);
53
54
55
56
57
58
59
	priv->input2 = (priv->rx_buffer[0] & 2);
	priv->input3 = (priv->rx_buffer[0] & 4);
	priv->input4 = (priv->rx_buffer[0] & 8);
	priv->input5 = (priv->rx_buffer[0] & 16);
	priv->input6 = (priv->rx_buffer[0] & 32);
	priv->input7 = (priv->rx_buffer[0] & 64);
	priv->input8 = (priv->rx_buffer[0] & 128);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
60
61
62
63
64
65
66
67
68
}

static void calculate_4(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	
	ec_rx_pdo(controller_samplenr);
	
	priv->input1 = (priv->rx_buffer[0] & 1);
69
70
71
	priv->input2 = (priv->rx_buffer[0] & 2);
	priv->input3 = (priv->rx_buffer[0] & 4);
	priv->input4 = (priv->rx_buffer[0] & 8);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
72
73
74
75
76
77
78
79
80
}

static void calculate_2(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	
	ec_rx_pdo(controller_samplenr);
	
	priv->input1 = (priv->rx_buffer[0] & 1);
81
	priv->input2 = (priv->rx_buffer[0] & 2);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
82
83
84
85
86
87
}

struct beckhoff_el1xxx_type {
	char *name;
	uint32_t productcode;
	int inputs;
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
	struct controller_block_outterm_list *outterms;
};

static struct controller_block_outterm_list outterms_2[] = {
	{ "input1",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input1)  },
	{ "input2",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input2)  },
	{ NULL },
};

static struct controller_block_outterm_list outterms_4[] = {
	{ "input1",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input1)  },
	{ "input2",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input2)  },
	{ "input3",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input3)  },
	{ "input4",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input4)  },
	{ NULL },
};

static struct controller_block_outterm_list outterms_8[] = {
	{ "input1",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input1)  },
	{ "input2",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input2)  },
	{ "input3",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input3)  },
	{ "input4",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input4)  },
	{ "input5",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input5)  },
	{ "input6",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input6)  },
	{ "input7",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input7)  },
	{ "input8",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input8)  },
	{ NULL },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
115
116
117
};

static struct beckhoff_el1xxx_type devlist[] = {
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
	{ "EL1002", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1002, 2, outterms_2 },
	{ "EL1004", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1004, 4, outterms_4 },
	{ "EL1008", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1008, 8, outterms_8 },
	{ "EL1012", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1012, 2, outterms_2 },
	{ "EL1014", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1014, 4, outterms_4 },
	{ "EL1018", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1018, 8, outterms_8 },
	{ "EL1024", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1024, 4, outterms_4 },
	{ "EL1034", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1034, 4, outterms_4 },
	{ "EL1084", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1084, 4, outterms_4 },
	{ "EL1088", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1088, 8, outterms_8 },
	{ "EL1094", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1094, 4, outterms_4 },
	{ "EL1098", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1098, 8, outterms_8 },
	{ "EL1104", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1104, 4, outterms_4 },
	{ "EL1114", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1114, 4, outterms_4 },
	{ "EL1124", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1124, 4, outterms_4 },
	{ "EL1134", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1134, 4, outterms_4 },
	{ "EL1144", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1144, 4, outterms_4 },
	{ "EL1202", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1202, 2, outterms_2 },
	{ "EL1252", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1252, 2, outterms_2 },
	{ "EL1262", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1262, 2, outterms_2 },
	{ "EL1702", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1702, 2, outterms_2 },
	{ "EL1712", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1712, 2, outterms_2 },
	{ "EL1722", ESC_ESI_PRODUCTCODE_BECKHOFF_EL1722, 2, outterms_2 },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
141
142
143
144
145
146
147
148
149
150
151
};

struct controller_block * block_beckhoff_el1xxx_create(char *name, va_list ap)
{
	struct controller_block *block;
	struct ec_dgram_addr addr;
	int i;
	int dev = -1;
	char *eltype;
	int devno;
	bool sim = !ec_initialized();
152
	struct timespec timeout = { 1, 0 };
Jeroen Vreeken's avatar
Jeroen Vreeken committed
153
154
155
156
157
158
159
160
161
162
163
		
	eltype = va_arg(ap, char *);
	devno = va_arg(ap, int);
	
	for (i = 0; i < sizeof(devlist)/sizeof(struct beckhoff_el1xxx_type); i++) {
		if (!strcmp(eltype, devlist[i].name)) {
			dev = i;
			break;
		}
	}
	if (dev < 0)
164
		goto err_ethercat;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
165
166
	
	if (!sim) {
167
168
169
170
171
172
173
174
		uint32_t vendor, product;
		
		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 != devlist[dev].productcode) {
175
176
			log_send(LOG_T_ERROR, "%s: Device not found\n", name);
			goto err_ethercat;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
177
178
179
		}
	}
		
180
181
182
	block = controller_block_alloc(
	    sim ? "beckhoff_el1xxx_sim" : "beckhoff_el1xxx",
	    name, sizeof(struct controller_block_private));
Jeroen Vreeken's avatar
Jeroen Vreeken committed
183
184

	if (!sim) {
185
		block->private->dev = esc_esi_device_create(&addr);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
186
	
187
		block->private->rx_buffer = block->private->dev->rx_data;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
188

189
190
		if (esc_al_state_set(&addr, ESC_AL_STATE_SAFE_OPERATIONAL, &timeout) < 0)
			log_send(LOG_T_ERROR, "Could not go to state safe operational\n");
Jeroen Vreeken's avatar
Jeroen Vreeken committed
191
192
193
	}
	
	/* Inputs on the hardware are outputs on this software block */
194
195
	if (controller_block_outterm_list_init(block, devlist[dev].outterms))
		goto err_outputs;	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
	
	switch (devlist[dev].inputs) {
		case 2:
			block->calculate = calculate_2;
			break;
		case 4:
			block->calculate = calculate_4;
			break;
		case 8:
			block->calculate = calculate_8;
			break;
	}
	if (sim)
		block->calculate = NULL;
	
	controller_block_add(block);
	return block;
213
214
215
216
217

err_outputs:
	controller_block_free(block);
err_ethercat:
	return NULL;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
218
}