Mercurial > hg > audiostuff

comparison spandsp-0.0.6pre17/src/bell_r2_mf.c @ 4:26cd8f1ef0b1

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
import spandsp-0.0.6pre17
author Peter Meerwald <pmeerw@cosy.sbg.ac.at>
date Fri, 25 Jun 2010 15:50:58 +0200
parents
children
comparison
equal deleted inserted replaced
3:c6c5a16ce2f2 4:26cd8f1ef0b1
1 /*
2 * SpanDSP - a series of DSP components for telephony
3 *
4 * bell_r2_mf.c - Bell MF and MFC/R2 tone generation and detection.
5 *
6 * Written by Steve Underwood <steveu@coppice.org>
7 *
8 * Copyright (C) 2001 Steve Underwood
9 *
10 * All rights reserved.
11 *
12 * This program is free software; you can redistribute it and/or modify
13 * it under the terms of the GNU Lesser General Public License version 2.1,
14 * as published by the Free Software Foundation.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Lesser General Public License for more details.
20 *
21 * You should have received a copy of the GNU Lesser General Public
22 * License along with this program; if not, write to the Free Software
23 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24 *
25 * $Id: bell_r2_mf.c,v 1.39.4.1 2009/12/23 14:23:48 steveu Exp $
26 */
27
28 /*! \file */
29
30 #if defined(HAVE_CONFIG_H)
31 #include "config.h"
32 #endif
33
34 #include <stdlib.h>
35 #include <inttypes.h>
36 #include <string.h>
37 #include <time.h>
38 #include <fcntl.h>
39 #if defined(HAVE_TGMATH_H)
40 #include <tgmath.h>
41 #endif
42 #if defined(HAVE_MATH_H)
43 #include <math.h>
44 #endif
45 #include "floating_fudge.h"
46
47 #include "spandsp/telephony.h"
48 #include "spandsp/fast_convert.h"
49 #include "spandsp/queue.h"
50 #include "spandsp/dc_restore.h"
51 #include "spandsp/complex.h"
52 #include "spandsp/dds.h"
53 #include "spandsp/tone_detect.h"
54 #include "spandsp/tone_generate.h"
55 #include "spandsp/super_tone_rx.h"
56 #include "spandsp/dtmf.h"
57 #include "spandsp/bell_r2_mf.h"
58
59 #include "spandsp/private/queue.h"
60 #include "spandsp/private/tone_generate.h"
61 #include "spandsp/private/bell_r2_mf.h"
62
63 #if !defined(M_PI)
64 /* C99 systems may not define M_PI */
65 #define M_PI 3.14159265358979323846264338327
66 #endif
67
68 /*!
69 MF tone descriptor.
70 */
71 typedef struct
72 {
73 int f1; /* First freq */
74 int f2; /* Second freq */
75 int8_t level1; /* Level of the first freq (dB) */
76 int8_t level2; /* Level of the second freq (dB) */
77 uint8_t on_time; /* Tone on time (ms) */
78 uint8_t off_time; /* Minimum post tone silence (ms) */
79 } mf_digit_tones_t;
80
81 int bell_mf_gen_inited = FALSE;
82 tone_gen_descriptor_t bell_mf_digit_tones[15];
83
84 int r2_mf_gen_inited = FALSE;
85 tone_gen_descriptor_t r2_mf_fwd_digit_tones[15];
86 tone_gen_descriptor_t r2_mf_back_digit_tones[15];
87
88 #if 0
89 tone_gen_descriptor_t socotel_mf_digit_tones[18];
90 #endif
91
92 /* Bell R1 tone generation specs.
93 * Power: -7dBm +- 1dB
94 * Frequency: within +-1.5%
95 * Mismatch between the start time of a pair of tones: <=6ms.
96 * Mismatch between the end time of a pair of tones: <=6ms.
97 * Tone duration: 68+-7ms, except KP which is 100+-7ms.
98 * Inter-tone gap: 68+-7ms.
99 */
100 static const mf_digit_tones_t bell_mf_tones[] =
101 {
102 { 700, 900, -7, -7, 68, 68},
103 { 700, 1100, -7, -7, 68, 68},
104 { 900, 1100, -7, -7, 68, 68},
105 { 700, 1300, -7, -7, 68, 68},
106 { 900, 1300, -7, -7, 68, 68},
107 {1100, 1300, -7, -7, 68, 68},
108 { 700, 1500, -7, -7, 68, 68},
109 { 900, 1500, -7, -7, 68, 68},
110 {1100, 1500, -7, -7, 68, 68},
111 {1300, 1500, -7, -7, 68, 68},
112 { 700, 1700, -7, -7, 68, 68}, /* ST''' - use 'C' */
113 { 900, 1700, -7, -7, 68, 68}, /* ST' - use 'A' */
114 {1100, 1700, -7, -7, 100, 68}, /* KP - use '*' */
115 {1300, 1700, -7, -7, 68, 68}, /* ST'' - use 'B' */
116 {1500, 1700, -7, -7, 68, 68}, /* ST - use '#' */
117 {0, 0, 0, 0, 0, 0}
118 };
119
120 /* The order of the digits here must match the list above */
121 static const char bell_mf_tone_codes[] = "1234567890CA*B#";
122
123 /* R2 tone generation specs.
124 * Power: -11.5dBm +- 1dB
125 * Frequency: within +-4Hz
126 * Mismatch between the start time of a pair of tones: <=1ms.
127 * Mismatch between the end time of a pair of tones: <=1ms.
128 */
129 static const mf_digit_tones_t r2_mf_fwd_tones[] =
130 {
131 {1380, 1500, -11, -11, 1, 0},
132 {1380, 1620, -11, -11, 1, 0},
133 {1500, 1620, -11, -11, 1, 0},
134 {1380, 1740, -11, -11, 1, 0},
135 {1500, 1740, -11, -11, 1, 0},
136 {1620, 1740, -11, -11, 1, 0},
137 {1380, 1860, -11, -11, 1, 0},
138 {1500, 1860, -11, -11, 1, 0},
139 {1620, 1860, -11, -11, 1, 0},
140 {1740, 1860, -11, -11, 1, 0},
141 {1380, 1980, -11, -11, 1, 0},
142 {1500, 1980, -11, -11, 1, 0},
143 {1620, 1980, -11, -11, 1, 0},
144 {1740, 1980, -11, -11, 1, 0},
145 {1860, 1980, -11, -11, 1, 0},
146 {0, 0, 0, 0, 0, 0}
147 };
148
149 static const mf_digit_tones_t r2_mf_back_tones[] =
150 {
151 {1140, 1020, -11, -11, 1, 0},
152 {1140, 900, -11, -11, 1, 0},
153 {1020, 900, -11, -11, 1, 0},
154 {1140, 780, -11, -11, 1, 0},
155 {1020, 780, -11, -11, 1, 0},
156 { 900, 780, -11, -11, 1, 0},
157 {1140, 660, -11, -11, 1, 0},
158 {1020, 660, -11, -11, 1, 0},
159 { 900, 660, -11, -11, 1, 0},
160 { 780, 660, -11, -11, 1, 0},
161 {1140, 540, -11, -11, 1, 0},
162 {1020, 540, -11, -11, 1, 0},
163 { 900, 540, -11, -11, 1, 0},
164 { 780, 540, -11, -11, 1, 0},
165 { 660, 540, -11, -11, 1, 0},
166 {0, 0, 0, 0, 0, 0}
167 };
168
169 /* The order of the digits here must match the lists above */
170 static const char r2_mf_tone_codes[] = "1234567890BCDEF";
171
172 #if 0
173 static const mf_digit_tones_t socotel_tones[] =
174 {
175 { 700, 900, -11, -11, 1, 0},
176 { 700, 1100, -11, -11, 1, 0},
177 { 900, 1100, -11, -11, 1, 0},
178 { 700, 1300, -11, -11, 1, 0},
179 { 900, 1300, -11, -11, 1, 0},
180 {1100, 1300, -11, -11, 1, 0},
181 { 700, 1500, -11, -11, 1, 0},
182 { 900, 1500, -11, -11, 1, 0},
183 {1100, 1500, -11, -11, 1, 0},
184 {1300, 1500, -11, -11, 1, 0},
185 {1500, 1700, -11, -11, 1, 0},
186 { 700, 1700, -11, -11, 1, 0},
187 { 900, 1700, -11, -11, 1, 0},
188 {1300, 1700, -11, -11, 1, 0},
189 {1100, 1700, -11, -11, 1, 0},
190 {1700, 0, -11, -11, 1, 0}, /* Use 'F' */
191 {1900, 0, -11, -11, 1, 0}, /* Use 'G' */
192 {0, 0, 0, 0, 0, 0}
193 };
194
195 /* The order of the digits here must match the list above */
196 static char socotel_mf_tone_codes[] = "1234567890ABCDEFG";
197 #endif
198
199 #if defined(SPANDSP_USE_FIXED_POINT)
200 #define BELL_MF_THRESHOLD 204089 /* -30.5dBm0 */
201 #define BELL_MF_TWIST 3.981f /* 6dB */
202 #define BELL_MF_RELATIVE_PEAK 12.589f /* 11dB */
203 #define BELL_MF_SAMPLES_PER_BLOCK 120
204
205 #define R2_MF_THRESHOLD 62974 /* -36.5dBm0 */
206 #define R2_MF_TWIST 5.012f /* 7dB */
207 #define R2_MF_RELATIVE_PEAK 12.589f /* 11dB */
208 #define R2_MF_SAMPLES_PER_BLOCK 133
209 #else
210 #define BELL_MF_THRESHOLD 3343803100.0f /* -30.5dBm0 [((120.0*32768.0/1.4142)*10^((-30.5 - DBM0_MAX_SINE_POWER)/20.0))^2 => 3343803100.0] */
211 #define BELL_MF_TWIST 3.981f /* 6dB [10^(6/10) => 3.981] */
212 #define BELL_MF_RELATIVE_PEAK 12.589f /* 11dB */
213 #define BELL_MF_SAMPLES_PER_BLOCK 120
214
215 #define R2_MF_THRESHOLD 1031766650.0f /* -36.5dBm0 [((133.0*32768.0/1.4142)*10^((-36.5 - DBM0_MAX_SINE_POWER)/20.0))^2 => 1031766650.0] */
216 #define R2_MF_TWIST 5.012f /* 7dB */
217 #define R2_MF_RELATIVE_PEAK 12.589f /* 11dB */
218 #define R2_MF_SAMPLES_PER_BLOCK 133
219 #endif
220
221 static goertzel_descriptor_t bell_mf_detect_desc[6];
222
223 static goertzel_descriptor_t mf_fwd_detect_desc[6];
224 static goertzel_descriptor_t mf_back_detect_desc[6];
225
226 static const int bell_mf_frequencies[] =
227 {
228 700, 900, 1100, 1300, 1500, 1700
229 };
230
231 /* Use the follow characters for the Bell MF special signals:
232 KP - use '*'
233 ST - use '#'
234 ST' - use 'A'
235 ST'' - use 'B'
236 ST''' - use 'C' */
237 static const char bell_mf_positions[] = "1247C-358A--69*---0B----#";
238
239 static const int r2_mf_fwd_frequencies[] =
240 {
241 1380, 1500, 1620, 1740, 1860, 1980
242 };
243
244 static const int r2_mf_back_frequencies[] =
245 {
246 1140, 1020, 900, 780, 660, 540
247 };
248
249 /* Use codes '1' to 'F' for the R2 signals 1 to 15, except for signal 'A'.
250 Use '0' for this, so the codes match the digits 0-9. */
251 static const char r2_mf_positions[] = "1247B-358C--69D---0E----F";
252
253 static void bell_mf_gen_init(void)
254 {
255 int i;
256 const mf_digit_tones_t *tones;
257
258 if (bell_mf_gen_inited)
259 return;
260 i = 0;
261 tones = bell_mf_tones;
262 while (tones->on_time)
263 {
264 /* Note: The duration of KP is longer than the other signals. */
265 make_tone_gen_descriptor(&bell_mf_digit_tones[i++],
266 tones->f1,
267 tones->level1,
268 tones->f2,
269 tones->level2,
270 tones->on_time,
271 tones->off_time,
272 0,
273 0,
274 FALSE);
275 tones++;
276 }
277 bell_mf_gen_inited = TRUE;
278 }
279 /*- End of function --------------------------------------------------------*/
280
281 SPAN_DECLARE(int) bell_mf_tx(bell_mf_tx_state_t *s, int16_t amp[], int max_samples)
282 {
283 int len;
284 const char *cp;
285 int digit;
286
287 len = 0;
288 if (s->tones.current_section >= 0)
289 {
290 /* Deal with the fragment left over from last time */
291 len = tone_gen(&(s->tones), amp, max_samples);
292 }
293 while (len < max_samples && (digit = queue_read_byte(&s->queue.queue)) >= 0)
294 {
295 /* Step to the next digit */
296 if ((cp = strchr(bell_mf_tone_codes, digit)) == NULL)
297 continue;
298 tone_gen_init(&(s->tones), &bell_mf_digit_tones[cp - bell_mf_tone_codes]);
299 len += tone_gen(&(s->tones), amp + len, max_samples - len);
300 }
301 return len;
302 }
303 /*- End of function --------------------------------------------------------*/
304
305 SPAN_DECLARE(int) bell_mf_tx_put(bell_mf_tx_state_t *s, const char *digits, int len)
306 {
307 size_t space;
308
309 /* This returns the number of characters that would not fit in the buffer.
310 The buffer will only be loaded if the whole string of digits will fit,
311 in which case zero is returned. */
312 if (len < 0)
313 {
314 if ((len = strlen(digits)) == 0)
315 return 0;
316 }
317 if ((space = queue_free_space(&s->queue.queue)) < (size_t) len)
318 return len - (int) space;
319 if (queue_write(&s->queue.queue, (const uint8_t *) digits, len) >= 0)
320 return 0;
321 return -1;
322 }
323 /*- End of function --------------------------------------------------------*/
324
325 SPAN_DECLARE(bell_mf_tx_state_t *) bell_mf_tx_init(bell_mf_tx_state_t *s)
326 {
327 if (s == NULL)
328 {
329 if ((s = (bell_mf_tx_state_t *) malloc(sizeof(*s))) == NULL)
330 return NULL;
331 }
332 memset(s, 0, sizeof(*s));
333
334 if (!bell_mf_gen_inited)
335 bell_mf_gen_init();
336 tone_gen_init(&(s->tones), &bell_mf_digit_tones[0]);
337 s->current_sample = 0;
338 queue_init(&s->queue.queue, MAX_BELL_MF_DIGITS, QUEUE_READ_ATOMIC | QUEUE_WRITE_ATOMIC);
339 s->tones.current_section = -1;
340 return s;
341 }
342 /*- End of function --------------------------------------------------------*/
343
344 SPAN_DECLARE(int) bell_mf_tx_release(bell_mf_tx_state_t *s)
345 {
346 return 0;
347 }
348 /*- End of function --------------------------------------------------------*/
349
350 SPAN_DECLARE(int) bell_mf_tx_free(bell_mf_tx_state_t *s)
351 {
352 free(s);
353 return 0;
354 }
355 /*- End of function --------------------------------------------------------*/
356
357 SPAN_DECLARE(int) r2_mf_tx(r2_mf_tx_state_t *s, int16_t amp[], int samples)
358 {
359 int len;
360
361 if (s->digit == 0)
362 {
363 len = samples;
364 memset(amp, 0, len*sizeof(int16_t));
365 }
366 else
367 {
368 len = tone_gen(&s->tone, amp, samples);
369 }
370 return len;
371 }
372 /*- End of function --------------------------------------------------------*/
373
374 SPAN_DECLARE(int) r2_mf_tx_put(r2_mf_tx_state_t *s, char digit)
375 {
376 char *cp;
377
378 if (digit && (cp = strchr(r2_mf_tone_codes, digit)))
379 {
380 if (s->fwd)
381 tone_gen_init(&s->tone, &r2_mf_fwd_digit_tones[cp - r2_mf_tone_codes]);
382 else
383 tone_gen_init(&s->tone, &r2_mf_back_digit_tones[cp - r2_mf_tone_codes]);
384 s->digit = digit;
385 }
386 else
387 {
388 s->digit = 0;
389 }
390 return 0;
391 }
392 /*- End of function --------------------------------------------------------*/
393
394 SPAN_DECLARE(r2_mf_tx_state_t *) r2_mf_tx_init(r2_mf_tx_state_t *s, int fwd)
395 {
396 int i;
397 const mf_digit_tones_t *tones;
398
399 if (s == NULL)
400 {
401 if ((s = (r2_mf_tx_state_t *) malloc(sizeof(*s))) == NULL)
402 return NULL;
403 }
404 memset(s, 0, sizeof(*s));
405
406 if (!r2_mf_gen_inited)
407 {
408 i = 0;
409 tones = r2_mf_fwd_tones;
410 while (tones->on_time)
411 {
412 make_tone_gen_descriptor(&r2_mf_fwd_digit_tones[i++],
413 tones->f1,
414 tones->level1,
415 tones->f2,
416 tones->level2,
417 tones->on_time,
418 tones->off_time,
419 0,
420 0,
421 (tones->off_time == 0));
422 tones++;
423 }
424 i = 0;
425 tones = r2_mf_back_tones;
426 while (tones->on_time)
427 {
428 make_tone_gen_descriptor(&r2_mf_back_digit_tones[i++],
429 tones->f1,
430 tones->level1,
431 tones->f2,
432 tones->level2,
433 tones->on_time,
434 tones->off_time,
435 0,
436 0,
437 (tones->off_time == 0));
438 tones++;
439 }
440 r2_mf_gen_inited = TRUE;
441 }
442 s->fwd = fwd;
443 return s;
444 }
445 /*- End of function --------------------------------------------------------*/
446
447 SPAN_DECLARE(int) r2_mf_tx_release(r2_mf_tx_state_t *s)
448 {
449 return 0;
450 }
451 /*- End of function --------------------------------------------------------*/
452
453 SPAN_DECLARE(int) r2_mf_tx_free(r2_mf_tx_state_t *s)
454 {
455 free(s);
456 return 0;
457 }
458 /*- End of function --------------------------------------------------------*/
459
460 SPAN_DECLARE(int) bell_mf_rx(bell_mf_rx_state_t *s, const int16_t amp[], int samples)
461 {
462 #if defined(SPANDSP_USE_FIXED_POINT)
463 int32_t energy[6];
464 int16_t xamp;
465 #else
466 float energy[6];
467 float xamp;
468 #endif
469 int i;
470 int j;
471 int sample;
472 int best;
473 int second_best;
474 int limit;
475 uint8_t hit;
476
477 hit = 0;
478 for (sample = 0; sample < samples; sample = limit)
479 {
480 if ((samples - sample) >= (BELL_MF_SAMPLES_PER_BLOCK - s->current_sample))
481 limit = sample + (BELL_MF_SAMPLES_PER_BLOCK - s->current_sample);
482 else
483 limit = samples;
484 for (j = sample; j < limit; j++)
485 {
486 xamp = goertzel_preadjust_amp(amp[j]);
487 goertzel_samplex(&s->out[0], xamp);
488 goertzel_samplex(&s->out[1], xamp);
489 goertzel_samplex(&s->out[2], xamp);
490 goertzel_samplex(&s->out[3], xamp);
491 goertzel_samplex(&s->out[4], xamp);
492 goertzel_samplex(&s->out[5], xamp);
493 }
494 s->current_sample += (limit - sample);
495 if (s->current_sample < BELL_MF_SAMPLES_PER_BLOCK)
496 continue;
497
498 /* We are at the end of an MF detection block */
499 /* Find the two highest energies. The spec says to look for
500 two tones and two tones only. Taking this literally -ie
501 only two tones pass the minimum threshold - doesn't work
502 well. The sinc function mess, due to rectangular windowing
503 ensure that! Find the two highest energies and ensure they
504 are considerably stronger than any of the others. */
505 energy[0] = goertzel_result(&s->out[0]);
506 energy[1] = goertzel_result(&s->out[1]);
507 if (energy[0] > energy[1])
508 {
509 best = 0;
510 second_best = 1;
511 }
512 else
513 {
514 best = 1;
515 second_best = 0;
516 }
517 for (i = 2; i < 6; i++)
518 {
519 energy[i] = goertzel_result(&s->out[i]);
520 if (energy[i] >= energy[best])
521 {
522 second_best = best;
523 best = i;
524 }
525 else if (energy[i] >= energy[second_best])
526 {
527 second_best = i;
528 }
529 }
530 /* Basic signal level and twist tests */
531 hit = 0;
532 if (energy[best] >= BELL_MF_THRESHOLD
533 &&
534 energy[second_best] >= BELL_MF_THRESHOLD
535 &&
536 energy[best] < energy[second_best]*BELL_MF_TWIST
537 &&
538 energy[best]*BELL_MF_TWIST > energy[second_best])
539 {
540 /* Relative peak test */
541 hit = 'X';
542 for (i = 0; i < 6; i++)
543 {
544 if (i != best && i != second_best)
545 {
546 if (energy[i]*BELL_MF_RELATIVE_PEAK >= energy[second_best])
547 {
548 /* The best two are not clearly the best */
549 hit = 0;
550 break;
551 }
552 }
553 }
554 }
555 if (hit)
556 {
557 /* Get the values into ascending order */
558 if (second_best < best)
559 {
560 i = best;
561 best = second_best;
562 second_best = i;
563 }
564 best = best*5 + second_best - 1;
565 hit = bell_mf_positions[best];
566 /* Look for two successive similar results */
567 /* The logic in the next test is:
568 For KP we need 4 successive identical clean detects, with
569 two blocks of something different preceeding it. For anything
570 else we need two successive identical clean detects, with
571 two blocks of something different preceeding it. */
572 if (hit == s->hits[4]
573 &&
574 hit == s->hits[3]
575 &&
576 ((hit != '*' && hit != s->hits[2] && hit != s->hits[1])
577 ||
578 (hit == '*' && hit == s->hits[2] && hit != s->hits[1] && hit != s->hits[0])))
579 {
580 if (s->current_digits < MAX_BELL_MF_DIGITS)
581 {
582 s->digits[s->current_digits++] = (char) hit;
583 s->digits[s->current_digits] = '\0';
584 if (s->digits_callback)
585 {
586 s->digits_callback(s->digits_callback_data, s->digits, s->current_digits);
587 s->current_digits = 0;
588 }
589 }
590 else
591 {
592 s->lost_digits++;
593 }
594 }
595 }
596 s->hits[0] = s->hits[1];
597 s->hits[1] = s->hits[2];
598 s->hits[2] = s->hits[3];
599 s->hits[3] = s->hits[4];
600 s->hits[4] = hit;
601 s->current_sample = 0;
602 }
603 if (s->current_digits && s->digits_callback)
604 {
605 s->digits_callback(s->digits_callback_data, s->digits, s->current_digits);
606 s->digits[0] = '\0';
607 s->current_digits = 0;
608 }
609 return 0;
610 }
611 /*- End of function --------------------------------------------------------*/
612
613 SPAN_DECLARE(size_t) bell_mf_rx_get(bell_mf_rx_state_t *s, char *buf, int max)
614 {
615 if (max > s->current_digits)
616 max = s->current_digits;
617 if (max > 0)
618 {
619 memcpy(buf, s->digits, max);
620 memmove(s->digits, s->digits + max, s->current_digits - max);
621 s->current_digits -= max;
622 }
623 buf[max] = '\0';
624 return max;
625 }
626 /*- End of function --------------------------------------------------------*/
627
628 SPAN_DECLARE(bell_mf_rx_state_t *) bell_mf_rx_init(bell_mf_rx_state_t *s,
629 digits_rx_callback_t callback,
630 void *user_data)
631 {
632 int i;
633 static int initialised = FALSE;
634
635 if (s == NULL)
636 {
637 if ((s = (bell_mf_rx_state_t *) malloc(sizeof(*s))) == NULL)
638 return NULL;
639 }
640 memset(s, 0, sizeof(*s));
641
642 if (!initialised)
643 {
644 for (i = 0; i < 6; i++)
645 make_goertzel_descriptor(&bell_mf_detect_desc[i], (float) bell_mf_frequencies[i], BELL_MF_SAMPLES_PER_BLOCK);
646 initialised = TRUE;
647 }
648 s->digits_callback = callback;
649 s->digits_callback_data = user_data;
650
651 s->hits[0] =
652 s->hits[1] =
653 s->hits[2] =
654 s->hits[3] =
655 s->hits[4] = 0;
656
657 for (i = 0; i < 6; i++)
658 goertzel_init(&s->out[i], &bell_mf_detect_desc[i]);
659 s->current_sample = 0;
660 s->lost_digits = 0;
661 s->current_digits = 0;
662 s->digits[0] = '\0';
663 return s;
664 }
665 /*- End of function --------------------------------------------------------*/
666
667 SPAN_DECLARE(int) bell_mf_rx_release(bell_mf_rx_state_t *s)
668 {
669 return 0;
670 }
671 /*- End of function --------------------------------------------------------*/
672
673 SPAN_DECLARE(int) bell_mf_rx_free(bell_mf_rx_state_t *s)
674 {
675 free(s);
676 return 0;
677 }
678 /*- End of function --------------------------------------------------------*/
679
680 SPAN_DECLARE(int) r2_mf_rx(r2_mf_rx_state_t *s, const int16_t amp[], int samples)
681 {
682 #if defined(SPANDSP_USE_FIXED_POINT)
683 int32_t energy[6];
684 int16_t xamp;
685 #else
686 float energy[6];
687 float xamp;
688 #endif
689 int i;
690 int j;
691 int sample;
692 int best;
693 int second_best;
694 int hit;
695 int hit_digit;
696 int limit;
697
698 hit = 0;
699 hit_digit = 0;
700 for (sample = 0; sample < samples; sample = limit)
701 {
702 if ((samples - sample) >= (R2_MF_SAMPLES_PER_BLOCK - s->current_sample))
703 limit = sample + (R2_MF_SAMPLES_PER_BLOCK - s->current_sample);
704 else
705 limit = samples;
706 for (j = sample; j < limit; j++)
707 {
708 xamp = goertzel_preadjust_amp(amp[j]);
709 goertzel_samplex(&s->out[0], xamp);
710 goertzel_samplex(&s->out[1], xamp);
711 goertzel_samplex(&s->out[2], xamp);
712 goertzel_samplex(&s->out[3], xamp);
713 goertzel_samplex(&s->out[4], xamp);
714 goertzel_samplex(&s->out[5], xamp);
715 }
716 s->current_sample += (limit - sample);
717 if (s->current_sample < R2_MF_SAMPLES_PER_BLOCK)
718 continue;
719
720 /* We are at the end of an MF detection block */
721 /* Find the two highest energies */
722 energy[0] = goertzel_result(&s->out[0]);
723 energy[1] = goertzel_result(&s->out[1]);
724 if (energy[0] > energy[1])
725 {
726 best = 0;
727 second_best = 1;
728 }
729 else
730 {
731 best = 1;
732 second_best = 0;
733 }
734
735 for (i = 2; i < 6; i++)
736 {
737 energy[i] = goertzel_result(&s->out[i]);
738 if (energy[i] >= energy[best])
739 {
740 second_best = best;
741 best = i;
742 }
743 else if (energy[i] >= energy[second_best])
744 {
745 second_best = i;
746 }
747 }
748 /* Basic signal level and twist tests */
749 hit = FALSE;
750 if (energy[best] >= R2_MF_THRESHOLD
751 &&
752 energy[second_best] >= R2_MF_THRESHOLD
753 &&
754 energy[best] < energy[second_best]*R2_MF_TWIST
755 &&
756 energy[best]*R2_MF_TWIST > energy[second_best])
757 {
758 /* Relative peak test */
759 hit = TRUE;
760 for (i = 0; i < 6; i++)
761 {
762 if (i != best && i != second_best)
763 {
764 if (energy[i]*R2_MF_RELATIVE_PEAK >= energy[second_best])
765 {
766 /* The best two are not clearly the best */
767 hit = FALSE;
768 break;
769 }
770 }
771 }
772 }
773 if (hit)
774 {
775 /* Get the values into ascending order */
776 if (second_best < best)
777 {
778 i = best;
779 best = second_best;
780 second_best = i;
781 }
782 best = best*5 + second_best - 1;
783 hit_digit = r2_mf_positions[best];
784 }
785 else
786 {
787 hit_digit = 0;
788 }
789 if (s->current_digit != hit_digit && s->callback)
790 {
791 i = (hit_digit) ? -99 : -10;
792 s->callback(s->callback_data, hit_digit, i, 0);
793 }
794 s->current_digit = hit_digit;
795 s->current_sample = 0;
796 }
797 return 0;
798 }
799 /*- End of function --------------------------------------------------------*/
800
801 SPAN_DECLARE(int) r2_mf_rx_get(r2_mf_rx_state_t *s)
802 {
803 return s->current_digit;
804 }
805 /*- End of function --------------------------------------------------------*/
806
807 SPAN_DECLARE(r2_mf_rx_state_t *) r2_mf_rx_init(r2_mf_rx_state_t *s,
808 int fwd,
809 tone_report_func_t callback,
810 void *user_data)
811 {
812 int i;
813 static int initialised = FALSE;
814
815 if (s == NULL)
816 {
817 if ((s = (r2_mf_rx_state_t *) malloc(sizeof(*s))) == NULL)
818 return NULL;
819 }
820 memset(s, 0, sizeof(*s));
821
822 s->fwd = fwd;
823
824 if (!initialised)
825 {
826 for (i = 0; i < 6; i++)
827 {
828 make_goertzel_descriptor(&mf_fwd_detect_desc[i], (float) r2_mf_fwd_frequencies[i], R2_MF_SAMPLES_PER_BLOCK);
829 make_goertzel_descriptor(&mf_back_detect_desc[i], (float) r2_mf_back_frequencies[i], R2_MF_SAMPLES_PER_BLOCK);
830 }
831 initialised = TRUE;
832 }
833 if (fwd)
834 {
835 for (i = 0; i < 6; i++)
836 goertzel_init(&s->out[i], &mf_fwd_detect_desc[i]);
837 }
838 else
839 {
840 for (i = 0; i < 6; i++)
841 goertzel_init(&s->out[i], &mf_back_detect_desc[i]);
842 }
843 s->callback = callback;
844 s->callback_data = user_data;
845 s->current_digit = 0;
846 s->current_sample = 0;
847 return s;
848 }
849 /*- End of function --------------------------------------------------------*/
850
851 SPAN_DECLARE(int) r2_mf_rx_release(r2_mf_rx_state_t *s)
852 {
853 return 0;
854 }
855 /*- End of function --------------------------------------------------------*/
856
857 SPAN_DECLARE(int) r2_mf_rx_free(r2_mf_rx_state_t *s)
858 {
859 free(s);
860 return 0;
861 }
862 /*- End of function --------------------------------------------------------*/
863 /*- End of file ------------------------------------------------------------*/

Repositories maintained by Peter Meerwald, pmeerw@pmeerw.net.