block_filter_iir.c 3.98 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), 2007
	Copyright Stichting C.A. Muller Radioastronomiestation, 2007

	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 <controller/controller_block.h>
#include <block/block_filter_iir.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

/*
       inputs      outputs
       nr name     nr name

     ----------------------
     |                    |
  ---| 0  in       0  out |----
     |	                  |
     ----------------------
     
 */

struct controller_block_private {
	float *input0;
	float out;
	
	int ind;
	int len;
	float (*transfer)[4];
	
	struct block_filter_iir_param param;
};

50
51
52
53
54
static void filter_iir_calculate(struct controller_block *iir)
{
	struct controller_block_private *priv = iir->private;
	float input0 = *priv->input0;
	float (*transfer)[4] = priv->transfer;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
55
56
	int i;
	int ind, len;
57
	float out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
58

59
60
61
62
63
64
65
	ind = priv->ind;
	transfer[ind][2] = input0;
	len = priv->len;
	ind++;
	ind = ind % len;
	
	priv->ind = ind;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
66

67
	out = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
68
	for (i = 0; i < len; i++) {
69
70
71
72
73
74
75
76
		int j = (i + ind) % len;
		
		out += transfer[i][1] * transfer[j][2];
		out += transfer[i][0] * transfer[j][3];
	}
	transfer[ind][3] = out;

	priv->out = out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
77
78
}

79
static int param_set(struct controller_block *iir, va_list ap)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
80
81
{
	int i;
82
83
84
85
	int len;
	float gain;
	float *poles;
	float *zeros;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
86
			
87
88
89
90
	len = va_arg(ap, int);
	gain = va_arg(ap, double);
	poles = va_arg(ap, float *);
	zeros = va_arg(ap, float *);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
91
			
92
93
94
95
	iir->private->param.len = len;
	iir->private->param.gain = gain;
	iir->private->param.poles = poles;
	iir->private->param.zeros = zeros;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
96
			
97
98
	if (iir->private->transfer)
		free(iir->private->transfer);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
99
		
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
	iir->private->len = iir->private->param.len + 1;
	iir->private->transfer = malloc(sizeof(float[4]) *
	     iir->private->len);
	if (!iir->private->transfer) {
		iir->private->len = 0;
		return -1;
	}
	iir->private->transfer[0][0] = 0;
	iir->private->transfer[iir->private->param.len ][1] =
	    1.0 / iir->private->param.gain;
	for (i = 0; i < iir->private->param.len; i++) {
		iir->private->transfer[i + 1][0] =
		    iir->private->param.poles[i];
		iir->private->transfer[i][1] =
		    iir->private->param.zeros[i] / iir->private->param.gain;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
115
	}
116
117
118
119
120
121
122
	for (i = 0; i < iir->private->len; i++) {
		iir->private->transfer[i][2] = 0;
		iir->private->transfer[i][3] = 0;
	}
	iir->private->ind = 0;
	iir->private->out = 0;
	return 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
123
124
}

125
126
127
128
129
static struct controller_block_param_list params[] = {
	{ "transfer", true, param_set },
	{ NULL },
};

130
131
132
133
134
135
136
137
138
139
static struct controller_block_interm_list interms[] = {
	{ "in", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, input0) },
	{ NULL }
};

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

Jeroen Vreeken's avatar
Jeroen Vreeken committed
140
141
142
143
struct controller_block * block_filter_iir_create(char *name)
{
	struct controller_block *iir;
	
144
	if (!(iir = controller_block_alloc("filter_iir", name, sizeof(struct controller_block_private))))
Jeroen Vreeken's avatar
Jeroen Vreeken committed
145
146
147
148
149
150
151
		return NULL;

	iir->private->out = 0.0;
	iir->private->len = 0;
	iir->private->transfer = NULL;
	iir->private->ind = 0;

152
153
154
155
156
157
158
	if (controller_block_interm_list_init(iir, interms))
		goto err_block;

	if (controller_block_outterm_list_init(iir, outterms))
		goto err_block;

	iir->calculate = filter_iir_calculate;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
159

160
	if (controller_block_param_list_add(iir, params))
161
		goto err_block;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
162
163
164
165

	controller_block_add(iir);
	return iir;

166
167
err_block:
	controller_block_free(iir);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
168
169
	return NULL;
}