block_servo_state.c 9.31 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), 2011
	Copyright Stichting C.A. Muller Radioastronomiestation, 2011

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

Jeroen Vreeken's avatar
Jeroen Vreeken committed
26
27
28
29
30
31
32
33
34
#include "controller_block.h"
#include "controller_sample.h"

/*
       inputs      outputs
       nr name     nr name

     ---------------------------
     |                         |
35
  ---| 0  spg_x    0  out_x    |----
Jeroen Vreeken's avatar
Jeroen Vreeken committed
36
     |	                       |
37
  ---| 1  spg_v    1  out_v    |----
Jeroen Vreeken's avatar
Jeroen Vreeken committed
38
     |	                       |
39
  ---| 2  spg_a    2  out_a    |----
Jeroen Vreeken's avatar
Jeroen Vreeken committed
40
41
42
43
44
45
46
47
48
49
     |	                       |
  ---| 3  safe     3  spg_rst  |----
     |	                       |
     |             4  spg_en   |----
     |	                       |
     ---------------------------
     
 */

struct controller_block_private {
50
51
52
	float *spg_x;
	float *spg_v;
	float *spg_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
53
	bool *safe;
54
55
56
	float out_x;
	float out_v;
	float out_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
57
58
	bool reset;
	bool enable;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
59
	bool emergency;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
60

61
62
63
64
	float min_x;
	float max_x;
	float max_v;
	float max_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
65
66
67
68
69
70
	enum {
		SERVO_STATE_DISABLED,
		SERVO_STATE_ENABLED,
		SERVO_STATE_DISABLING
	} state;
	bool enable_param;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
71
	bool override;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
72
73
74
75
76
77
};

static void calculate(struct controller_block *servo_state)
{
	bool safe = *servo_state->private->safe;
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
78
79
80
81
82
83
	if (servo_state->private->emergency) {
		servo_state->private->emergency = false;
		safe = false;
		log_send(LOG_T_ERROR, "%s: External emergency triggered",
		    servo_state->name);
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
84
85
86
87
	if (!safe) {
		servo_state->private->enable_param = false;
	}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
88
	if (servo_state->private->override) {
89
90
91
		servo_state->private->out_x = *servo_state->private->spg_x;
		servo_state->private->out_v = *servo_state->private->spg_v;
		servo_state->private->out_a = *servo_state->private->spg_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
92
93
94
95
96
		servo_state->private->enable = true;
		servo_state->private->reset = false;
		return;
	}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
97
98
	switch (servo_state->private->state) {
		case SERVO_STATE_ENABLED:
99
100
101
102
103
104
			servo_state->private->out_x =
			     *servo_state->private->spg_x;
			servo_state->private->out_v =
			     *servo_state->private->spg_v;
			servo_state->private->out_a =
			     *servo_state->private->spg_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
105
106
107
108
109
110
			     
			if (!safe) {
				servo_state->private->enable = false;
				servo_state->private->reset = true;
				servo_state->private->state =
				    SERVO_STATE_DISABLED;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
111
112
113
				log_send(LOG_T_ERROR, 
				    "%s: Unsafe, going to state DISABLED",
				    servo_state->name);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
114
115
116
117
118
119
120
121
122
123
124
				break;
			}
			
			servo_state->private->enable = true;
			servo_state->private->reset = false;
			
			if (servo_state->private->enable_param) {
				break;
			} else {
				servo_state->private->state =
				    SERVO_STATE_DISABLING;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
125
126
127
				log_send(LOG_T_INFO, 
				    "%s: Going to state DISABLING",
				    servo_state->name);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
128
129
130
			}
			/* Fallthrough if disabling */
		case SERVO_STATE_DISABLING: {
131
132
133
			float out_x = servo_state->private->out_x;
			float out_v = servo_state->private->out_v;
			float out_a = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
134
135
136
137
138
139
140
141
			float t = controller_sample_period();

			
			if (!safe) {
				servo_state->private->enable = false;
				servo_state->private->reset = true;
				servo_state->private->state =
				    SERVO_STATE_DISABLED;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
142
143
144
				log_send(LOG_T_ERROR, 
				    "%s: Unsafe, going to state DISABLED",
				    servo_state->name);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
145
146
147
				break;
			}

148
149
150
151
152
153
			if (out_v > 0.0) {
				out_a = -servo_state->private->max_a;
				out_v += out_a * t;
				if (out_v < 0.0) {
					out_a += out_v;
					out_v = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
154
				}
155
156
157
158
159
160
			} else if (out_v < 0.0) {
				out_a = servo_state->private->max_a;
				out_v += out_a * t;
				if (out_v > 0.0) {
					out_a -= out_v;
					out_v = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
161
162
163
				}
			}
			
164
			if (out_v == 0.0) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
165
166
				servo_state->private->state =
				    SERVO_STATE_DISABLED;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
167
168
169
				log_send(LOG_T_INFO, 
				    "%s: Going to state DISABLED",
				    servo_state->name);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
170
171
			}
			
172
			out_x += out_v * t;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
173
			
174
175
176
177
			if (out_x > servo_state->private->max_x)
				out_x = servo_state->private->max_x;
			if (out_x < servo_state->private->min_x)
				out_x = servo_state->private->min_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
178
			
179
180
181
			servo_state->private->out_x = out_x;
			servo_state->private->out_v = out_v;
			servo_state->private->out_a = out_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
182
183
184
185
186
187
188
			
			servo_state->private->enable = true;
			servo_state->private->reset = true;
			
			break;
		}
		case SERVO_STATE_DISABLED:
189
190
191
192
			servo_state->private->out_x =
			     *servo_state->private->spg_x;
			servo_state->private->out_v = 0.0;
			servo_state->private->out_a = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
193
194
195
196
			     
			servo_state->private->enable = false;
			servo_state->private->reset = true;
			
Jeroen Vreeken's avatar
Jeroen Vreeken committed
197
			if (servo_state->private->enable_param) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
198
199
				servo_state->private->state =
				    SERVO_STATE_ENABLED;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
200
201
202
203
				log_send(LOG_T_INFO, 
				    "%s: Going to state ENABLED",
				    servo_state->name);
			}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
204
205
206
207
208
			break;
	}
}

static struct controller_block_param_list params[] = {
209
210
211
212
213
	{ "min_x",    true },
	{ "max_x",    true },
	{ "max_v",    true },
	{ "max_a",    true },
	{ "enabled",  true },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
214
215
	{ "emergency", true },
	{ "override", true },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
216
217
218
	{ NULL },
};

219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
static struct controller_block_interm_list interms[] = {
	{ "spg_x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, spg_x) },
	{ "spg_v", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, spg_v) },
	{ "spg_a", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, spg_a) },
	{ "safe",  CONTROLLER_BLOCK_TERM_BOOL,  offsetof(struct controller_block_private, safe)  },
	{ NULL },
};

static struct controller_block_outterm_list outterms[] = {
	{ "out_x",  CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, out_x)  },
	{ "out_v",  CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, out_v)  },
	{ "out_a",  CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, out_a)  },
	{ "reset",  CONTROLLER_BLOCK_TERM_BOOL,  offsetof(struct controller_block_private, reset)  },
	{ "enable", CONTROLLER_BLOCK_TERM_BOOL,  offsetof(struct controller_block_private, enable) },
	{ NULL },
};

Jeroen Vreeken's avatar
Jeroen Vreeken committed
236
237
238
239
static void param_get(struct controller_block *servo_state, int param, void *val)
{
	switch (param) {
		case 0:
240
			*(float*)val = servo_state->private->min_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
241
242
			break;
		case 1:
243
			*(float*)val = servo_state->private->max_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
244
245
246

			break;
		case 2:
247
			*(float*)val = servo_state->private->max_v;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
248
249
			break;
		case 3:
250
			*(float*)val = servo_state->private->max_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
251
252
253
254
			break;
		case 4:
			*(bool*)val = servo_state->private->enable_param;
			break;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
255
256
257
258
259
260
		case 5:
			*(bool*)val = servo_state->private->emergency;
			break;
		case 6:
			*(bool*)val = servo_state->private->override;
			break;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
261
262
263
264
265
266
267
	}
}

static void param_set(struct controller_block *servo_state, int param, va_list val)
{
	switch (param) {
		case 0:
268
			servo_state->private->min_x = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
269
270
			break;
		case 1:
271
			servo_state->private->max_x = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
272
273
			break;
		case 2:
274
			servo_state->private->max_v = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
275
276
			break;
		case 3:
277
			servo_state->private->max_a = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
278
279
280
281
			break;
		case 4:
			servo_state->private->enable_param = va_arg(val, int);
			break;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
		case 5:
			servo_state->private->emergency = va_arg(val, int);
			break;
		case 6: {
			bool override = va_arg(val, int);
			if (override && !servo_state->private->override) {
				log_send(LOG_T_WARNING,
				    "%s: Switching to override mode, beware!",
				    servo_state->name);
			}
			if (!override && servo_state->private->override) {
				log_send(LOG_T_INFO,
				    "%s: Switching back to normal mode",
				    servo_state->name);
			}
			servo_state->private->override = override;
		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
	}
}

struct controller_block * block_servo_state_create(char *name)
{
	struct controller_block *servo_state;
	
	servo_state = malloc(sizeof(struct controller_block));
	if (!servo_state)
		return NULL;

	servo_state->type = "servo_state";
	servo_state->name = malloc(strlen(name)+1);
	if (!servo_state->name)
		goto err_servo_state;
	strcpy(servo_state->name, name);

	servo_state->private = malloc(sizeof(struct controller_block_private));
	if (!servo_state->private)
		goto err_name;
319
320
321
	servo_state->private->out_x = 0.0;
	servo_state->private->out_v = 0.0;
	servo_state->private->out_a = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
322
323
324
	servo_state->private->reset = true;
	servo_state->private->enable = false;
	servo_state->private->enable_param = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
325
	servo_state->private->emergency = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
326
	servo_state->private->state = SERVO_STATE_DISABLED;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
327
	servo_state->private->override = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
328

329
	if (controller_block_interm_list_init(servo_state, interms))
Jeroen Vreeken's avatar
Jeroen Vreeken committed
330
		goto err_private;
331
332

	if (controller_block_outterm_list_init(servo_state, outterms))
Jeroen Vreeken's avatar
Jeroen Vreeken committed
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
		goto err_input;

	servo_state->calculate = calculate;

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

	servo_state->param_get = param_get;
	servo_state->param_set = param_set;

	controller_block_add(servo_state);
	return servo_state;

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