dt_el_stoeber_l_sim.c 4.54 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>
23
#include <stdlib.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
24

Jeroen Vreeken's avatar
Jeroen Vreeken committed
25
26
#include <controller/controller_block.h>
#include <log/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
	{ "ae1", 	"dt_el_ae1",		"value"  },
50
	{ "external_enable", "dt_el_bex",       "value"  },
51
	{ NULL },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
52
53
54
55
56
57
58
59
60
};

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

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

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

Jeroen Vreeken's avatar
Jeroen Vreeken committed
78
79
80
		stoeber->output[i].name = ghost_outputs[i].name;
		subblock = controller_block_find(ghost_outputs[i].realblock);
		if (!subblock) {
81
			log_send(LOG_T_ERROR, "subblock %s not found", ghost_outputs[i].realblock);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
82
83
84
85
86
87
88
89
90
91
92
93
94
95
			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;
			}
		}
96
97
98
99
100
		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
101
102
103
	}

	for (i = 0; ghost_inputs[i].name; i++);
104
105
	stoeber->inputs = i + 2;
	stoeber->input = malloc(sizeof(struct controller_block_interm)*stoeber->inputs);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
	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;
			}
		}
126
127
128
129
130
		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
131
	}
132
133
134
135
136
137
138
139
140
	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
141
142
143
144
145
146
147
148
149
150
151

	stoeber->calculate = NULL;

	controller_block_add(stoeber);

	return stoeber;

err_input:
err_ghost_output:
	free(stoeber->output);
err_output:
152
	controller_block_free(stoeber);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
153
154
155
err_malloc:
	return NULL;
}
156
157
158
159
160

BLOCK_CREATE(dt_el_stoeber_l_sim) = {
	.create = block_dt_el_stoeber_l_sim_create,
	.args = { NULL },
};