-
Notifications
You must be signed in to change notification settings - Fork 1
/
fsk.c
143 lines (112 loc) · 3.71 KB
/
fsk.c
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
#include "main.h"
// Lots of stuff here is from rtl_fm
// Am I excused if I understood what's going on ? :D
// Complex multiply (ar,aj)*(br,bj) into (cr,cj)
void multiply(int ar, int aj, int br, int bj, int *cr, int *cj) {
*cr = ar*br - aj*bj;
*cj = aj*br + ar*bj;
}
// Return rotation direction
int rot_direction(int ar, int aj, int br, int bj) {
int cr, cj;
double angle;
multiply(ar, aj, br, -bj, &cr, &cj);
angle = atan2((double)cj, (double)cr);
return (angle < 0.0 ? -1 : 1);
}
void process_dir(struct fm_state *fm, int dir) {
// We need to sample dir right in the middle of each bit
// We're using bit_phase (u16) to time this
// Sampling must be done at 0x10000
// Bit transitions must be at 0x8000
// The phase accumulator is inc'd by the phase delta each sample so that the phase acc wraps at 1200bps
// When the bit changes, see how far from 0 we are to adjust the phase accumulator
// Since we're running at 20000sps, an fsk bit lasts 16.7 samples
// So the phase delta is 0x10000 / 16.7 = 3932
if (dir != fm->dir_current) {
// State change
fm->dir_current = dir;
if (fm->phase_acc < 0x8000U-(3932/2))
fm->phase_acc += (3932 / 8); // Lags
else if (fm->phase_acc > 0x8000U+(3932/2))
fm->phase_acc -= (3932 / 8); // Leads
else
fm->bit_counter++; // DEBUG
}
fm->phase_acc += 3932; // TODO: Change sampling rate to get integer here
if (fm->phase_acc >= 0x10000U) {
fm->phase_acc &= 0xFFFFU;
fm->sr_data <<= 1;
fm->sr_data |= (dir > 0 ? 0 : 1);
if (fm->sr_data == 0x7CD215D8) {
printf("Got sync on channel %u!\n", scan_index);
snprintf(txt_buffer, 128, "Got sync on channel %u at %u", scan_index, (uint32_t)time(NULL));
send_dm(txt_buffer);
fm->sync_status = 2;
fm->timeout = 1000;
//} else if (fm->sr_data == 0x7A89C197) {
//printf("Got idle !\n");
//fm->sync_status = 3;
} else if (fm->sr_data == 0xAAAAAAAA) {
// 32 bits should last (32/1200)*20000 = 533 samples
if (fm->sync_status == 0)
printf("Got preamble on channel %u!\n", scan_index);
fm->sync_status = 1;
fm->timeout = 1000;
}
}
if (!fm->timeout)
fm->sync_status = 0;
else
fm->timeout--;
}
void full_demod(struct fm_state *fm) {
// Here, we get a buffer full of unsigned 8bit I/Q samples
// The POCSAG signal is at -samplerate/4 (= -75000) Hz
// Rotating by +90° makes the signal appear at 0Hz
// It is downsampled by averaging to 20ksps before being demodulated
uint32_t i, i2 = 0;
uint8_t tmp;
int dir;
uint8_t* buf = fm->buf;
uint32_t len = fm->buf_len;
// Rotate +90°:
for (i = 0; i < len; i += 8) { // 4 samples
tmp = 255 - buf[i + 3];
buf[i + 3] = buf[i + 2];
buf[i + 2] = tmp;
buf[i + 4] = 255 - buf[i + 4];
buf[i + 5] = 255 - buf[i + 5];
tmp = 255 - buf[i + 6];
buf[i + 6] = buf[i + 7];
buf[i + 7] = tmp;
}
// Downsample:
i = 0;
while (i < len) {
// Unsigned to signed
fm->now_r += ((int)buf[i] - 128); // Accumulate
fm->now_j += ((int)buf[i + 1] - 128);
i += 2;
fm->prev_index++;
if (fm->prev_index < fm->downsample)
continue; // Skip for downsampling
fm->signal[i2] = fm->now_r * fm->output_scale;
fm->signal[i2+1] = fm->now_j * fm->output_scale;
fm->prev_index = 0;
fm->now_r = 0;
fm->now_j = 0;
i2 += 2;
}
fm->signal_len = i2;
// Demodulate FSK by just figuring out the direction of rotation
dir = rot_direction(fm->signal[0], fm->signal[1], fm->pre_r, fm->pre_j);
process_dir(fm, dir);
// We're running at 300000 / 15 = 20ksps here
for (i = 2; i < (fm->signal_len); i += 2) {
dir = rot_direction(fm->signal[i], fm->signal[i+1], fm->signal[i-2], fm->signal[i-1]);
process_dir(fm, dir);
}
fm->pre_r = fm->signal[fm->signal_len - 2];
fm->pre_j = fm->signal[fm->signal_len - 1];
}