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

	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/>.

 */
/*
	esc_watchdog.c
	
	An ethercat ESC may have a watchdog.
	This code checks the device's flags if one is requested, and if needed
	enables it (or if requested disables it).	
 */


Jeroen Vreeken's avatar
Jeroen Vreeken committed
28
29
30
31
#include <ec/esc_watchdog.h>
#include <ec/esc_registers.h>
#include <controller/controller_sample.h>
#include <log/log.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
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
115
116
117
118
119
120

int esc_watchdog_enable(struct esc_device *dev, struct timespec *timeout)
{
	struct ec_dgram_addr addr = dev->addr;
	uint16_t leval16;
	uint16_t divider;
	unsigned long increment_ns;
	unsigned long time_pd;
	int r = 0;

	addr.addr.position.off = ESC_ADDR_MAP_WATCHDOG_DIVIDER;
	if (ec_datagram_read(&addr, &leval16, 2) != 2) {
		log_send(LOG_T_ERROR, "%s: Could not read watchdog divider",
		    dev->name);
		r = -1;
	}
	
	/* divider: number of 40ns (25MHz) increments - 2 */
	divider = le16toh(leval16) + 2;
	increment_ns = 40 * divider;

	time_pd = timeout->tv_nsec / increment_ns;
	time_pd += (timeout->tv_sec * 1000 * 1000 * 1000) / increment_ns;
	
	leval16 = htole16(time_pd);
	addr.addr.position.off = ESC_ADDR_MAP_WATCHDOG_TIME_PD;
	if (ec_datagram_write(&addr, &leval16, 2) != 2) {
		log_send(LOG_T_ERROR, "%s: Could not set PD watchdog time",
		    dev->name);
		r = -1;
	} else {
		log_send(LOG_T_DEBUG, 
		    "%s: PD watchdog time set to: %lu increments of %lu ns",
		    dev->name, time_pd, increment_ns);
	}

	return r;	
}

int esc_watchdog_disable(struct esc_device *dev)
{
	struct ec_dgram_addr addr = dev->addr;
	uint16_t leval16;
	int r = 0;
	
	addr.addr.position.off = ESC_ADDR_MAP_WATCHDOG_TIME_PD;
	leval16 = 0x0000;
	if (ec_datagram_write(&addr, &leval16, 2) != 2) {
		log_send(LOG_T_ERROR, "%s: Could not disable PD watchdog",
		    dev->name);
		r = -1;
	}

	return r;
}

int esc_watchdog_init(struct esc_device *dev)
{
	struct ec_dgram_addr addr = dev->addr;
	uint8_t val8;
	int r = 0;
	double timeout_sec;

	timeout_sec = controller_sample_period() * 2;
	
	addr.addr.position.off = ESC_ADDR_MAP_WATCHDOG_COUNTER_PD;
	val8 = 0;
	if (ec_datagram_write(&addr, &val8, 1) != 1) {
		log_send(LOG_T_WARNING, 
		    "%s: Could not clear PD watchdog counter. "
		    "Probably no support",
		    dev->name);
		r = -1;
	}

	if (dev->tx_pdo_watchdog||1) {
		struct timespec timeout;
		
		timeout.tv_sec = (int)(timeout_sec);
		timeout.tv_nsec = (int)(timeout_sec * 1000000000) % 1000000000;
		
		r = esc_watchdog_enable(dev, &timeout);
	} else {
		r = esc_watchdog_disable(dev);
	}
	
	return r;
}