301 lines
8.9 KiB
C
301 lines
8.9 KiB
C
/*
|
|
* 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.
|
|
*/
|
|
|
|
/*! \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 ------------------------------------------------------------*/
|