audiostuff: spandsp-0.0.6pre17/src/tone

annotate spandsp-0.0.6pre17/src/tone_detect.c @ 6:22a74b01a099 default tip

implement more meaningful test program

author	Peter Meerwald <pmeerw@cosy.sbg.ac.at>
date	Fri, 25 Jun 2010 16:14:50 +0200
parents	26cd8f1ef0b1
children

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

Mercurial > hg > audiostuff

annotate spandsp-0.0.6pre17/src/tone_detect.c @ 6:22a74b01a099 default tip