source: roaraudio/libroardsp/filter_goertzel.c @ 5960:65dc0ae5bc75

Last change on this file since 5960:65dc0ae5bc75 was 5960:65dc0ae5bc75, checked in by phi, 10 years ago

some coding style fixes

File size: 4.1 KB
Line 
1//filter_goertzel.c:
2
3/*
4 *      Copyright (C) Philipp 'ph3-der-loewe' Schafft - 2008-2013
5 *
6 *  This file is part of libroardsp a part of RoarAudio,
7 *  a cross-platform sound system for both, home and professional use.
8 *  See README for details.
9 *
10 *  This file is free software; you can redistribute it and/or modify
11 *  it under the terms of the GNU General Public License version 3
12 *  as published by the Free Software Foundation.
13 *
14 *  libroardsp is distributed in the hope that it will be useful,
15 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
16 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17 *  GNU General Public License for more details.
18 *
19 *  You should have received a copy of the GNU General Public License
20 *  along with this software; see the file COPYING.  If not, write to
21 *  the Free Software Foundation, 51 Franklin Street, Fifth Floor,
22 *  Boston, MA 02110-1301, USA.
23 *
24 */
25
26#include "libroardsp.h"
27
28#ifdef ROAR_HAVE_LIBM
29
30#define DEFAULT_FREQ 1000.0
31
32struct roardsp_goertzel {
33 float   freq;
34 int32_t coeff;
35 int32_t old[ROAR_MAX_CHANNELS][2];
36};
37
38int roardsp_goertzel_init  (struct roardsp_filter * filter, struct roar_stream * stream, int id) {
39 struct roardsp_goertzel * self = roar_mm_malloc(sizeof(struct roardsp_goertzel));
40
41 (void)stream, (void)id;
42
43 if ( self == NULL )
44  return -1;
45
46 memset(self, 0, sizeof(struct roardsp_goertzel));
47
48 filter->inst = (void*) self;
49
50 roardsp_goertzel_reset(filter, ROARDSP_RESET_FULL);
51
52 return 0;
53}
54
55int roardsp_goertzel_uninit(struct roardsp_filter * filter) {
56 roar_mm_free(filter->inst);
57 return 0;
58}
59
60#define _calcX(bits,twobits,limit) \
61int roardsp_goertzel_calc##bits  (struct roardsp_filter * filter, void * data, size_t samples) { \
62 struct roardsp_goertzel * self = (struct roardsp_goertzel *) filter->inst; \
63 int##bits##_t * samp = (int##bits##_t *) data; \
64 /*register int##twobits##_t s, g, h;*/ \
65 register float s, g, h; \
66 /*register int64_t s, g, h;*/ \
67 int channel; \
68 size_t i; \
69\
70 roardsp_goertzel_reset(filter, ROARDSP_RESET_STATE); \
71\
72 for (i = 0, channel = 0; i < samples; i++, channel = channel == (filter->channels - 1) ? 0 : channel + 1) { \
73  g        = self->old[channel][0]; \
74  g       *= self->coeff; \
75  g       /= 32767; \
76  s        = samp[i]; \
77  ROAR_DBG("roardsp_goertzel_calc*(*): channel=%i, g=%f, s=%f, old[0]=%li, old[1]=%li", channel, g, s, (long int)self->old[channel][0], (long int)self->old[channel][1]); \
78  s       += g; \
79  s       -= self->old[channel][1]; \
80  self->old[channel][1] = self->old[channel][0]; \
81  self->old[channel][0] = s; \
82  /* samp[i]  = s; */ \
83  g  = s; \
84  g *= self->old[channel][1]; \
85  g *= self->coeff; \
86  g /= 32767; \
87  h  = self->old[channel][1]; \
88  h *= self->old[channel][1]; \
89  s  *= s; \
90  s  += h; \
91  s  -= g; \
92  s  *= 0.0001; \
93  s  /= (float)((i+1)*(i+1)); \
94  if ( s > (limit) ) \
95   s = (limit); \
96  samp[i]  = s; \
97 } \
98\
99 return 0; \
100}
101
102_calcX(8,16,127.)
103_calcX(16,32,32767.)
104_calcX(32,64,2147483647.)
105
106int roardsp_goertzel_ctl   (struct roardsp_filter * filter, int cmd, void * data) {
107 struct roardsp_goertzel * self = (struct roardsp_goertzel *) filter->inst;
108 float old;
109
110 switch (cmd) {
111  case ROARDSP_FCTL_FREQ:
112    old = self->freq;
113    self->freq = *(float*)data;
114    *(float*)data = old;
115    old = 2.*cos(2.*M_PI*self->freq/(float)filter->rate);
116    self->coeff = old*32767.;
117    ROAR_DBG("roardsp_goertzel_ctl(filter=%p, cmd=%i, data=%p): self->coeff=%li (%f)", filter, cmd, data, (long int)self->coeff, old);
118   break;
119  default:
120    roar_err_set(ROAR_ERROR_BADRQC);
121    return -1;
122   break;
123 }
124
125 return 0;
126}
127
128int roardsp_goertzel_reset (struct roardsp_filter * filter, int what) {
129 struct roardsp_goertzel * self;
130 float freq;
131
132 if ( filter == NULL )
133  return -1;
134
135 if ( filter->inst == NULL )
136  return -1;
137
138 self = filter->inst;
139
140 switch (what) {
141  case ROARDSP_RESET_NONE:
142    return 0;
143   break;
144  case ROARDSP_RESET_FULL:
145    freq = self->freq = DEFAULT_FREQ;
146    roardsp_goertzel_ctl(filter, ROARDSP_FCTL_FREQ, &freq);
147  case ROARDSP_RESET_STATE:
148    memset(self->old, 0, sizeof(self->old));
149    return 0;
150   break;
151  default:
152    roar_err_set(ROAR_ERROR_BADRQC);
153    return -1;
154   break;
155 }
156
157 return -1;
158}
159
160#endif
161
162//ll
Note: See TracBrowser for help on using the repository browser.