dt_el_stoeber_l_sim.c 4.65 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
/*
	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/>.

 */


#include <string.h>
#include <stdio.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
23
#include <malloc.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
24
25

#include "controller_block.h"
26
#include "log.h"
Jeroen Vreeken's avatar
Jeroen Vreeken committed
27

28
29
30
31
32
struct controller_block_private {
	bool *ba1;
	bool *ba2;
};

Jeroen Vreeken's avatar
Jeroen Vreeken committed
33
34
35
36
37
38
39
40
41
42
43
struct ghost_term {
	char *name;
	char *realblock;
	char *realname;
};

static struct ghost_term ghost_outputs[] = {
	{ "position", 	"dt_el_pos_quantize_l", "out"    },
	{ "speed",    	"dt_el_accelerate_l",   "out"    },
	{ "torque",   	"dt_el_torque_limit_l", "out"    },
	{ "enabled", 	"dt_el_enabled",      	"output" },
44
	{ "be1",	"dt_el_safety_b_not",	"output" },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
45
	{ "be2",	"dt_el_safety_b",	"value"  },
46
	{ "be3",	"dt_el_safety_t_not",	"output" },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
47
	{ "be4",	"dt_el_safety_t",	"value"  },
48
	{ "be5",	"dt_el_bex",		"value"  },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
49
50
51
52
53
54
55
56
57
58
59
60
61
	{ "ae1", 	"dt_el_ae1",		"value"  },
};

static struct ghost_term ghost_inputs[] = {
	{ "speed",	"dt_el_speed_limit_l",  "in" },
	{ "torque",	"dt_el_torque_gain_l", 	"in" },
	{ "enable",	"dt_el_enabled",      	"input" },
	{ NULL },
};

struct controller_block *block_dt_el_stoeber_l_sim_create(char *name, va_list ap)
{
	struct controller_block *stoeber;
62
	struct controller_block_private *private;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
63
64
65
66
67
68
69
70
71
72
73
74
	int i;
	
	stoeber = malloc(sizeof(struct controller_block));
	if (!stoeber)
		goto err_malloc;
	
	stoeber->type = "dt_el_stoeber_l_sim";

	stoeber->name = strdup(name);
	if (!stoeber->name)
		goto err_name;

75
76
77
78
79
	private = calloc(1, sizeof(struct controller_block_private));
	if (!private)
		goto err_private;
	stoeber->private = private;

Jeroen Vreeken's avatar
Jeroen Vreeken committed
80
81
82
83
84
85
86
87
88
89
90
91
92

	for (i = 0; ghost_outputs[i].name; i++);
	stoeber->outputs = i;
	stoeber->output = malloc(sizeof(struct controller_block_outterm)*i);
	if (!stoeber->output)
		goto err_output;
	for (i = 0; ghost_outputs[i].name; i++) {
		struct controller_block *subblock;
		int j;
		
		stoeber->output[i].name = ghost_outputs[i].name;
		subblock = controller_block_find(ghost_outputs[i].realblock);
		if (!subblock) {
93
			log_send(LOG_T_ERROR, "subblock %s not found", ghost_outputs[i].realblock);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
94
95
96
97
98
99
100
101
102
103
104
105
106
107
			goto err_ghost_output;
		}
		for (j = 0; j < subblock->outputs; j++) {
			if (!strcmp(subblock->output[j].name,
			    ghost_outputs[i].realname)) {
				stoeber->output[i].value =
				    subblock->output[j].value;
				stoeber->output[i].type =
				    subblock->output[j].type;
				stoeber->output[i].source = subblock->output[j].source;
				stoeber->output[i].sourceterm = &subblock->output[j];
				break;
			}
		}
108
109
110
111
112
		if (j == subblock->outputs) {
			log_send(LOG_T_ERROR, "Could not find subblock output %s",
			    ghost_outputs[i].realname);
			goto err_ghost_output;
		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
113
114
115
	}

	for (i = 0; ghost_inputs[i].name; i++);
116
117
	stoeber->inputs = i + 2;
	stoeber->input = malloc(sizeof(struct controller_block_interm)*stoeber->inputs);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
	if (!stoeber->input)
		goto err_input;
	for (i = 0; ghost_inputs[i].name; i++) {
		struct controller_block *subblock;
		int j;
		
		stoeber->input[i].name = ghost_inputs[i].name;
		subblock = controller_block_find(ghost_inputs[i].realblock);
		for (j = 0; j < subblock->inputs; j++) {
			if (!strcmp(subblock->input[j].name,
			    ghost_inputs[i].realname)) {
				stoeber->input[i].value =
				    subblock->input[j].value;
				stoeber->input[i].type =
				    subblock->input[j].type;
				stoeber->input[i].ghostof =
				    &subblock->input[j];
				break;
			}
		}
138
139
140
141
142
		if (j == subblock->inputs) {
			log_send(LOG_T_ERROR, "Could not find subblock input %s",
			    ghost_inputs[i].realname);
			goto err_ghost_output;
		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
143
	}
144
145
146
147
148
149
150
151
152
	stoeber->input[i].name = "ba1";
	stoeber->input[i].value.b = &stoeber->private->ba1;
	stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
	stoeber->input[i].ghostof = NULL;
	i++;
	stoeber->input[i].name = "ba2";
	stoeber->input[i].value.b = &stoeber->private->ba2;
	stoeber->input[i].type = CONTROLLER_BLOCK_TERM_BOOL;
	stoeber->input[i].ghostof = NULL;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
153
154
155
156
157
158
159
160
161
162
163
164
165

	stoeber->params = 0;

	stoeber->calculate = NULL;

	controller_block_add(stoeber);

	return stoeber;

err_input:
err_ghost_output:
	free(stoeber->output);
err_output:
166
167
	free(stoeber->private);
err_private:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
168
169
170
171
172
173
	free(stoeber->name);
err_name:
	free(stoeber);
err_malloc:
	return NULL;
}