dt_az_safety.c 9.82 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
28
29
30
31
32
33
34
35
36
37
38
39
/*
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008
	Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2008

	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_block.h"
#include "dt_az_safety.h"
#include "log.h"

/*
       inputs          outputs
       nr name         nr name

     ---------------------------------
     |                               |
  ---| 0  speed_in     0  speed_out  |----
     |	                             |
  ---| 1  position_in  1  safe_out   |----
     |	                             |
  ---| 2  torque_in    2  torque_out |----
     |	                             |
40
  ---| 3  safety_in_positive         |
Jeroen Vreeken's avatar
Jeroen Vreeken committed
41
     |                               |
42
  ---| 4  safety_in_negative         |
Jeroen Vreeken's avatar
Jeroen Vreeken committed
43
     |                               |
44
  ---| 5  enable                     |
Jeroen Vreeken's avatar
Jeroen Vreeken committed
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
     |                               |
     ---------------------------------

	This is not a generic block.
	It implements the safety checks specific for the dt azimuth
	control loop.

	Checks:	
		+ Enabled
		+ Direction limit if position beyond max/min
		+ Speed limit if within safe-zone.

       min           safe_zone_min                safe_zone_max           max
        |------------------|----------------------------|------------------|
        safe_zone_min_speed       normal operation      safe_zone_max_speed

 */

struct controller_block_private {
	float *speed_in;
	float *position_in;
	float *torque_in;
67
68
	bool *safety_in_positive;
	bool *safety_in_negative;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
69
70
71
72
73
74
75
76
77
78
79
80
81
82
	float speed_out;
	bool safe_out;
	float torque_out;
	bool *enable_in;

	float position_min;
	float position_max;
	float emergency_torque;
	
	float safe_zone_min;
	float safe_zone_max;
	float safe_zone_min_speed;
	float safe_zone_max_speed;

83
84
85
86
87
88
89
	/* keep log state to prevent flooding */
	bool err_max;
	bool err_min;
	bool warn_safe_max;
	bool warn_safe_min;
	bool err_hwsafe_positive;
	bool err_hwsafe_negative;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
90
91

	bool recover;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
92
93
94
95
96
};

static void calculate(struct controller_block *safety)
{
	float out;
97
	bool safe = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
98
99
100
101
102
103
104
	bool enabled = *safety->private->enable_in;

	out = *safety->private->speed_in;

	if (*safety->private->position_in >= safety->private->position_max &&
	    out > 0.0) {
		out = 0.0;
105
		safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
106
		if (!safety->private->err_max) {
107
			safety->private->err_max = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
108
109
110
			log_send(LOG_T_ERROR, "Azimuth position above maximum %e > %e",
			    *safety->private->position_in,
			    safety->private->position_max);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
111
112
		}
	} else {
113
		safety->private->err_max = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
114
115
116
117
118
	}
	if (*safety->private->position_in >= safety->private->safe_zone_max &&
	    out > safety->private->safe_zone_max_speed) {
		out = safety->private->safe_zone_max_speed;
		if (!safety->private->warn_safe_max) {
119
			safety->private->warn_safe_max = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
120
			log_send(LOG_T_WARNING, "Azimuth position in safe zone");
Jeroen Vreeken's avatar
Jeroen Vreeken committed
121
122
		}
	} else {
123
		safety->private->warn_safe_max = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
124
125
126
127
128
	}

	if (*safety->private->position_in <= safety->private->position_min &&
	    out < 0.0) {
		out = 0.0;
129
		safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
130
		if (!safety->private->err_min) {
131
			safety->private->err_min = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
132
133
134
			log_send(LOG_T_ERROR, "Azimuth position under minimum %e < %e",
			    *safety->private->position_in,
			    safety->private->position_min);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
135
136
		}
	} else {
137
		safety->private->err_min = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
138
139
140
141
142
	}
	if (*safety->private->position_in <= safety->private->safe_zone_min &&
	    out < safety->private->safe_zone_min_speed) {
		out = safety->private->safe_zone_min_speed;
		if (!safety->private->warn_safe_min) {
143
			safety->private->warn_safe_min = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
144
			log_send(LOG_T_ERROR, "Azimuth position in safe zone");
Jeroen Vreeken's avatar
Jeroen Vreeken committed
145
146
		}
	} else {
147
		safety->private->warn_safe_min = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
148
149
	}
	
150
151
152
153
	if (*safety->private->safety_in_positive == 0) {
		safe = false;
		if (!safety->private->err_hwsafe_positive) {
			safety->private->err_hwsafe_positive = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
154
155
156
			log_send(LOG_T_ERROR, "Azimuth HW positive safety switch is open.");
		}
	} else {
157
		safety->private->err_hwsafe_positive = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
158
	}
159
160
161
162
	if (*safety->private->safety_in_negative == 0) {
		safe = false;
		if (!safety->private->err_hwsafe_negative) {
			safety->private->err_hwsafe_negative = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
163
164
165
			log_send(LOG_T_ERROR, "Azimuth HW negative safety switch is open.");
		}
	} else {
166
		safety->private->err_hwsafe_negative = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
167
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
168
169

	if (!safe) {
170
		enabled = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
171
172
	}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
173
	if (!enabled && !safety->private->recover) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
		out = 0.0;
		safety->private->torque_out = safety->private->emergency_torque;
	} else {
		safety->private->torque_out = *safety->private->torque_in;
	}

	safety->private->safe_out = safe;
	safety->private->speed_out = out;
}

static struct controller_block_param_list params[] = {
	{ "position_min",        false },
	{ "position_max",        false },
	{ "safe_zone_min",       false },
	{ "safe_zone_max",       false },
	{ "safe_zone_min_speed", false },
	{ "safe_zone_max_speed", false },
	{ "emergency_torque",    false },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
192
	{ "recover",             false },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
	{ NULL },
};

static void param_get(struct controller_block *safety, int param, void *val)
{
	switch (param) {
		case 0:
			*(float*)val = safety->private->position_min;
			break;
		case 1:
			*(float*)val = safety->private->position_max;
			break;
		case 2:
			*(float*)val = safety->private->safe_zone_min;
			break;
		case 3:
			*(float*)val = safety->private->safe_zone_max;
			break;
		case 4:
			*(float*)val = safety->private->safe_zone_min_speed;
			break;
		case 5:
			*(float*)val = safety->private->safe_zone_max_speed;
			break;
		case 6:
			*(float*)val = safety->private->emergency_torque;
			break;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
220
221
222
		case 7:
			*(int*)val = safety->private->recover;
			break;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
	}
}

static void param_set(struct controller_block *safety, int param, va_list val)
{
	switch (param) {
		case 0:
			safety->private->position_min = va_arg(val, double);
			break;
		case 1:
			safety->private->position_max = va_arg(val, double);
			break;
		case 2:
			safety->private->safe_zone_min = va_arg(val, double);
			break;
		case 3:
			safety->private->safe_zone_max = va_arg(val, double);
			break;
		case 4:
			safety->private->safe_zone_min_speed = va_arg(val, double);
			break;
		case 5:
			safety->private->safe_zone_max_speed = va_arg(val, double);
			break;
		case 6:
			safety->private->emergency_torque = va_arg(val, double);
			break;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
		case 7: {
			bool recover = va_arg(val, int);
		
			if (recover && !safety->private->recover) {
				log_send(LOG_T_WARNING,
				    "%s: Switching to recover mode, beware!",
				    safety->name);
			}
			if (!recover && safety->private->recover) {
				log_send(LOG_T_INFO,
				    "%s: Switching back to normal mode",
				    safety->name);
			}
		
			safety->private->recover = recover;
		
			break;
		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
268
269
270
	}
}

271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
static struct controller_block_interm_list interms[] = {
	{ "speed_in",           CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, speed_in)           },
	{ "position_in",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, position_in)        },
	{ "torque_in",          CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_in)          },
	{ "safety_in_positive", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private,  safety_in_positive) },
	{ "safety_in_negative", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private,  safety_in_negative) },
	{ "enable",             CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private,  enable_in)          },
	{ NULL }
};

static struct controller_block_outterm_list outterms[] = {
	{ "speed_out",  CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, speed_out)  },
	{ "safe_out",   CONTROLLER_BLOCK_TERM_BOOL,  offsetof(struct controller_block_private, safe_out)   },
	{ "torque_out", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_out) },
	{ NULL }
};

Jeroen Vreeken's avatar
Jeroen Vreeken committed
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
struct controller_block * block_dt_az_safety_create(char *name)
{
	struct controller_block *safety;
	
	safety = malloc(sizeof(struct controller_block));
	if (!safety)
		return NULL;

	safety->type = "dt_az_safety";
	safety->name = malloc(strlen(name)+1);
	if (!safety->name)
		goto err_safety;
	strcpy(safety->name, name);

	safety->private = malloc(sizeof(struct controller_block_private));
	if (!safety->private)
		goto err_name;
	safety->private->speed_out = 0.0;
	safety->private->safe_out = 0.0;
	safety->private->position_min = 0.0;
	safety->private->position_max = 0.0;
	safety->private->safe_zone_min = 0.0;
	safety->private->safe_zone_max = 0.0;
	safety->private->safe_zone_min_speed = 0.0;
	safety->private->safe_zone_max_speed = 0.0;
	safety->private->emergency_torque = 0.0;

315
316
317
318
319
320
	safety->private->err_max = false;
	safety->private->err_min = false;
	safety->private->warn_safe_max = false;
	safety->private->warn_safe_min = false;
	safety->private->err_hwsafe_positive = false;
	safety->private->err_hwsafe_negative = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
321
322
	
	safety->private->recover = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
323

324
	if (controller_block_interm_list_init(safety, interms))
Jeroen Vreeken's avatar
Jeroen Vreeken committed
325
		goto err_private;
326
327

	if (controller_block_outterm_list_init(safety, outterms))
Jeroen Vreeken's avatar
Jeroen Vreeken committed
328
		goto err_input;
329

Jeroen Vreeken's avatar
Jeroen Vreeken committed
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353

	safety->calculate = calculate;

	if (controller_block_param_list_init(safety, params))
		goto err_output;

	safety->param_get = param_get;
	safety->param_set = param_set;

	controller_block_add(safety);
	return safety;

err_output:
	free(safety->output);
err_input:
	free(safety->input);
err_private:
	free(safety->private);
err_name:
	free(safety->name);
err_safety:
	free(safety);
	return NULL;
}