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

#include "controller_block.h"
#include "esc.h"
#include "ec_stoeber.h"
27
#include "log.h"
Jeroen Vreeken's avatar
Jeroen Vreeken committed
28

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

Jeroen Vreeken's avatar
Jeroen Vreeken committed
34
35
36
37
38
39
40
41
42
43
44
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" },
45
	{ "be1",	"dt_el_safety_b_not",	"output" },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
46
	{ "be2",	"dt_el_safety_b",	"value"  },
47
	{ "be3",	"dt_el_safety_t_not",	"output" },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
48
	{ "be4",	"dt_el_safety_t",	"value"  },
49
	{ "be5",	"dt_el_bex",		"value"  },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
50
51
52
53
54
55
56
57
58
59
60
61
62
	{ "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;
63
	struct controller_block_private *private;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
64
65
66
67
68
69
70
71
72
73
74
75
	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;

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

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

	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) {
94
			log_send(LOG_T_ERROR, "subblock %s not found", ghost_outputs[i].realblock);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
95
96
97
98
99
100
101
102
103
104
105
106
107
108
			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;
			}
		}
109
110
111
112
113
		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
114
115
116
	}

	for (i = 0; ghost_inputs[i].name; i++);
117
118
	stoeber->inputs = i + 2;
	stoeber->input = malloc(sizeof(struct controller_block_interm)*stoeber->inputs);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
	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;
			}
		}
139
140
141
142
143
		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
144
	}
145
146
147
148
149
150
151
152
153
	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
154
155
156
157
158
159
160
161
162
163
164
165
166

	stoeber->params = 0;

	stoeber->calculate = NULL;

	controller_block_add(stoeber);

	return stoeber;

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