dt_az_safety.c 9.49 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
/*
	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>

Jeroen Vreeken's avatar
Jeroen Vreeken committed
24
#include <controller/controller_block.h>
25
#include <controller/controller_command.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
26
#include <log/log.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
27
28
29
30
31
32
33
34
35
36
37
38
39

/*
       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
	/* keep log state to prevent flooding */
	bool warn_safe_max;
	bool warn_safe_min;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
86
87

	bool recover;
88
89

	struct controller_command *command;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
90
91
};

92
static void dt_az_safety_calculate(struct controller_block *safety)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
93
{
94
	struct controller_block_private *priv = safety->private;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
95
	float out;
96
	bool safe = true;
97
	bool enabled = *priv->enable_in;
98
	struct controller_command_entry c_entry;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
99

100
101
	if (!controller_command_queue_read(priv->command, &c_entry)) {
		bool recover = c_entry.value.b;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
102

103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
		if (recover && !priv->recover) {
			log_send(LOG_T_WARNING,
			    "%s: Switching to recover mode, beware!",
			    safety->name);
		}
		if (!recover && priv->recover) {
			log_send(LOG_T_INFO,
			    "%s: Switching back to normal mode",
			    safety->name);
		}
		
		priv->recover = recover;
	}

	out = *priv->speed_in;

	if (*priv->position_in >= priv->position_max &&
Jeroen Vreeken's avatar
Jeroen Vreeken committed
120
121
	    out > 0.0) {
		out = 0.0;
122
		safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
123
	}
124
125
126
127
128
	if (*priv->position_in >= priv->safe_zone_max &&
	    out > priv->safe_zone_max_speed) {
		out = priv->safe_zone_max_speed;
		if (!priv->warn_safe_max) {
			priv->warn_safe_max = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
129
			log_send(LOG_T_WARNING, "Azimuth position in safe zone");
Jeroen Vreeken's avatar
Jeroen Vreeken committed
130
131
		}
	} else {
132
		priv->warn_safe_max = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
133
134
	}

135
	if (*priv->position_in <= priv->position_min &&
Jeroen Vreeken's avatar
Jeroen Vreeken committed
136
137
	    out < 0.0) {
		out = 0.0;
138
		safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
139
	}
140
141
142
143
144
	if (*priv->position_in <= priv->safe_zone_min &&
	    out < priv->safe_zone_min_speed) {
		out = priv->safe_zone_min_speed;
		if (!priv->warn_safe_min) {
			priv->warn_safe_min = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
145
			log_send(LOG_T_ERROR, "Azimuth position in safe zone");
Jeroen Vreeken's avatar
Jeroen Vreeken committed
146
147
		}
	} else {
148
		priv->warn_safe_min = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
149
150
	}
	
151
	if (*priv->safety_in_positive == 0) {
152
		safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
153
	}
154
	if (*priv->safety_in_negative == 0) {
155
		safe = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
156
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
157
158

	if (!safe) {
159
		enabled = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
160
161
	}

162
	if (!enabled && !priv->recover) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
163
		out = 0.0;
164
		priv->torque_out = priv->emergency_torque;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
165
	} else {
166
		priv->torque_out = *priv->torque_in;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
167
168
	}

169
170
	priv->safe_out = safe || priv->recover;
	priv->speed_out = out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
171
172
}

173
174
static int param_set_position_min(struct controller_block *safety,
    char *param, int argc, va_list val)
175
176
177
178
{
	safety->private->position_min = va_arg(val, double);
	return 0;
}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
179

180
181
static int param_set_position_max(struct controller_block *safety,
    char *param, int argc, va_list val)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
182
{
183
184
	safety->private->position_max = va_arg(val, double);
	return 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
185
186
}

187
188
static int param_set_zone_min(struct controller_block *safety,
    char *param, int argc, va_list val)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
189
{
190
191
192
193
	safety->private->safe_zone_min = va_arg(val, double);
	return 0;
}

194
195
static int param_set_zone_max(struct controller_block *safety, char *param,
    int argc, va_list val)
196
197
198
199
200
{
	safety->private->safe_zone_max = va_arg(val, double);
	return 0;
}

201
202
static int param_set_zone_min_speed(struct controller_block *safety,
    char *param, int argc, va_list val)
203
204
205
206
207
{
	safety->private->safe_zone_min_speed = va_arg(val, double);
	return 0;
}

208
209
static int param_set_zone_max_speed(struct controller_block *safety,
    char *param, int argc, va_list val)
210
211
212
213
214
{
	safety->private->safe_zone_max_speed = va_arg(val, double);
	return 0;
}

215
216
static int param_set_emergency_torque(struct controller_block *safety,
    char *param, int argc, va_list val)
217
218
219
220
221
{
	safety->private->emergency_torque = va_arg(val, double);
	return 0;
}

222
223
static int param_set_recover(struct controller_block *safety, char *param,
    int argc, va_list val)
224
225
{
	bool recover = va_arg(val, int);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
226
		
227
228
229
230
	if (recover && !safety->private->recover) {
		log_send(LOG_T_WARNING,
		    "%s: Switching to recover mode, beware!",
		    safety->name);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
231
	}
232
233
234
235
236
237
238
239
240
	if (!recover && safety->private->recover) {
		log_send(LOG_T_INFO,
		    "%s: Switching back to normal mode",
		    safety->name);
	}

	safety->private->recover = recover;

	return 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
241
242
}

243
static struct controller_block_param_list params[] = {
244
245
246
247
248
249
250
251
	{ "position_min",        false, param_set_position_min, .args = { "double", NULL } },
	{ "position_max",        false, param_set_position_max, .args = { "double", NULL } },
	{ "safe_zone_min",       false, param_set_zone_min, .args = { "double", NULL } },
	{ "safe_zone_max",       false, param_set_zone_max, .args = { "double", NULL } },
	{ "safe_zone_min_speed", false, param_set_zone_min_speed, .args = { "double", NULL } },
	{ "safe_zone_max_speed", false, param_set_zone_max_speed, .args = { "double", NULL } },
	{ "emergency_torque",    false, param_set_emergency_torque, .args = { "double", NULL } },
	{ "recover",             false, param_set_recover, .args = { "int", NULL } },
252
253
254
255
	{ NULL },
};


256
257
258
259
260
261
262
263
264
265
266
267
268
269
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) },
270
	{ "recover",    CONTROLLER_BLOCK_TERM_BOOL,  offsetof(struct controller_block_private, recover)    },
271
272
273
	{ NULL }
};

274
static struct controller_block * block_dt_az_safety_create(char *name, int argc, va_list val)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
275
276
277
{
	struct controller_block *safety;
	
278
279
	safety = controller_block_alloc("dt_az_safety", name,
	    sizeof(struct controller_block_private));
Jeroen Vreeken's avatar
Jeroen Vreeken committed
280
281
282
283
284
285
286
287
288
289
290
291
292
	if (!safety)
		return NULL;

	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;

293
294
	safety->private->warn_safe_max = false;
	safety->private->warn_safe_min = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
295
296
	
	safety->private->recover = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
297

298
	if (controller_block_interm_list_init(safety, interms))
Jeroen Vreeken's avatar
Jeroen Vreeken committed
299
		goto err_private;
300
301

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

Jeroen Vreeken's avatar
Jeroen Vreeken committed
304

305
	safety->calculate = dt_az_safety_calculate;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
306

307
	if (controller_block_param_list_add(safety, params))
Jeroen Vreeken's avatar
Jeroen Vreeken committed
308
309
		goto err_output;

310
311
	if (controller_block_add(safety) != 0)
		goto err_add;
312
313
314
315
316
317

	safety->private->command = controller_command_create(
	    safety, name, "Recover");
	safety->private->command->value_type = COMMAND_VALUE_TYPE_BOOL;
	safety->private->command->command_types[0] = COMMAND_PTYPE_SETPOINT;

Jeroen Vreeken's avatar
Jeroen Vreeken committed
318
319
	return safety;

320
err_add:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
321
322
323
324
325
326
327
328
329
330
err_output:
	free(safety->output);
err_input:
	free(safety->input);
err_private:
	free(safety->private);
	free(safety->name);
	free(safety);
	return NULL;
}
331
332
333
334
335

BLOCK_CREATE(dt_az_safety) = {
	.create = block_dt_az_safety_create,
	.args = { NULL },
};