dt_el_safety.c 16.3 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), 2007, 2008, 2013
	Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2008, 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
28
29
30
31
32
33
34

	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 <math.h>

#include "controller_block.h"
#include "dt_el_safety.h"
#include "log.h"

/*
       inputs          outputs
       nr name         nr name

     ----------------------------------
     |                                |
Jeroen Vreeken's avatar
Jeroen Vreeken committed
35
  ---| 0  speed_in_r    0 speed_out_r |----
Jeroen Vreeken's avatar
Jeroen Vreeken committed
36
     |	                              |
Jeroen Vreeken's avatar
Jeroen Vreeken committed
37
  ---| 1  speed_in_l    1 speed_out_l |----
Jeroen Vreeken's avatar
Jeroen Vreeken committed
38
     |	                              |
Jeroen Vreeken's avatar
Jeroen Vreeken committed
39
  ---| 2  position_in_r 2  safe_out   |----
Jeroen Vreeken's avatar
Jeroen Vreeken committed
40
     |	                              |
Jeroen Vreeken's avatar
Jeroen Vreeken committed
41
  ---| 3  position_in_l               |
Jeroen Vreeken's avatar
Jeroen Vreeken committed
42
43
44
45
46
     |	                              |
  ---| 4  torsion_in                  |
     |	                              |
  ---| 5  torque_in                   |
     |	                              |
Jeroen Vreeken's avatar
Jeroen Vreeken committed
47
  ---| 6  torque_in_r  3 torque_out_r |----
Jeroen Vreeken's avatar
Jeroen Vreeken committed
48
     |	                              |
Jeroen Vreeken's avatar
Jeroen Vreeken committed
49
  ---| 7  torque_in_l  4 torque_out_l |----
Jeroen Vreeken's avatar
Jeroen Vreeken committed
50
     |	                              |
51
  ---| 8  safety_in_top               |
Jeroen Vreeken's avatar
Jeroen Vreeken committed
52
     |                                |
53
  ---| 9  safety_in_bottom            |
Jeroen Vreeken's avatar
Jeroen Vreeken committed
54
55
     |                                |
  ---| 10 enable                      |
Jeroen Vreeken's avatar
Jeroen Vreeken committed
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
     |                                |
     ----------------------------------

	This is not a generic block.
	It implements the safety checks specific for the dt elevation
	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 {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
75
76
77
78
79
	float *input_r;
	float *input_l;
	float *position_in_r;
	float *position_in_l;
	float *torsion_in;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
80
	float *torque_in;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
81
82
	float *torque_in_r;
	float *torque_in_l;
83
84
	bool *safety_in_top;
	bool *safety_in_bottom;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
85
86
	float speed_out_r;
	float speed_out_l;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
87
	bool safe_out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
88
89
	float torque_out_r;
	float torque_out_l;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
	bool *enable_in;

	float position_min;
	float position_max;
	
	float safe_zone_min;
	float safe_zone_max;
	float safe_zone_min_speed;
	float safe_zone_max_speed;
	
	float torsion_max;
	float torsion_recover_max;
	float torque_max;
	float emergency_torque;

	int recover;

107
108
109
110
111
112
113
114
115
116
117
118
119
120
	/* flags to prevent log flooding */
	bool err_max_r;
	bool err_min_r;
	bool err_max_l;
	bool err_min_l;
	bool err_torque_max;
	bool err_torsion;
	bool err_torsion_recover;
	bool warn_safe_max_l;
	bool warn_safe_min_l;
	bool warn_safe_max_r;
	bool warn_safe_min_r;
	bool err_hwsafe_top;
	bool err_hwsafe_bottom;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
121
122
123
124
125
};

static void calculate(struct controller_block *safety)
{
	float out0, out1;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
126
127
	float position_in_r, position_in_l;
	float torsion_in, torque_in, torque_in_r, torque_in_l;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
128
129
130
	bool safe;
	bool enabled = *safety->private->enable_in;

131
	safe = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
132
133
134
135
136
137
138
139
140
	out0 = *safety->private->input_r;
	out1 = -*safety->private->input_l;
	position_in_r = *safety->private->position_in_r;
	position_in_l = -*safety->private->position_in_l;
	torsion_in = *safety->private->torsion_in;
	torque_in = *safety->private->torque_in;
	torque_in_r = *safety->private->torque_in_r;
	torque_in_l = *safety->private->torque_in_l;

141
	/* outside maximum range and not moving back */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
142
	if (position_in_r >= safety->private->position_max &&
Jeroen Vreeken's avatar
Jeroen Vreeken committed
143
144
	    out0 > 0.0) {
		out0 = 0.0;
145
		safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
146
		if (!safety->private->err_max_r) {
147
			safety->private->err_max_r = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
148
149
150
			log_send(LOG_T_ERROR,
			    "Elevation position right %e above maximum %e",
			    position_in_r,
Jeroen Vreeken's avatar
Jeroen Vreeken committed
151
152
153
			    safety->private->position_max);
		}
	} else {
154
		safety->private->err_max_r = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
155
	}
156
157

	/* inside safe zone and not moving at a safe speed */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
158
	if (position_in_r >= safety->private->safe_zone_max &&
Jeroen Vreeken's avatar
Jeroen Vreeken committed
159
160
161
	    out0 > safety->private->safe_zone_max_speed) {
		out0 = safety->private->safe_zone_max_speed;
		if (!safety->private->warn_safe_max_r) {
162
			safety->private->warn_safe_max_r = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
163
164
165
			log_send(LOG_T_WARNING,
			    "Elevation position right %e in safe zone %e",
			    position_in_r,
Jeroen Vreeken's avatar
Jeroen Vreeken committed
166
167
168
			    safety->private->safe_zone_max);
		}
	} else {
169
		safety->private->warn_safe_max_r = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
170
171
	}

172
	/* outside minimum and not moving back */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
173
	if (position_in_r <= safety->private->position_min &&
Jeroen Vreeken's avatar
Jeroen Vreeken committed
174
175
	    out0 < 0.0) {
		out0 = 0.0;
176
		safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
177
		if (!safety->private->err_min_r) {
178
			safety->private->err_min_r = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
179
180
181
			log_send(LOG_T_ERROR, 
			"Elevation position right under minimum, %e < %e",
			position_in_r, safety->private->position_min);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
182
183
		}
	} else {
184
		safety->private->err_min_r = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
185
	}
186
187
	
	/* inside safe zone and not moving back */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
188
	if (position_in_r <= safety->private->safe_zone_min &&
Jeroen Vreeken's avatar
Jeroen Vreeken committed
189
190
191
	    out0 < safety->private->safe_zone_min_speed) {
		out0 = safety->private->safe_zone_min_speed;
		if (!safety->private->warn_safe_min_r) {
192
			safety->private->warn_safe_min_r = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
193
194
			log_send(LOG_T_WARNING, 
			    "Elevation position right in safe zone");
Jeroen Vreeken's avatar
Jeroen Vreeken committed
195
196
		}
	} else {
197
		safety->private->warn_safe_min_r = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
198
199
200
	}


201
	/* outside maximum range and not moving back */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
202
	if (position_in_l >= safety->private->position_max &&
Jeroen Vreeken's avatar
Jeroen Vreeken committed
203
204
	    out1 > 0.0) {
		out1 = 0.0;
205
		safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
206
		if (!safety->private->err_max_l) {
207
			safety->private->err_max_l = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
208
209
210
			log_send(LOG_T_ERROR, 
			    "Elevation left position %e above maximum %e",
			    position_in_l,
Jeroen Vreeken's avatar
Jeroen Vreeken committed
211
212
213
			    safety->private->position_max);
		}
	} else {
214
		safety->private->err_max_l = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
215
	}
216
217

	/* inside safe zone and not moving at a safe speed */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
218
	if (position_in_l >= safety->private->safe_zone_max &&
Jeroen Vreeken's avatar
Jeroen Vreeken committed
219
220
221
	    out1 > safety->private->safe_zone_max_speed) {
		out1 = safety->private->safe_zone_max_speed;
		if (!safety->private->warn_safe_max_l) {
222
			safety->private->warn_safe_max_l = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
223
224
225
			log_send(LOG_T_WARNING,
			    "Elevation left position %e in safe zone %e",
			    position_in_l,
Jeroen Vreeken's avatar
Jeroen Vreeken committed
226
227
228
			    safety->private->safe_zone_max);
		}
	} else {
229
		safety->private->warn_safe_max_l = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
230
231
	}

232
	/* outside minimum and not moving back */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
233
	if (position_in_l <= safety->private->position_min &&
Jeroen Vreeken's avatar
Jeroen Vreeken committed
234
235
	    out1 < 0.0) {
		out1 = 0.0;
236
		safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
237
		if (!safety->private->err_min_l) {
238
			safety->private->err_min_l = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
239
240
241
			log_send(LOG_T_ERROR, 
			    "Elevation left position %e under minimum %e",
			    position_in_l, safety->private->position_min);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
242
243
		}
	} else {
244
		safety->private->err_min_l = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
245
	}
246
247

	/* inside safe zone and not moving back */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
248
	if (position_in_l <= safety->private->safe_zone_min &&
Jeroen Vreeken's avatar
Jeroen Vreeken committed
249
250
251
	    out1 < safety->private->safe_zone_min_speed) {
		out1 = safety->private->safe_zone_min_speed;
		if (!safety->private->warn_safe_min_l) {
252
			safety->private->warn_safe_min_l = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
253
254
			log_send(LOG_T_WARNING, 
			    "Elevation left position in safe zone");
Jeroen Vreeken's avatar
Jeroen Vreeken committed
255
256
		}
	} else {
257
		safety->private->warn_safe_min_l = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
258
259
	}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
260
	if (fabsf(torsion_in) >= safety->private->torsion_recover_max) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
261
262
		out0 = 0.0;
		out1 = 0.0;
263
		safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
264
		if (!safety->private->err_torsion_recover) {
265
			safety->private->err_torsion_recover = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
266
			log_send(LOG_T_ERROR,
Jeroen Vreeken's avatar
Jeroen Vreeken committed
267
268
			    "Elevation torsion to big (%e) to recover, check calibration",
			    torsion_in);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
269
270
		}
	} else {
271
		safety->private->err_torsion_recover = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
272
273
	}

274
	/* torsion to big */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
275
	if (fabsf(torsion_in) >= safety->private->torsion_max) {
276
		/* moving back? */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
277
		if (position_in_r > position_in_l) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
278
279
			if (out0 > 0.0) {
				out0 = 0.0;
280
				safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
281
282
283
			}
			if (out1 < 0.0) {
				out1 = 0.0;
284
				safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
285
286
			}
		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
287
		if (position_in_r < position_in_l) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
288
289
			if (out0 < 0.0) {
				out0 = 0.0;
290
				safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
291
292
293
			}
			if (out1 > 0.0) {
				out1 = 0.0;
294
				safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
295
296
297
			}
		}
		if (!safety->private->err_torsion) {
298
			safety->private->err_torsion = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
299
300
			log_send(LOG_T_ERROR, "Elevation torsion (%e) to big",
			    torsion_in);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
301
302
		}
	} else {
303
		safety->private->err_torsion = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
304
305
	}

306
307
308
309
310
311
312
313
314
315
	/* (filtered) total torque to big (elevation stuck?) */
	if (fabs(torque_in) > safety->private->torque_max) {
		if (!safety->private->err_torque_max) {
			safety->private->err_torque_max = true;
			log_send(LOG_T_ERROR, "Elevation torque to big. %e > %e",
			    torque_in, safety->private->torque_max);
		}
		safe = false;
	} else {
		safety->private->err_torque_max = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
316
317
	}

318
319
320
321
322
	/* top safety switch */
	if (!*safety->private->safety_in_top) {
		safe = false;
		if (!safety->private->err_hwsafe_top) {
			safety->private->err_hwsafe_top = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
323
324
			log_send(LOG_T_ERROR, 
			    "Elevation HW top safety switch is open.");
Jeroen Vreeken's avatar
Jeroen Vreeken committed
325
326
		}
	} else {
327
		safety->private->err_hwsafe_top = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
328
	}
329
330
331
332
333
334
	
	/* bottom safety switch */
	if (!*safety->private->safety_in_bottom) {
		safe = false;
		if (!safety->private->err_hwsafe_bottom) {
			safety->private->err_hwsafe_bottom = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
335
336
337
338
			log_send(LOG_T_ERROR, 
			    "Elevation HW bottom safety switch is open.");
		}
	} else {
339
		safety->private->err_hwsafe_bottom = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
340
	}
341
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
342
	if (!safe)
343
		enabled = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
344
345
346
347

	if (!enabled && !safety->private->recover) {
		out0 = 0.0;
		out1 = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
348
		safety->private->torque_out_r =
Jeroen Vreeken's avatar
Jeroen Vreeken committed
349
		    safety->private->emergency_torque;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
350
		safety->private->torque_out_l =
Jeroen Vreeken's avatar
Jeroen Vreeken committed
351
352
		    safety->private->emergency_torque;
	} else {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
353
354
		safety->private->torque_out_r = torque_in_r;
		safety->private->torque_out_l = torque_in_l;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
355
356
	}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
357
358
	safety->private->speed_out_r = out0;
	safety->private->speed_out_l = -out1;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
	safety->private->safe_out = safe;
}


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 },
	{ "torsion_max",         false },
	{ "torsion_recover_max", false },
	{ "recover",             false },
	{ "torque_max",          false },
	{ "emergency_torque",    false },
	{ 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->torsion_max;
			break;
		case 7:
			*(float*)val = safety->private->torsion_recover_max;
			break;
		case 8:
			*(int*)val = safety->private->recover;
			break;
		case 9:
			*(float*)val = safety->private->torque_max;
			break;
		case 10:
			*(float*)val = safety->private->emergency_torque;
			break;
	}
}

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->torsion_max = va_arg(val, double);
			break;
		case 7:
			safety->private->torsion_recover_max = va_arg(val, double);
			break;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
		case 8: {
			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;
		
Jeroen Vreeken's avatar
Jeroen Vreeken committed
460
			break;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
461
		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
462
463
464
465
466
467
468
469
470
		case 9:
			safety->private->torque_max = va_arg(val, double);
			break;
		case 10:
			safety->private->emergency_torque = va_arg(val, double);
			break;
	}
}

471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
static struct controller_block_outterm_list outterms[] = {
	{ "speed_out_r",  CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, speed_out_r)   },
	{ "speed_out_l",  CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, speed_out_l)   },
	{ "safe_out",     CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, safe_out)       },
	{ "torque_out_r", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_out_r) },
	{ "torque_out_l", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_out_l) },
	{ NULL }
};

static struct controller_block_interm_list interms[] = {
	{ "speed_in_r",       CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, input_r)         },
	{ "speed_in_l",       CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, input_l)         },
	{ "position_in_r",    CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, position_in_r)   },
	{ "position_in_l",    CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, position_in_l)   },
	{ "torsion_in",       CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torsion_in)      },
	{ "torque_in",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_in)       },
	{ "torque_in_r",      CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_in_r)     },
	{ "torque_in_l",      CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_in_l)     },
	{ "safety_in_top",    CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, safety_in_top)    },
	{ "safety_in_bottom", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, safety_in_bottom) },
	{ "enable",           CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, enable_in)        },
	{ NULL }
};

Jeroen Vreeken's avatar
Jeroen Vreeken committed
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
struct controller_block * block_dt_el_safety_create(char *name)
{
	struct controller_block *safety;
	
	safety = malloc(sizeof(struct controller_block));
	if (!safety)
		return NULL;

	safety->type = "dt_el_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;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
512
513
	safety->private->speed_out_r = 0.0;
	safety->private->speed_out_l = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
514
515
516
517
518
519
520
521
522
523
524
	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->recover = 0;
	safety->private->torsion_max = 0.0;
	safety->private->torsion_recover_max = 0.0;
	safety->private->torque_max = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
525
526
	safety->private->torque_out_r = 0.0;
	safety->private->torque_out_l = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
527
528
	safety->private->emergency_torque = 0.0;

529
530
531
532
533
534
535
536
537
538
539
540
	safety->private->err_max_r = false;
	safety->private->err_min_r = false;
	safety->private->err_max_l = false;
	safety->private->err_min_l = false;
	safety->private->err_torsion = false;
	safety->private->warn_safe_max_r = false;
	safety->private->warn_safe_min_r = false;
	safety->private->warn_safe_max_l = false;
	safety->private->warn_safe_min_l = false;
	safety->private->err_hwsafe_top = false;
	safety->private->err_hwsafe_bottom = false;
	safety->private->err_torsion_recover = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
541

542
	if (controller_block_interm_list_init(safety, interms))
Jeroen Vreeken's avatar
Jeroen Vreeken committed
543
		goto err_private;
544
545

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

Jeroen Vreeken's avatar
Jeroen Vreeken committed
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571

	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;
}