summaryrefslogtreecommitdiffstats
path: root/ss5_decode.c
blob: 35e2664e5cef13109e9f3ca24892a167f08b04c4 (plain) (blame)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
/*
 * SS5 signal decoder.
 *
 * Copyright            by Andreas Eversberg (jolly@eversberg.eu)
 *			based on different decoders such as ISDN4Linux
 *			copyright by Karsten Keil
 *
 * This software may be used and distributed according to the terms
 * of the GNU General Public License, incorporated herein by reference.
 *
 */

#include "main.h"
#include "ss5_decode.h"

/* enable level debugging */
//#define DEBUG_LEVELS

#define NCOEFF		8	/* number of frequencies to be analyzed */

#define TONE_MIN_DB	0.01995262 /* -17 db */
#define TONE_DIFF_DB	0.2 // 0.31622777 /*  -5 db */
#define NOISE_MIN_DB	(TONE_MIN_DB / 2) /* noise must be higher than the minimum of two tones */
#define SNR		1.3	/* noise may not exceed signal by that factor */

/* For DTMF recognition:
 * 2 * cos(2 * PI * k / N) precalculated for all k
 */
static signed long long cos2pik[NCOEFF] =
{
	/* k = 2*cos(2*PI*f/8000), k << 15 
	 * 700, 900, 1100, 1300, 1500, 1700, 2400, 2600 */
	55879, 49834, 42562, 34242, 25080, 15299, -20252, -29753
};

/* detection matrix for two frequencies */
static char decode_two[8][8] =
{
	{' ', '1', '2', '4', '7', '*', ' ', ' '}, /* * = code 11 */
	{'1', ' ', '3', '5', '8', '#', ' ', ' '}, /* # = code 12 */
	{'2', '3', ' ', '6', '9', 'a', ' ', ' '}, /* a = KP1 */
	{'4', '5', '6', ' ', '0', 'b', ' ', ' '}, /* b = KP2 */
	{'7', '8', '9', '0', ' ', 'c', ' ', ' '}, /* c = ST */
	{'*', '#', 'a', 'b', 'c', ' ', ' ', ' '},
	{' ', ' ', ' ', ' ', ' ', ' ', ' ', 'C'}, /* C = 2600+2400 */
	{' ', ' ', ' ', ' ', ' ', ' ', 'C', ' '}
};

static char decode_one[8] =
	{' ', ' ', ' ', ' ', ' ', ' ', 'A', 'B'}; /* A = 2400, B = 2600 */
/*
 * calculate the coefficients of the given sample and decode
 */

char ss5_decode(unsigned char *data, int len)
{
	signed short buf[len];
	signed long sk, sk1, sk2, low, high;
	int k, n, i;
	int f1 = 0, f2 = 0;
	double result[NCOEFF], power, noise, snr;
	signed long long cos2pik_;
	char digit = ' ';

	/* convert samples */
	for (i = 0; i < len; i++)
		buf[i] = audio_law_to_s32[*data++];

	/* now we do noise level calculation */
	low = 32767;
	high = -32768;
	for (n = 0; n < len; n++) {
		sk = buf[n];
		if (sk < low)
			low = sk;
		if (sk > high)
			high = sk;
	}
	noise = ((double)(high-low) / 65536.0);

	/* check for minimum noise, or tone detection will not be necessary */
	if (noise < NOISE_MIN_DB)
		return digit;

	/* now we have a full buffer of signed long samples - we do goertzel */
	for (k = 0; k < NCOEFF; k++) {
		sk = 0;
		sk1 = 0;
		sk2 = 0;
		cos2pik_ = cos2pik[k];
		for (n = 0; n < len; n++) {
			sk = ((cos2pik_*sk1)>>15) - sk2 + buf[n];
			sk2 = sk1;
			sk1 = sk;
		}
		sk >>= 8;
		sk2 >>= 8;
		if (sk > 32767 || sk < -32767 || sk2 > 32767 || sk2 < -32767)
			PERROR("Tone-Detection overflow\n");
		/* compute |X(k)|**2 */
		result[k] = sqrt (
				(sk * sk) -
				(((cos2pik[k] * sk) >> 15) * sk2) +
				(sk2 * sk2)
			) / len / 62; /* level of 1 is 0 db*/
	}

	/* find the two loudest frequencies + one less lower frequency to detect noise */
	power = 0.0;
	for (i = 0; i < NCOEFF; i++) {
		if (result[i] > power) {
			power = result[i];
			f1 = i;
		}
	}
	power = 0.0;
	for (i = 0; i < NCOEFF; i++) {
		if (i != f1 && result[i] > power) {
			power = result[i];
			f2 = i;
		}
	}

	snr = 0;
	/* check one frequency */
	if (result[f1] > TONE_MIN_DB /* must be at least -17 db */
	 && result[f1]*SNR > noise) { /*  */
		digit = decode_one[f1];
		if (digit != ' ')
			snr = result[f1] / noise;
	}
	/* check two frequencies */
	if (result[f1] > TONE_MIN_DB && result[f2] > TONE_MIN_DB /* must be at lease -17 db */
	 && result[f1]*TONE_DIFF_DB <= result[f2] /* f2 must be not less than 5 db below f1 */
	 && (result[f1]+result[f2])*SNR > noise) { /* */
		digit = decode_two[f1][f2];
		if (digit != ' ')
			snr = (result[f1]+result[f2]) / noise;
	}

	/* debug powers */
#ifdef DEBUG_LEVELS
	for (i = 0; i < NCOEFF; i++)
		printf("%d:%3d %c ", i, (int)(result[i]*100), (f1==i || f2==i)?'*':' ');
	printf("N:%3d digit:%c snr=%3d\n", (int)(noise*100), digit, (int)(snr*100));
// 	if (result[f1]*TONE_DIFF_DB <= result[f2]) /* f2 must be not less than 5 db below f1 */
//		printf("jo!");
#endif

	return digit;
}

void ss5_test_decode(void)
{
#ifdef DEBUG_LEVELS
	double phase;
	int i, j;
	signed short sample;

	unsigned char buffer[SS5_DECODER_NPOINTS];
	for (i = 0; i < 4000; i += 10) {
		phase = 2.0 * 3.14159265 * i / 8000.0;
		for (j = 0; j < SS5_DECODER_NPOINTS; j++) {
			sample = sin(phase * j) * 1000;
			buffer[j] = audio_s16_to_law[sample & 0xffff];
		}
		printf("FRQ:%04d:", i);
		ss5_decode(buffer, SS5_DECODER_NPOINTS);
	}
#endif
}