|
5
|
1 /*
|
|
|
2 * SpanDSP - a series of DSP components for telephony
|
|
|
3 *
|
|
|
4 * tone_detect.c - General telephony tone detection, and specific
|
|
|
5 * detection of Bell MF, and MFC/R2.
|
|
|
6 *
|
|
|
7 * Written by Steve Underwood <steveu@coppice.org>
|
|
|
8 *
|
|
|
9 * Copyright (C) 2001-2003, 2005 Steve Underwood
|
|
|
10 *
|
|
|
11 * All rights reserved.
|
|
|
12 *
|
|
|
13 * This program is free software; you can redistribute it and/or modify
|
|
|
14 * it under the terms of the GNU General Public License version 2, as
|
|
|
15 * published by the Free Software Foundation.
|
|
|
16 *
|
|
|
17 * This program is distributed in the hope that it will be useful,
|
|
|
18 * but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
19 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
20 * GNU General Public License for more details.
|
|
|
21 *
|
|
|
22 * You should have received a copy of the GNU General Public License
|
|
|
23 * along with this program; if not, write to the Free Software
|
|
|
24 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
|
|
25 *
|
|
|
26 * $Id: tone_detect.c,v 1.30 2006/11/19 14:07:25 steveu Exp $
|
|
|
27 */
|
|
|
28
|
|
|
29 /*! \file tone_detect.h */
|
|
|
30
|
|
|
31 #ifdef HAVE_CONFIG_H
|
|
|
32 #include "config.h"
|
|
|
33 #endif
|
|
|
34
|
|
|
35 #include <inttypes.h>
|
|
|
36 #include <stdlib.h>
|
|
|
37 #if defined(HAVE_TGMATH_H)
|
|
|
38 #include <tgmath.h>
|
|
|
39 #endif
|
|
|
40 #if defined(HAVE_MATH_H)
|
|
|
41 #include <math.h>
|
|
|
42 #endif
|
|
|
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/tone_detect.h"
|
|
|
50 #include "spandsp/tone_generate.h"
|
|
|
51
|
|
|
52 #if !defined(M_PI)
|
|
|
53 /* C99 systems may not define M_PI */
|
|
|
54 #define M_PI 3.14159265358979323846264338327
|
|
|
55 #endif
|
|
|
56
|
|
|
57 void make_goertzel_descriptor(goertzel_descriptor_t *t, float freq, int samples)
|
|
|
58 {
|
|
|
59 t->fac = 2.0f*cosf(2.0f*M_PI*(freq/(float) SAMPLE_RATE));
|
|
|
60 t->samples = samples;
|
|
|
61 }
|
|
|
62 /*- End of function --------------------------------------------------------*/
|
|
|
63
|
|
|
64 goertzel_state_t *goertzel_init(goertzel_state_t *s,
|
|
|
65 goertzel_descriptor_t *t)
|
|
|
66 {
|
|
|
67 if (s || (s = malloc(sizeof(goertzel_state_t))))
|
|
|
68 {
|
|
|
69 s->v2 =
|
|
|
70 s->v3 = 0.0;
|
|
|
71 s->fac = t->fac;
|
|
|
72 s->samples = t->samples;
|
|
|
73 s->current_sample = 0;
|
|
|
74 }
|
|
|
75 return s;
|
|
|
76 }
|
|
|
77 /*- End of function --------------------------------------------------------*/
|
|
|
78
|
|
|
79 void goertzel_reset(goertzel_state_t *s)
|
|
|
80 {
|
|
|
81 s->v2 =
|
|
|
82 s->v3 = 0.0;
|
|
|
83 s->current_sample = 0;
|
|
|
84 }
|
|
|
85 /*- End of function --------------------------------------------------------*/
|
|
|
86
|
|
|
87 int goertzel_update(goertzel_state_t *s,
|
|
|
88 const int16_t amp[],
|
|
|
89 int samples)
|
|
|
90 {
|
|
|
91 int i;
|
|
|
92 float v1;
|
|
|
93
|
|
|
94 if (samples > s->samples - s->current_sample)
|
|
|
95 samples = s->samples - s->current_sample;
|
|
|
96 for (i = 0; i < samples; i++)
|
|
|
97 {
|
|
|
98 v1 = s->v2;
|
|
|
99 s->v2 = s->v3;
|
|
|
100 s->v3 = s->fac*s->v2 - v1 + amp[i];
|
|
|
101 }
|
|
|
102 s->current_sample += samples;
|
|
|
103 return samples;
|
|
|
104 }
|
|
|
105 /*- End of function --------------------------------------------------------*/
|
|
|
106
|
|
|
107 float goertzel_result(goertzel_state_t *s)
|
|
|
108 {
|
|
|
109 float v1;
|
|
|
110
|
|
|
111 /* Push a zero through the process to finish things off. */
|
|
|
112 v1 = s->v2;
|
|
|
113 s->v2 = s->v3;
|
|
|
114 s->v3 = s->fac*s->v2 - v1;
|
|
|
115 /* Now calculate the non-recursive side of the filter. */
|
|
|
116 /* The result here is not scaled down to allow for the magnification
|
|
|
117 effect of the filter (the usual DFT magnification effect). */
|
|
|
118 return s->v3*s->v3 + s->v2*s->v2 - s->v2*s->v3*s->fac;
|
|
|
119 }
|
|
|
120 /*- End of function --------------------------------------------------------*/
|
|
|
121 /*- End of file ------------------------------------------------------------*/
|
