block_beckhoff_el4xxx.c 17.1 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
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
	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>

Jeroen Vreeken's avatar
Jeroen Vreeken committed
26
27
28
29
30
31
#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>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
32
33

struct controller_block_private {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
34
	float	*output[8];
Jeroen Vreeken's avatar
Jeroen Vreeken committed
35
36
37
38
39

	float gain;
	float offset;
	
	unsigned char *tx_buffer;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
40
41

	struct esc_device *dev;
42
	struct canopen_dev *candev;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
43
44
45
46
47
48
49
50
};

static void calculate_1u(struct controller_block *block)
{
	struct controller_block_private *priv = block->private;
	float value1;
	struct {
		uint16_t value1;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
51
	} __packed *out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
52
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
53
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
54
55
56
57
58
59
60
61
62
63
64
65
66
67
	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;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
68
	} __packed *out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
69
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
70
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
	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;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
88
	} __packed *out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
89
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
90
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
91
92
	if (value1 > 65535)
		value1 = 65535;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
93
	value2 = (priv->offset + *priv->output[1]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
	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;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
110
	} __packed *out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
111
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
112
113
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
	value2 = (priv->offset + *priv->output[1]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
	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;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
138
	} __packed *out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
139
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
140
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
141
142
	if (value1 > 65535)
		value1 = 65535;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
143
	value2 = (priv->offset + *priv->output[1]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
144
145
	if (value2 > 65535)
		value2 = 65535;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
146
	value3 = (priv->offset + *priv->output[2]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
147
148
	if (value3 > 65535)
		value3 = 65535;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
149
	value4 = (priv->offset + *priv->output[3]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
	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;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
170
	} __packed *out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
171
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
172
173
174
175
	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;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
	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;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
215
	} __packed *out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
216
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
217
	value1 = (priv->offset + *priv->output[0]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
218
219
	if (value1 > 65535)
		value1 = 65535;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
220
	value2 = (priv->offset + *priv->output[1]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
221
222
	if (value2 > 65535)
		value2 = 65535;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
223
	value3 = (priv->offset + *priv->output[2]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
224
225
	if (value3 > 65535)
		value3 = 65535;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
226
	value4 = (priv->offset + *priv->output[3]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
227
228
	if (value4 > 65535)
		value4 = 65535;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
229
	value5 = (priv->offset + *priv->output[4]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
230
231
	if (value5 > 65535)
		value5 = 65535;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
232
	value6 = (priv->offset + *priv->output[5]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
233
234
	if (value6 > 65535)
		value6 = 65535;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
235
	value7 = (priv->offset + *priv->output[6]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
236
237
	if (value7 > 65535)
		value7 = 65535;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
238
	value8 = (priv->offset + *priv->output[7]) * priv->gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
	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;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
268
269
270
271
272
273
274
275
276
277
	} __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;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
	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);
}

323
324
325
static void callback_tx(struct controller_block *block, void *data)
{
	block->private->tx_buffer = data;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
326
327
}

328

Jeroen Vreeken's avatar
Jeroen Vreeken committed
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
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 },
};

Jeroen Vreeken's avatar
Jeroen Vreeken committed
360
361
362
363
364
365
366
struct beckhoff_el4xxx_type {
	char *name;
	uint32_t productcode;
	int outputs;
	float gain;
	float offset;
	bool sign;
367
	bool driver_verified;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
368
	struct controller_block_interm_list *interms;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
369
370
371
};

static struct beckhoff_el4xxx_type devlist[] = {
372
373
	// 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
374
375
376
	{ "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 },
377
	{ "EL4008", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4008, 8, 32767 / 10.0,  0.0, true, true, interms_8 },
378
379
380
381
382
383
384
385
386
387
388
	// 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 },
389

390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
	// -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.
Jeroen Vreeken's avatar
Jeroen Vreeken committed
409
	{ "EL4132", ESC_ESI_PRODUCTCODE_BECKHOFF_EL4132, 2, 32768 / 10.0, 0.0,   true, true, interms_2  },
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424

	// 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 :)
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
425
426
};

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

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

433
	if (!block->private->candev)
434
		block->private->candev = esc_coe_create(esc);
435
436
437
438

	return 0;
}

439
static struct controller_block * block_beckhoff_el4xxx_create(char *name, int argc, va_list ap)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
440
441
442
443
444
{
	struct controller_block *block;
	int i;
	int dev = -1;
	char *eltype;
445
	char *busname;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
446
	int devno;
447
	struct esc_device *esc;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
448
		
Jeroen Vreeken's avatar
Jeroen Vreeken committed
449
	uint32_t vendor, product;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
450

451
	busname = va_arg(ap, char *);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
452
	devno = va_arg(ap, int);
453
	eltype = va_arg(ap, char *);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
454
455
456
457
458
459
460
	
	for (i = 0; i < sizeof(devlist)/sizeof(struct beckhoff_el4xxx_type); i++) {
		if (!strcmp(eltype, devlist[i].name)) {
			dev = i;
			break;
		}
	}
461
462
	if (dev < 0) {
		log_send(LOG_T_ERROR, "Unsupported device: '%s'", eltype);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
463
		return NULL;
464
	}
465
466
	if (! devlist[i].driver_verified) {
		log_send(LOG_T_ERROR, "This driver hasn't been verified to "
467
		    "work with the '%s'. Please implement it first!", eltype);
468
469
		return NULL;
	}
470
471

	esc = esc_device_create(busname, devno, name);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
472
	
473
474
	vendor = esc_esi_vendorid_get(esc);
	product = esc_esi_productcode_get(esc);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
475
476
477

	if (vendor != ESC_ESI_VENDORID_BECKHOFF ||
	    product != devlist[i].productcode) {
478
		log_send(LOG_T_ERROR, "%s: Device not found\n", name);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
479
480
		return NULL;
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
481

Jeroen Vreeken's avatar
Jeroen Vreeken committed
482
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
483
484
	block = controller_block_alloc("beckhoff_el4xxx", name, 
	    sizeof(struct controller_block_private));
Jeroen Vreeken's avatar
Jeroen Vreeken committed
485
486
487
	if (!block)
		return NULL;

488
	block->private->dev = esc;
489
	esc->block = block;
490
	esc->block_tx = block;
491
	esc_esi_device_fill(esc);
492
	esc->callback_tx = callback_tx;
493
	esc->pre_operational_hook = pre_operational_hook;
494
495
496
497

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

498
	esc_device_initialize_operational(esc);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
499
	
500
	if (esc->sm[esc->tx_pdo_sm].len !=
Jeroen Vreeken's avatar
Jeroen Vreeken committed
501
502
503
	    0x2 * devlist[dev].outputs) {
		log_send(LOG_T_ERROR, "TX PDO length mismatch");
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
504
	
505
506
//	esc_dc_init(&esc->addr);
//	esc_dc_sync0_cycle_time(&esc->addr, 1000*1000);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
507

Jeroen Vreeken's avatar
Jeroen Vreeken committed
508
509
/*
	addr.addr.position.off = ESC_ADDR_MAP_DC_ACTIVATION_REGISTER;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
510
511
512
513
	val8 = 0x00;
	ec_datagram_write(&addr, &val8, 1);


514
515
516
	addr.addr.position.off = ESC_ADDR_MAP_WATCHDOG_TIME_PD;
	leval16 = 0x0000;
	ec_datagram_write(&addr, &leval16, 2);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
517
518


519
520
521
522
	addr.addr.position.off = ESC_ADDR_MAP_WATCHDOG_COUNTER_PD;
	val8 = 0;
	ec_datagram_write(&addr, &val8, 1);
}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
523
*/
Jeroen Vreeken's avatar
Jeroen Vreeken committed
524

Jeroen Vreeken's avatar
Jeroen Vreeken committed
525
526
	/* Inputs on the hardware are outputs on this software block */
	if (controller_block_interm_list_init(block, devlist[dev].interms))
Jeroen Vreeken's avatar
Jeroen Vreeken committed
527
		return NULL;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
528

Jeroen Vreeken's avatar
Jeroen Vreeken committed
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
	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;
	
558
559
560
	if (controller_block_add(block))
		goto err_add;
		
Jeroen Vreeken's avatar
Jeroen Vreeken committed
561
	return block;
562
563
564
565

err_add:
	controller_block_free(block);
	return NULL;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
566
}
567

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