Mercurial > hg > audiostuff

comparison spandsp-0.0.3/spandsp-0.0.3/src/bell_r2_mf.c @ 5:f762bf195c4b

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

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