From 58f1c9a91226f4954a4799fab082f186923aa806 Mon Sep 17 00:00:00 2001 From: Andreas Eversberg Date: Sat, 3 Oct 2020 16:25:48 +0200 Subject: Add libraries from Osmocom-Analog --- src/libdtmf/dtmf_decode.c | 259 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 259 insertions(+) create mode 100644 src/libdtmf/dtmf_decode.c (limited to 'src/libdtmf/dtmf_decode.c') diff --git a/src/libdtmf/dtmf_decode.c b/src/libdtmf/dtmf_decode.c new file mode 100644 index 0000000..740d504 --- /dev/null +++ b/src/libdtmf/dtmf_decode.c @@ -0,0 +1,259 @@ +/* DTMF coder + * + * (C) 2016 by Andreas Eversberg + * All Rights Reserved + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include "../libsample/sample.h" +#include "dtmf_decode.h" + +//#define DEBUG + +#define level2db(level) (20 * log10(level)) +#define db2level(db) pow(10, (double)db / 20.0) + +#define DTMF_LOW_1 697.0 +#define DTMF_LOW_2 770.0 +#define DTMF_LOW_3 852.0 +#define DTMF_LOW_4 941.0 +#define DTMF_HIGH_1 1209.0 +#define DTMF_HIGH_2 1336.0 +#define DTMF_HIGH_3 1477.0 +#define DTMF_HIGH_4 1633.0 + +static const char dtmf_digit[] = " 123A456B789C*0#D"; + +#ifdef DEBUG +const char *_debug_amplitude(double level) +{ + static char text[42]; + + strcpy(text, " : "); + if (level > 1.0) + level = 1.0; + if (level < -1.0) + level = -1.0; + text[20 + (int)(level * 20)] = '*'; + + return text; +} +#endif + +int dtmf_decode_init(dtmf_dec_t *dtmf, void *priv, void (*recv_digit)(void *priv, char digit, dtmf_meas_t *meas), int samplerate, double max_amplitude, double min_amplitude) +{ + int rc; + + memset(dtmf, 0, sizeof(*dtmf)); + dtmf->priv = priv; + dtmf->recv_digit = recv_digit; + dtmf->samplerate = samplerate; + dtmf->freq_margin = 1.03; /* 1.8 .. 3.5 % */ + dtmf->max_amplitude = max_amplitude; + dtmf->min_amplitude = min_amplitude; + dtmf->forward_twist = db2level(4.0); + dtmf->reverse_twist = db2level(8.0); + dtmf->time_detect = (int)(0.025 * (double)samplerate); + dtmf->time_meas = (int)(0.015 * (double)samplerate); + dtmf->time_pause = (int)(0.010 * (double)samplerate); + + /* init fm demodulator */ + rc = fm_demod_init(&dtmf->demod_low, (double)samplerate, (DTMF_LOW_1 + DTMF_LOW_4) / 2.0, DTMF_LOW_4 - DTMF_LOW_1); + if (rc < 0) + goto error; + rc = fm_demod_init(&dtmf->demod_high, (double)samplerate, (DTMF_HIGH_1 + DTMF_HIGH_4) / 2.0, DTMF_HIGH_4 - DTMF_HIGH_1); + if (rc < 0) + goto error; + + /* use fourth order (2 iter) filter, since it is as fast as second order (1 iter) filter */ + iir_lowpass_init(&dtmf->freq_lp[0], 100.0, samplerate, 2); + iir_lowpass_init(&dtmf->freq_lp[1], 100.0, samplerate, 2); + + return 0; + +error: + dtmf_decode_exit(dtmf); + return rc; +} + +void dtmf_decode_exit(dtmf_dec_t *dtmf) +{ + fm_demod_exit(&dtmf->demod_low); + fm_demod_exit(&dtmf->demod_high); +} + +void dtmf_decode_filter(dtmf_dec_t *dtmf, sample_t *samples, int length, sample_t *frequency_low, sample_t *frequency_high, sample_t *amplitude_low, sample_t *amplitude_high) +{ + sample_t I_low[length], Q_low[length]; + sample_t I_high[length], Q_high[length]; + int i; + + fm_demodulate_real(&dtmf->demod_low, frequency_low, length, samples, I_low, Q_low); + fm_demodulate_real(&dtmf->demod_high, frequency_high, length, samples, I_high, Q_high); + /* peak amplitude is the length of I/Q vector + * since we filter out the unwanted modulation product, the vector is only half of length */ + for (i = 0; i < length; i++) { + amplitude_low[i] = sqrt(I_low[i] * I_low[i] + Q_low[i] * Q_low[i]) * 2.0; + amplitude_high[i] = sqrt(I_high[i] * I_high[i] + Q_high[i] * Q_high[i]) * 2.0; + } + iir_process(&dtmf->freq_lp[0], frequency_low, length); + iir_process(&dtmf->freq_lp[1], frequency_high, length); +} +void dtmf_decode(dtmf_dec_t *dtmf, sample_t *samples, int length) +{ + sample_t frequency_low[length], amplitude_low[length]; + sample_t frequency_high[length], amplitude_high[length]; + double margin, min_amplitude, max_amplitude, forward_twist, reverse_twist, f1, f2; + int time_detect, time_meas, time_pause; + int low = 0, high = 0; + char detected, digit; + int count; + int amplitude_ok, twist_ok; + int i; + + margin = dtmf->freq_margin; + min_amplitude = dtmf->min_amplitude; + max_amplitude = dtmf->max_amplitude; + forward_twist = dtmf->forward_twist; + reverse_twist = dtmf->reverse_twist; + time_detect = dtmf->time_detect; + time_meas = dtmf->time_meas; + time_pause = dtmf->time_pause; + detected = dtmf->detected; + count = dtmf->count; + + /* FM/AM demod */ + dtmf_decode_filter(dtmf, samples, length, frequency_low, frequency_high, amplitude_low, amplitude_high); + + for (i = 0; i < length; i++) { +#ifdef DEBUG +// printf("%s %.5f\n", _debug_amplitude(samples[i]/2.0), samples[i]/2.0); +#endif + /* get frequency of low frequencies, correct amplitude drop at cutoff point */ + f1 = frequency_low[i] + (DTMF_LOW_1 + DTMF_LOW_4) / 2.0; + if (f1 >= DTMF_LOW_1 / margin && f1 <= DTMF_LOW_1 * margin) { + /* cutoff point */ + amplitude_low[i] /= 0.7071; + low = 1; + f1 -= DTMF_LOW_1; + } else + if (f1 >= DTMF_LOW_2 / margin && f1 <= DTMF_LOW_2 * margin) { + amplitude_low[i] /= 1.0734; + low = 2; + f1 -= DTMF_LOW_2; + } else + if (f1 >= DTMF_LOW_3 / margin && f1 <= DTMF_LOW_3 * margin) { + amplitude_low[i] /= 1.0389; + low = 3; + f1 -= DTMF_LOW_3; + } else + if (f1 >= DTMF_LOW_4 / margin && f1 <= DTMF_LOW_4 * margin) { + /* cutoff point */ + amplitude_low[i] /= 0.7071; + low = 4; + f1 -= DTMF_LOW_4; + } else + low = 0; + /* get frequency of high frequencies, correct amplitude drop at cutoff point */ + f2 = frequency_high[i] + (DTMF_HIGH_1 + DTMF_HIGH_4) / 2.0; + if (f2 >= DTMF_HIGH_1 / margin && f2 <= DTMF_HIGH_1 * margin) { + /* cutoff point */ + amplitude_high[i] /= 0.7071; + high = 1; + f2 -= DTMF_HIGH_1; + } else + if (f2 >= DTMF_HIGH_2 / margin && f2 <= DTMF_HIGH_2 * margin) { + amplitude_high[i] /= 1.0731; + high = 2; + f2 -= DTMF_HIGH_2; + } else + if (f2 >= DTMF_HIGH_3 / margin && f2 <= DTMF_HIGH_3 * margin) { + amplitude_high[i] /= 1.0372; + high = 3; + f2 -= DTMF_HIGH_3; + } else + if (f2 >= DTMF_HIGH_4 / margin && f2 <= DTMF_HIGH_4 * margin) { + /* cutoff point */ + amplitude_high[i] /= 0.7071; + high = 4; + f2 -= DTMF_HIGH_4; + } else + high = 0; + digit = 0; + amplitude_ok = 0; + twist_ok = 0; + if (low && high) { + digit = dtmf_digit[low*4+high]; + /* check for limits */ + if (amplitude_low[i] <= max_amplitude && amplitude_low[i] >= min_amplitude && amplitude_high[i] <= max_amplitude && amplitude_high[i] >= min_amplitude) { + amplitude_ok = 1; +#ifdef DEBUG + printf("%.1f %.1f (limits %.1f .. %.1f) %.1f\n", level2db(amplitude_low[i]), level2db(amplitude_high[i]), level2db(min_amplitude), level2db(max_amplitude), level2db(amplitude_high[i] / amplitude_low[i])); +#endif + if (amplitude_high[i] / amplitude_low[i] <= forward_twist && amplitude_low[i] / amplitude_high[i] <= reverse_twist) + twist_ok = 1; + } + } + + if (!detected) { + if (digit && amplitude_ok && twist_ok) { + if (count == 0) { + memset(&dtmf->meas, 0, sizeof(dtmf->meas)); + } + if (count >= time_meas) { + dtmf->meas.frequency_low += f1; + dtmf->meas.frequency_high += f2; + dtmf->meas.amplitude_low += amplitude_low[i]; + dtmf->meas.amplitude_high += amplitude_high[i]; + dtmf->meas.count++; + } + count++; + if (count >= time_detect) { + detected = digit; + dtmf->meas.frequency_low /= dtmf->meas.count; + dtmf->meas.frequency_high /= dtmf->meas.count; + dtmf->meas.amplitude_low /= dtmf->meas.count; + dtmf->meas.amplitude_high /= dtmf->meas.count; + dtmf->meas.count = 1; + dtmf->recv_digit(dtmf->priv, digit, &dtmf->meas); + } + } else + count = 0; + } else { + if (!digit || digit != detected || !amplitude_ok || !twist_ok) { + count++; + if (count >= time_pause) { + detected = 0; +#ifdef DEBUG + printf("lost!\n"); +#endif + } + } else + count = 0; + } +#ifdef DEBUG + if (digit) + printf("DTMF tone='%c' diff frequency=%.1f %.1f amplitude=%.1f %.1f dB (%s) twist=%.1f dB (%s)\n", digit, f1, f2, level2db(amplitude_low[i]), level2db(amplitude_high[i]), (amplitude_ok) ? "OK" : "nok", level2db(amplitude_high[i] / amplitude_low[i]), (twist_ok) ? "OK" : "nok"); +#endif + + dtmf->detected = detected; + dtmf->count = count; + } +} + -- cgit v1.2.3