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

	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 "block_ec.h"
#include "ec.h"
28
#include "log.h"
Jeroen Vreeken's avatar
Jeroen Vreeken committed
29

Jeroen Vreeken's avatar
Jeroen Vreeken committed
30
31
static struct controller_block *ec_block = NULL;

Jeroen Vreeken's avatar
Jeroen Vreeken committed
32
33
34
35
36
static void calculate(struct controller_block *ec)
{
	ec_datagram_sample();
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
37
38
39
40
41
42
43
44
45
46
47
48
49
static void param_set(struct controller_block *spg, int param, va_list val)
{
	int enabled = va_arg(val, int);

	ec_rx_pdo_enable(enabled);
	ec_tx_pdo_enable(enabled);
}

static struct controller_block_param_list params[] = {
	{ "pdo_enabled", true },
	{ NULL },
};

Jeroen Vreeken's avatar
Jeroen Vreeken committed
50
51
52
53
54
55
56
57
struct controller_block * block_ec_create(char *name, va_list ap)
{
	struct controller_block *ec;
	int ret;
	char *ifname;
	int nr_slaves;
	int single_cycle;
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
58
59
60
61
62
63
64
	/* For now we only support a single ethercat interface,
	   but we allow multiple inits
	 */
	if (ec_block) {
		return ec_block;
	}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
65
66
67
68
69
70
71
72
73
74
75
	ifname = va_arg(ap, char *);
	nr_slaves = va_arg(ap, int);
	single_cycle = va_arg(ap, int);
	
	ret = ec_init(ifname, single_cycle);

	if (ret != 0)
		return NULL;

	while ((ret = ec_slave_count()) != nr_slaves) {
		if (ret == -1) {
76
			log_send(LOG_T_ERROR, "Error counting slaves");
77
		} else if (ret < nr_slaves) {
78
79
			log_send(LOG_T_ERROR, 
			   "Found only %d slaves, waiting for %d.",
Jeroen Vreeken's avatar
Jeroen Vreeken committed
80
			   ret, nr_slaves);
81
82
83
84
85
		} else {
			log_send(LOG_T_WARNING,
			   "More slaves found (%d) than expected (%d).",
			   ret, nr_slaves);
			break;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
86
87
88
89
		}
		sleep(10);
	}

90
	ec = controller_block_alloc("ec", name, 0);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
91
92
93
94
	if (!ec)
		goto err_ec;

	ec->calculate = calculate;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
95
96
97
98

	if (controller_block_param_list_init(ec, params))
		goto err_params;

Jeroen Vreeken's avatar
Jeroen Vreeken committed
99
	ec->param_get = NULL;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
100
	ec->param_set = param_set;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
101
102

	controller_block_add(ec);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
103
104
105

	ec_block = ec;

Jeroen Vreeken's avatar
Jeroen Vreeken committed
106
107
	return ec;

Jeroen Vreeken's avatar
Jeroen Vreeken committed
108
err_params:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
109
	free(ec);
110
err_ec:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
111
112
	return NULL;
}