This source file includes following definitions.
- dsp_fsk_attr_init
- dsp_fsk_attr_get_bithandler
- dsp_fsk_attr_set_bithandler
- dsp_fsk_attr_get_bytehandler
- dsp_fsk_attr_set_bytehandler
- dsp_fsk_attr_get_samplerate
- dsp_fsk_attr_set_samplerate
- dsp_fsk_create
- dsp_fsk_destroy
- dsp_fsk_sample
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 #include <private/ftdm_core.h>
36 #include <stdio.h>
37 #include <stdlib.h>
38 #include <string.h>
39 #include <errno.h>
40 #include <math.h>
41
42 #include "fsk.h"
43 #include "uart.h"
44
45 #ifndef M_PI
46 #define M_PI 3.14159265358979323846
47 #endif
48
49 fsk_modem_definition_t fsk_modem_definitions[] =
50 {
51 { 1700, 1300, 600 },
52 { 2100, 1300, 1200 },
53 { 450, 390, 75 },
54 { 2200, 1200, 1200 },
55 };
56
57
58
59
60
61
62
63
64 void dsp_fsk_attr_init (dsp_fsk_attr_t *attr)
65 {
66 memset(attr, 0, sizeof(*attr));
67 }
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82 bithandler_func_t dsp_fsk_attr_get_bithandler(dsp_fsk_attr_t *attr, void **bithandler_arg)
83 {
84 *bithandler_arg = attr->bithandler_arg;
85 return attr->bithandler;
86 }
87
88 void dsp_fsk_attr_set_bithandler(dsp_fsk_attr_t *attr, bithandler_func_t bithandler, void *bithandler_arg)
89 {
90 attr->bithandler = bithandler;
91 attr->bithandler_arg = bithandler_arg;
92 }
93
94 bytehandler_func_t dsp_fsk_attr_get_bytehandler(dsp_fsk_attr_t *attr, void **bytehandler_arg)
95 {
96 *bytehandler_arg = attr->bytehandler_arg;
97 return attr->bytehandler;
98 }
99
100 void dsp_fsk_attr_set_bytehandler(dsp_fsk_attr_t *attr, bytehandler_func_t bytehandler, void *bytehandler_arg)
101 {
102 attr->bytehandler = bytehandler;
103 attr->bytehandler_arg = bytehandler_arg;
104 }
105
106 int dsp_fsk_attr_get_samplerate (dsp_fsk_attr_t *attr)
107 {
108 return attr->sample_rate;
109 }
110
111 int dsp_fsk_attr_set_samplerate (dsp_fsk_attr_t *attr, int samplerate)
112 {
113 if (samplerate <= 0) {
114 return -1;
115 }
116 attr->sample_rate = samplerate;
117 return 0;
118 }
119
120
121
122
123
124
125
126
127
128
129
130
131 dsp_fsk_handle_t *dsp_fsk_create(dsp_fsk_attr_t *attr)
132 {
133 int i;
134 double phi_mark, phi_space;
135 dsp_fsk_handle_t *handle;
136
137 handle = ftdm_malloc(sizeof(*handle));
138 if (!handle) {
139 return NULL;
140 }
141
142 memset(handle, 0, sizeof(*handle));
143
144
145 memcpy(&handle->attr, attr, sizeof(*attr));
146
147
148 if (attr->sample_rate / fsk_modem_definitions[FSK_BELL202].freq_mark > 6) {
149 handle->downsampling_count = attr->sample_rate / fsk_modem_definitions[FSK_BELL202].freq_mark / 6;
150 } else {
151 handle->downsampling_count = 1;
152 }
153 handle->current_downsample = 1;
154
155
156 handle->corrsize = attr->sample_rate / handle->downsampling_count / fsk_modem_definitions[FSK_BELL202].freq_mark;
157
158
159 for (i = 0; i < 4; i++) {
160 handle->correlates[i] = ftdm_malloc(sizeof(double) * handle->corrsize);
161 if (handle->correlates[i] == NULL) {
162
163 dsp_fsk_destroy(&handle);
164 return NULL;
165 }
166 }
167
168
169 phi_mark = 2. * M_PI / ((double) attr->sample_rate / (double) handle->downsampling_count / (double) fsk_modem_definitions[FSK_BELL202].freq_mark);
170 phi_space = 2. * M_PI / ((double) attr->sample_rate / (double) handle->downsampling_count / (double) fsk_modem_definitions[FSK_BELL202].freq_space);
171
172 for (i = 0; i < handle->corrsize; i++) {
173 handle->correlates[0][i] = sin(phi_mark * (double) i);
174 handle->correlates[1][i] = cos(phi_mark * (double) i);
175 handle->correlates[2][i] = sin(phi_space * (double) i);
176 handle->correlates[3][i] = cos(phi_space * (double) i);
177 }
178
179
180 handle->buffer = ftdm_malloc(sizeof(double) * handle->corrsize);
181 if (!handle->buffer) {
182 dsp_fsk_destroy(&handle);
183 return NULL;
184 }
185 memset(handle->buffer, 0, sizeof(double) * handle->corrsize);
186 handle->ringstart = 0;
187
188
189 handle->cellpos = 0;
190 handle->celladj = fsk_modem_definitions[FSK_BELL202].baud_rate / (double) attr->sample_rate * (double) handle->downsampling_count;
191
192
193 if (handle->attr.bytehandler) {
194 dsp_uart_attr_t uart_attr;
195 dsp_uart_handle_t *uart_handle;
196
197 dsp_uart_attr_init(&uart_attr);
198 dsp_uart_attr_set_bytehandler(&uart_attr, handle->attr.bytehandler, handle->attr.bytehandler_arg);
199 uart_handle = dsp_uart_create(&uart_attr);
200 if (uart_handle == NULL) {
201 dsp_fsk_destroy(&handle);
202 return NULL;
203 }
204 handle->attr.bithandler = dsp_uart_bit_handler;
205 handle->attr.bithandler_arg = uart_handle;
206 }
207
208 return handle;
209 }
210
211
212
213
214
215
216
217
218 void dsp_fsk_destroy(dsp_fsk_handle_t **handle)
219 {
220 int i;
221
222
223 if (*handle == NULL) {
224 return;
225 }
226
227 for (i = 0; i < 4; i++) {
228 if ((*handle)->correlates[i] != NULL) {
229 ftdm_safe_free((*handle)->correlates[i]);
230 (*handle)->correlates[i] = NULL;
231 }
232 }
233
234 if ((*handle)->buffer != NULL) {
235 ftdm_safe_free((*handle)->buffer);
236 (*handle)->buffer = NULL;
237 }
238
239 if ((*handle)->attr.bytehandler) {
240 dsp_uart_handle_t** dhandle = (void *)(&(*handle)->attr.bithandler_arg);
241 dsp_uart_destroy(dhandle);
242 }
243
244 ftdm_safe_free(*handle);
245 *handle = NULL;
246 }
247
248
249
250
251
252
253
254
255
256
257
258
259
260 void
261 dsp_fsk_sample (dsp_fsk_handle_t *handle, double normalized_sample)
262 {
263 double val;
264 double factors[4];
265 int i, j;
266
267
268 if (handle->downsampling_count != 1) {
269 if (handle->current_downsample < handle->downsampling_count) {
270 handle->current_downsample++;
271 return;
272 }
273 handle->current_downsample = 1;
274 }
275
276
277 handle->buffer[handle->ringstart++] = normalized_sample;
278 if (handle->ringstart >= handle->corrsize) {
279 handle->ringstart = 0;
280 }
281
282
283 factors[0] = factors[1] = factors[2] = factors[3] = 0;
284 j = handle->ringstart;
285 for (i = 0; i < handle->corrsize; i++) {
286 if (j >= handle->corrsize) {
287 j = 0;
288 }
289 val = handle->buffer[j];
290 factors[0] += handle->correlates[0][i] * val;
291 factors[1] += handle->correlates[1][i] * val;
292 factors[2] += handle->correlates[2][i] * val;
293 factors[3] += handle->correlates[3][i] * val;
294 j++;
295 }
296
297
298 handle->previous_bit = handle->current_bit;
299 handle->current_bit = (factors[0] * factors[0] + factors[1] * factors[1] > factors[2] * factors[2] + factors[3] * factors[3]);
300
301
302 if (handle->previous_bit != handle->current_bit) {
303 handle->cellpos = 0.5;
304 }
305 handle->cellpos += handle->celladj;
306
307 if (handle->cellpos > 1.0) {
308 handle->cellpos -= 1.0;
309
310 switch (handle->state) {
311 case FSK_STATE_DATA:
312 {
313
314 (*handle->attr.bithandler) (handle->attr.bithandler_arg, handle->current_bit);
315 }
316 break;
317 case FSK_STATE_CHANSEIZE:
318 {
319
320 if (handle->last_bit != handle->current_bit) {
321 handle->conscutive_state_bits++;
322 } else {
323 handle->conscutive_state_bits = 0;
324 }
325
326 if (handle->conscutive_state_bits > 15) {
327 handle->state = FSK_STATE_CARRIERSIG;
328 handle->conscutive_state_bits = 0;
329 }
330 }
331 break;
332 case FSK_STATE_CARRIERSIG:
333 {
334 if (handle->current_bit) {
335 handle->conscutive_state_bits++;
336 } else {
337 handle->conscutive_state_bits = 0;
338 }
339
340 if (handle->conscutive_state_bits > 15) {
341 handle->state = FSK_STATE_DATA;
342 handle->conscutive_state_bits = 0;
343 }
344 }
345 break;
346 }
347
348 handle->last_bit = handle->current_bit;
349 }
350 }
351