Mercurial > hg > audiostuff

comparison spandsp-0.0.6pre17/src/tone_detect.c @ 4:26cd8f1ef0b1

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
import spandsp-0.0.6pre17
author Peter Meerwald <pmeerw@cosy.sbg.ac.at>
date Fri, 25 Jun 2010 15:50:58 +0200
parents
children
comparison
equal deleted inserted replaced
3:c6c5a16ce2f2 4:26cd8f1ef0b1
1 /*
2 * SpanDSP - a series of DSP components for telephony
3 *
4 * tone_detect.c - General telephony tone detection.
5 *
6 * Written by Steve Underwood <steveu@coppice.org>
7 *
8 * Copyright (C) 2001-2003, 2005 Steve Underwood
9 *
10 * All rights reserved.
11 *
12 * This program is free software; you can redistribute it and/or modify
13 * it under the terms of the GNU Lesser General Public License version 2.1,
14 * as published by the Free Software Foundation.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Lesser General Public License for more details.
20 *
21 * You should have received a copy of the GNU Lesser General Public
22 * License along with this program; if not, write to the Free Software
23 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24 *
25 * $Id: tone_detect.c,v 1.53 2009/04/12 09:12:10 steveu Exp $
26 */
27
28 /*! \file */
29
30 #if defined(HAVE_CONFIG_H)
31 #include "config.h"
32 #endif
33
34 #include <inttypes.h>
35 #include <stdlib.h>
36 #if defined(HAVE_TGMATH_H)
37 #include <tgmath.h>
38 #endif
39 #if defined(HAVE_MATH_H)
40 #include <math.h>
41 #endif
42 #include "floating_fudge.h"
43 #include <string.h>
44 #include <stdio.h>
45 #include <time.h>
46 #include <fcntl.h>
47
48 #include "spandsp/telephony.h"
49 #include "spandsp/complex.h"
50 #include "spandsp/complex_vector_float.h"
51 #include "spandsp/tone_detect.h"
52 #include "spandsp/tone_generate.h"
53
54 #include "spandsp/private/tone_detect.h"
55
56 #if !defined(M_PI)
57 /* C99 systems may not define M_PI */
58 #define M_PI 3.14159265358979323846264338327
59 #endif
60
61 SPAN_DECLARE(void) make_goertzel_descriptor(goertzel_descriptor_t *t, float freq, int samples)
62 {
63 #if defined(SPANDSP_USE_FIXED_POINT)
64 t->fac = 16383.0f*2.0f*cosf(2.0f*M_PI*(freq/(float) SAMPLE_RATE));
65 #else
66 t->fac = 2.0f*cosf(2.0f*M_PI*(freq/(float) SAMPLE_RATE));
67 #endif
68 t->samples = samples;
69 }
70 /*- End of function --------------------------------------------------------*/
71
72 SPAN_DECLARE(goertzel_state_t *) goertzel_init(goertzel_state_t *s,
73 goertzel_descriptor_t *t)
74 {
75 if (s == NULL)
76 {
77 if ((s = (goertzel_state_t *) malloc(sizeof(*s))) == NULL)
78 return NULL;
79 }
80 #if defined(SPANDSP_USE_FIXED_POINT)
81 s->v2 =
82 s->v3 = 0;
83 #else
84 s->v2 =
85 s->v3 = 0.0f;
86 #endif
87 s->fac = t->fac;
88 s->samples = t->samples;
89 s->current_sample = 0;
90 return s;
91 }
92 /*- End of function --------------------------------------------------------*/
93
94 SPAN_DECLARE(int) goertzel_release(goertzel_state_t *s)
95 {
96 return 0;
97 }
98 /*- End of function --------------------------------------------------------*/
99
100 SPAN_DECLARE(int) goertzel_free(goertzel_state_t *s)
101 {
102 if (s)
103 free(s);
104 return 0;
105 }
106 /*- End of function --------------------------------------------------------*/
107
108 SPAN_DECLARE(void) goertzel_reset(goertzel_state_t *s)
109 {
110 #if defined(SPANDSP_USE_FIXED_POINT)
111 s->v2 =
112 s->v3 = 0;
113 #else
114 s->v2 =
115 s->v3 = 0.0f;
116 #endif
117 s->current_sample = 0;
118 }
119 /*- End of function --------------------------------------------------------*/
120
121 SPAN_DECLARE(int) goertzel_update(goertzel_state_t *s,
122 const int16_t amp[],
123 int samples)
124 {
125 int i;
126 #if defined(SPANDSP_USE_FIXED_POINT)
127 int16_t x;
128 int16_t v1;
129 #else
130 float v1;
131 #endif
132
133 if (samples > s->samples - s->current_sample)
134 samples = s->samples - s->current_sample;
135 for (i = 0; i < samples; i++)
136 {
137 v1 = s->v2;
138 s->v2 = s->v3;
139 #if defined(SPANDSP_USE_FIXED_POINT)
140 x = (((int32_t) s->fac*s->v2) >> 14);
141 /* Scale down the input signal to avoid overflows. 9 bits is enough to
142 monitor the signals of interest with adequate dynamic range and
143 resolution. In telephony we generally only start with 13 or 14 bits,
144 anyway. */
145 s->v3 = x - v1 + (amp[i] >> 7);
146 #else
147 s->v3 = s->fac*s->v2 - v1 + amp[i];
148 #endif
149 }
150 s->current_sample += samples;
151 return samples;
152 }
153 /*- End of function --------------------------------------------------------*/
154
155 #if defined(SPANDSP_USE_FIXED_POINT)
156 SPAN_DECLARE(int32_t) goertzel_result(goertzel_state_t *s)
157 #else
158 SPAN_DECLARE(float) goertzel_result(goertzel_state_t *s)
159 #endif
160 {
161 #if defined(SPANDSP_USE_FIXED_POINT)
162 int16_t v1;
163 int32_t x;
164 int32_t y;
165 #else
166 float v1;
167 #endif
168
169 /* Push a zero through the process to finish things off. */
170 v1 = s->v2;
171 s->v2 = s->v3;
172 #if defined(SPANDSP_USE_FIXED_POINT)
173 x = (((int32_t) s->fac*s->v2) >> 14);
174 s->v3 = x - v1;
175 #else
176 s->v3 = s->fac*s->v2 - v1;
177 #endif
178 /* Now calculate the non-recursive side of the filter. */
179 /* The result here is not scaled down to allow for the magnification
180 effect of the filter (the usual DFT magnification effect). */
181 #if defined(SPANDSP_USE_FIXED_POINT)
182 x = (int32_t) s->v3*s->v3;
183 y = (int32_t) s->v2*s->v2;
184 x += y;
185 y = ((int32_t) s->v3*s->fac) >> 14;
186 y *= s->v2;
187 x -= y;
188 x <<= 1;
189 goertzel_reset(s);
190 /* The number returned in a floating point build will be 16384^2 times
191 as big as for a fixed point build, due to the 14 bit shifts
192 (or the square of the 7 bit shifts, depending how you look at it). */
193 return x;
194 #else
195 v1 = s->v3*s->v3 + s->v2*s->v2 - s->v2*s->v3*s->fac;
196 v1 *= 2.0;
197 goertzel_reset(s);
198 return v1;
199 #endif
200 }
201 /*- End of function --------------------------------------------------------*/
202
203 SPAN_DECLARE(complexf_t) periodogram(const complexf_t coeffs[], const complexf_t amp[], int len)
204 {
205 complexf_t sum;
206 complexf_t diff;
207 complexf_t x;
208 int i;
209
210 x = complex_setf(0.0f, 0.0f);
211 for (i = 0; i < len/2; i++)
212 {
213 sum = complex_addf(&amp[i], &amp[len - 1 - i]);
214 diff = complex_subf(&amp[i], &amp[len - 1 - i]);
215 x.re += (coeffs[i].re*sum.re - coeffs[i].im*diff.im);
216 x.im += (coeffs[i].re*sum.im + coeffs[i].im*diff.re);
217 }
218 return x;
219 }
220 /*- End of function --------------------------------------------------------*/
221
222 SPAN_DECLARE(int) periodogram_prepare(complexf_t sum[], complexf_t diff[], const complexf_t amp[], int len)
223 {
224 int i;
225
226 for (i = 0; i < len/2; i++)
227 {
228 sum[i] = complex_addf(&amp[i], &amp[len - 1 - i]);
229 diff[i] = complex_subf(&amp[i], &amp[len - 1 - i]);
230 }
231 return len/2;
232 }
233 /*- End of function --------------------------------------------------------*/
234
235 SPAN_DECLARE(complexf_t) periodogram_apply(const complexf_t coeffs[], const complexf_t sum[], const complexf_t diff[], int len)
236 {
237 complexf_t x;
238 int i;
239
240 x = complex_setf(0.0f, 0.0f);
241 for (i = 0; i < len/2; i++)
242 {
243 x.re += (coeffs[i].re*sum[i].re - coeffs[i].im*diff[i].im);
244 x.im += (coeffs[i].re*sum[i].im + coeffs[i].im*diff[i].re);
245 }
246 return x;
247 }
248 /*- End of function --------------------------------------------------------*/
249
250 SPAN_DECLARE(int) periodogram_generate_coeffs(complexf_t coeffs[], float freq, int sample_rate, int window_len)
251 {
252 float window;
253 float sum;
254 float x;
255 int i;
256
257 sum = 0.0f;
258 for (i = 0; i < window_len/2; i++)
259 {
260 /* Apply a Hamming window as we go */
261 window = 0.53836f - 0.46164f*cosf(2.0f*3.1415926535f*i/(window_len - 1.0f));
262 x = (i - window_len/2.0f + 0.5f)*freq*2.0f*3.1415926535f/sample_rate;
263 coeffs[i].re = cosf(x)*window;
264 coeffs[i].im = -sinf(x)*window;
265 sum += window;
266 }
267 /* Rescale for unity gain in the periodogram. The 2.0 factor is to allow for the full window,
268 rather than just the half over which we have summed the coefficients. */
269 sum = 1.0f/(2.0f*sum);
270 for (i = 0; i < window_len/2; i++)
271 {
272 coeffs[i].re *= sum;
273 coeffs[i].im *= sum;
274 }
275 return window_len/2;
276 }
277 /*- End of function --------------------------------------------------------*/
278
279 SPAN_DECLARE(float) periodogram_generate_phase_offset(complexf_t *offset, float freq, int sample_rate, int interval)
280 {
281 float x;
282
283 /* The phase offset is how far the phase rotates in one frame */
284 x = 2.0f*3.1415926535f*(float) interval/(float) sample_rate;
285 offset->re = cosf(freq*x);
286 offset->im = sinf(freq*x);
287 return 1.0f/x;
288 }
289 /*- End of function --------------------------------------------------------*/
290
291 SPAN_DECLARE(float) periodogram_freq_error(const complexf_t *phase_offset, float scale, const complexf_t *last_result, const complexf_t *result)
292 {
293 complexf_t prediction;
294
295 /* Rotate the last result by the expected phasor offset to the current result. Then
296 find the difference between that predicted position, and the actual one. When
297 scaled by the current signal level, this gives us the frequency error. */
298 prediction = complex_mulf(last_result, phase_offset);
299 return scale*(result->im*prediction.re - result->re*prediction.im)/(result->re*result->re + result->im*result->im);
300 }
301 /*- End of function --------------------------------------------------------*/
302 /*- End of file ------------------------------------------------------------*/

Repositories maintained by Peter Meerwald, pmeerw@pmeerw.net.