block_filter_lp.c 2.7 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
#include <controller/controller_block.h>
25
#include <controller/controller_sample.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

/*
       inputs      outputs
       nr name     nr name

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

struct controller_block_private {
	float *input0;
	float out;
	
	float alpha;
	float t;
	float tau;
};

48
static void filter_lp_calculate(struct controller_block *lp)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
49
{
50
51
52
53
54
55
56
	struct controller_block_private *priv = lp->private;
	float alpha = priv->alpha;
	float out = priv->out;
	
	out *= 1.0 - alpha;
	out += *lp->private->input0 * alpha;
	priv->out = out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
57
58
}

59
static int param_set(struct controller_block *lp, va_list val)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
60
{
61
	lp->private->tau = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
62
63

	lp->private->alpha =
64
65
	    controller_sample_period() / (controller_sample_period() + lp->private->tau);
	return 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
66
67
}

68
69
70
71
72
73
static struct controller_block_param_list params[] = {
	{ "tau", true, param_set },
	{ NULL },
};


74
75
76
77
78
79
80
81
82
83
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
84
85
86
87
struct controller_block * block_filter_lp_create(char *name)
{
	struct controller_block *lp;
	
88
	if (!(lp = controller_block_alloc("filter_lp", name, sizeof(struct controller_block_private))))
Jeroen Vreeken's avatar
Jeroen Vreeken committed
89
90
91
92
93
94
95
		return NULL;

	lp->private->out = 0.0;
	lp->private->t = 1.0;
	lp->private->tau = 1.0;
	lp->private->alpha = 0.0;

96
97
98
99
100
101
102
	if (controller_block_interm_list_init(lp, interms))
		goto err_block;

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

	lp->calculate = filter_lp_calculate;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
103

104
	if (controller_block_param_list_add(lp, params))
105
		goto err_block;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
106
107
108
109

	controller_block_add(lp);
	return lp;

110
111
err_block:
	controller_block_free(lp);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
112
113
	return NULL;
}