Mercurial > hg > audiostuff
diff spandsp-0.0.6pre17/src/tone_detect.c @ 4:26cd8f1ef0b1
import spandsp-0.0.6pre17
author | Peter Meerwald <pmeerw@cosy.sbg.ac.at> |
---|---|
date | Fri, 25 Jun 2010 15:50:58 +0200 |
parents | |
children |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/spandsp-0.0.6pre17/src/tone_detect.c Fri Jun 25 15:50:58 2010 +0200 @@ -0,0 +1,302 @@ +/* + * SpanDSP - a series of DSP components for telephony + * + * tone_detect.c - General telephony tone detection. + * + * Written by Steve Underwood <steveu@coppice.org> + * + * Copyright (C) 2001-2003, 2005 Steve Underwood + * + * All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License version 2.1, + * as published by the Free Software Foundation. + * + * 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 Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + * + * $Id: tone_detect.c,v 1.53 2009/04/12 09:12:10 steveu Exp $ + */ + +/*! \file */ + +#if defined(HAVE_CONFIG_H) +#include "config.h" +#endif + +#include <inttypes.h> +#include <stdlib.h> +#if defined(HAVE_TGMATH_H) +#include <tgmath.h> +#endif +#if defined(HAVE_MATH_H) +#include <math.h> +#endif +#include "floating_fudge.h" +#include <string.h> +#include <stdio.h> +#include <time.h> +#include <fcntl.h> + +#include "spandsp/telephony.h" +#include "spandsp/complex.h" +#include "spandsp/complex_vector_float.h" +#include "spandsp/tone_detect.h" +#include "spandsp/tone_generate.h" + +#include "spandsp/private/tone_detect.h" + +#if !defined(M_PI) +/* C99 systems may not define M_PI */ +#define M_PI 3.14159265358979323846264338327 +#endif + +SPAN_DECLARE(void) make_goertzel_descriptor(goertzel_descriptor_t *t, float freq, int samples) +{ +#if defined(SPANDSP_USE_FIXED_POINT) + t->fac = 16383.0f*2.0f*cosf(2.0f*M_PI*(freq/(float) SAMPLE_RATE)); +#else + t->fac = 2.0f*cosf(2.0f*M_PI*(freq/(float) SAMPLE_RATE)); +#endif + t->samples = samples; +} +/*- End of function --------------------------------------------------------*/ + +SPAN_DECLARE(goertzel_state_t *) goertzel_init(goertzel_state_t *s, + goertzel_descriptor_t *t) +{ + if (s == NULL) + { + if ((s = (goertzel_state_t *) malloc(sizeof(*s))) == NULL) + return NULL; + } +#if defined(SPANDSP_USE_FIXED_POINT) + s->v2 = + s->v3 = 0; +#else + s->v2 = + s->v3 = 0.0f; +#endif + s->fac = t->fac; + s->samples = t->samples; + s->current_sample = 0; + return s; +} +/*- End of function --------------------------------------------------------*/ + +SPAN_DECLARE(int) goertzel_release(goertzel_state_t *s) +{ + return 0; +} +/*- End of function --------------------------------------------------------*/ + +SPAN_DECLARE(int) goertzel_free(goertzel_state_t *s) +{ + if (s) + free(s); + return 0; +} +/*- End of function --------------------------------------------------------*/ + +SPAN_DECLARE(void) goertzel_reset(goertzel_state_t *s) +{ +#if defined(SPANDSP_USE_FIXED_POINT) + s->v2 = + s->v3 = 0; +#else + s->v2 = + s->v3 = 0.0f; +#endif + s->current_sample = 0; +} +/*- End of function --------------------------------------------------------*/ + +SPAN_DECLARE(int) goertzel_update(goertzel_state_t *s, + const int16_t amp[], + int samples) +{ + int i; +#if defined(SPANDSP_USE_FIXED_POINT) + int16_t x; + int16_t v1; +#else + float v1; +#endif + + if (samples > s->samples - s->current_sample) + samples = s->samples - s->current_sample; + for (i = 0; i < samples; i++) + { + v1 = s->v2; + s->v2 = s->v3; +#if defined(SPANDSP_USE_FIXED_POINT) + x = (((int32_t) s->fac*s->v2) >> 14); + /* Scale down the input signal to avoid overflows. 9 bits is enough to + monitor the signals of interest with adequate dynamic range and + resolution. In telephony we generally only start with 13 or 14 bits, + anyway. */ + s->v3 = x - v1 + (amp[i] >> 7); +#else + s->v3 = s->fac*s->v2 - v1 + amp[i]; +#endif + } + s->current_sample += samples; + return samples; +} +/*- End of function --------------------------------------------------------*/ + +#if defined(SPANDSP_USE_FIXED_POINT) +SPAN_DECLARE(int32_t) goertzel_result(goertzel_state_t *s) +#else +SPAN_DECLARE(float) goertzel_result(goertzel_state_t *s) +#endif +{ +#if defined(SPANDSP_USE_FIXED_POINT) + int16_t v1; + int32_t x; + int32_t y; +#else + float v1; +#endif + + /* Push a zero through the process to finish things off. */ + v1 = s->v2; + s->v2 = s->v3; +#if defined(SPANDSP_USE_FIXED_POINT) + x = (((int32_t) s->fac*s->v2) >> 14); + s->v3 = x - v1; +#else + s->v3 = s->fac*s->v2 - v1; +#endif + /* Now calculate the non-recursive side of the filter. */ + /* The result here is not scaled down to allow for the magnification + effect of the filter (the usual DFT magnification effect). */ +#if defined(SPANDSP_USE_FIXED_POINT) + x = (int32_t) s->v3*s->v3; + y = (int32_t) s->v2*s->v2; + x += y; + y = ((int32_t) s->v3*s->fac) >> 14; + y *= s->v2; + x -= y; + x <<= 1; + goertzel_reset(s); + /* The number returned in a floating point build will be 16384^2 times + as big as for a fixed point build, due to the 14 bit shifts + (or the square of the 7 bit shifts, depending how you look at it). */ + return x; +#else + v1 = s->v3*s->v3 + s->v2*s->v2 - s->v2*s->v3*s->fac; + v1 *= 2.0; + goertzel_reset(s); + return v1; +#endif +} +/*- End of function --------------------------------------------------------*/ + +SPAN_DECLARE(complexf_t) periodogram(const complexf_t coeffs[], const complexf_t amp[], int len) +{ + complexf_t sum; + complexf_t diff; + complexf_t x; + int i; + + x = complex_setf(0.0f, 0.0f); + for (i = 0; i < len/2; i++) + { + sum = complex_addf(&[i], &[len - 1 - i]); + diff = complex_subf(&[i], &[len - 1 - i]); + x.re += (coeffs[i].re*sum.re - coeffs[i].im*diff.im); + x.im += (coeffs[i].re*sum.im + coeffs[i].im*diff.re); + } + return x; +} +/*- End of function --------------------------------------------------------*/ + +SPAN_DECLARE(int) periodogram_prepare(complexf_t sum[], complexf_t diff[], const complexf_t amp[], int len) +{ + int i; + + for (i = 0; i < len/2; i++) + { + sum[i] = complex_addf(&[i], &[len - 1 - i]); + diff[i] = complex_subf(&[i], &[len - 1 - i]); + } + return len/2; +} +/*- End of function --------------------------------------------------------*/ + +SPAN_DECLARE(complexf_t) periodogram_apply(const complexf_t coeffs[], const complexf_t sum[], const complexf_t diff[], int len) +{ + complexf_t x; + int i; + + x = complex_setf(0.0f, 0.0f); + for (i = 0; i < len/2; i++) + { + x.re += (coeffs[i].re*sum[i].re - coeffs[i].im*diff[i].im); + x.im += (coeffs[i].re*sum[i].im + coeffs[i].im*diff[i].re); + } + return x; +} +/*- End of function --------------------------------------------------------*/ + +SPAN_DECLARE(int) periodogram_generate_coeffs(complexf_t coeffs[], float freq, int sample_rate, int window_len) +{ + float window; + float sum; + float x; + int i; + + sum = 0.0f; + for (i = 0; i < window_len/2; i++) + { + /* Apply a Hamming window as we go */ + window = 0.53836f - 0.46164f*cosf(2.0f*3.1415926535f*i/(window_len - 1.0f)); + x = (i - window_len/2.0f + 0.5f)*freq*2.0f*3.1415926535f/sample_rate; + coeffs[i].re = cosf(x)*window; + coeffs[i].im = -sinf(x)*window; + sum += window; + } + /* Rescale for unity gain in the periodogram. The 2.0 factor is to allow for the full window, + rather than just the half over which we have summed the coefficients. */ + sum = 1.0f/(2.0f*sum); + for (i = 0; i < window_len/2; i++) + { + coeffs[i].re *= sum; + coeffs[i].im *= sum; + } + return window_len/2; +} +/*- End of function --------------------------------------------------------*/ + +SPAN_DECLARE(float) periodogram_generate_phase_offset(complexf_t *offset, float freq, int sample_rate, int interval) +{ + float x; + + /* The phase offset is how far the phase rotates in one frame */ + x = 2.0f*3.1415926535f*(float) interval/(float) sample_rate; + offset->re = cosf(freq*x); + offset->im = sinf(freq*x); + return 1.0f/x; +} +/*- End of function --------------------------------------------------------*/ + +SPAN_DECLARE(float) periodogram_freq_error(const complexf_t *phase_offset, float scale, const complexf_t *last_result, const complexf_t *result) +{ + complexf_t prediction; + + /* Rotate the last result by the expected phasor offset to the current result. Then + find the difference between that predicted position, and the actual one. When + scaled by the current signal level, this gives us the frequency error. */ + prediction = complex_mulf(last_result, phase_offset); + return scale*(result->im*prediction.re - result->re*prediction.im)/(result->re*result->re + result->im*result->im); +} +/*- End of function --------------------------------------------------------*/ +/*- End of file ------------------------------------------------------------*/