block_inverse_proportional.c 2.93 KB
Newer Older
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
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
/*
	Copyright Jeroen Vreeken (jeroen@vreeken.net), 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 <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <controller/controller_block.h>

/*
       inputs      outputs
       nr name     nr name

     ----------------------
     |                    |
  ---| 0  in       0  out |----
     |	                  |
     ----------------------

	out = c0 + c1 / (in + c2)
     
 */

struct controller_block_private {
	float *in;
	float out;

	float c0;
	float c1;
	float c2;
};

static void calculate(struct controller_block *gain)
{
	struct controller_block_private *priv = gain->private;
	
	priv->out = priv->c0 + priv->c1 / (*priv->in + priv->c2);
}

55
56
57
58
59
static int param_set_c0(struct controller_block *iprop, va_list val)
{
	iprop->private->c0 = va_arg(val, double);
	return 0;
}
60

61
static int param_set_c1(struct controller_block *iprop, va_list val)
62
{
63
64
	iprop->private->c1 = va_arg(val, double);
	return 0;
65
66
}

67
static int param_set_c2(struct controller_block *iprop, va_list val)
68
{
69
70
	iprop->private->c2 = va_arg(val, double);
	return 0;
71
72
}

73
74
75
76
77
78
79
static struct controller_block_param_list params[] = {
	{ "c0", false, param_set_c0 },
	{ "c1", false, param_set_c1 },
	{ "c2", false, param_set_c2 },
	{ NULL },
};

80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
static struct controller_block_interm_list interms[] = {
	{ "in",  CONTROLLER_BLOCK_TERM_FLOAT, 
	    offsetof(struct controller_block_private, in)  },
	{ NULL },
};

static struct controller_block_outterm_list outterms[] = {
	{ "out",  CONTROLLER_BLOCK_TERM_FLOAT, 
	    offsetof(struct controller_block_private, out)  },
	{ NULL },
};


struct controller_block * block_inverse_proportional_create(char *name)
{
	struct controller_block *iprop;
	
	iprop = controller_block_alloc("inverse_proportional", name,
	    sizeof(struct controller_block_private));
	if (!iprop)
		return NULL;

	iprop->private->out = 0.0;
	iprop->private->c0 = 0.0;
	iprop->private->c1 = 1.0;
	iprop->private->c2 = 0.0;

	if (controller_block_interm_list_init(iprop, interms))
		goto err_inputs;

	if (controller_block_outterm_list_init(iprop, outterms))
		goto err_outputs;

	iprop->calculate = calculate;

115
	if (controller_block_param_list_add(iprop, params))
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
		goto err_params;

	controller_block_add(iprop);
	return iprop;

err_params:
	free(iprop->output);
err_outputs:
	free(iprop->input);
err_inputs:
	free(iprop->private);
	free(iprop->name);
	free(iprop);
	return NULL;
}