Skip to content

Commit 63aa45d

Browse files
AlexandreRoumaAlexandreRouma
committed
beginning of new pager clock recovery
1 parent c0a84f8 commit 63aa45d

File tree

3 files changed

+263
-7
lines changed

3 files changed

+263
-7
lines changed

‎decoder_modules/pager_decoder/src/pocsag/decoder.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313

1414
class POCSAGDecoder : public Decoder {
1515
public:
16-
POCSAGDecoder(const std::string& name, VFOManager::VFO* vfo) : diag(0.6, BAUDRATE) {
16+
POCSAGDecoder(const std::string& name, VFOManager::VFO* vfo) : diag(0.6, 320) {
1717
this->name = name;
1818
this->vfo = vfo;
1919

@@ -26,7 +26,7 @@ class POCSAGDecoder : public Decoder {
2626
vfo->setBandwidthLimits(12500, 12500, true);
2727
vfo->setSampleRate(SAMPLERATE, 12500);
2828
dsp.init(vfo->output, SAMPLERATE, BAUDRATE);
29-
reshape.init(&dsp.soft, BAUDRATE, (BAUDRATE / 30.0) - BAUDRATE);
29+
reshape.init(&dsp.soft, 320, 0);
3030
dataHandler.init(&dsp.out, _dataHandler, this);
3131
diagHandler.init(&reshape.out, _diagHandler, this);
3232

‎decoder_modules/pager_decoder/src/pocsag/dsp.h‎

Lines changed: 97 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,89 @@
1111
#include <dsp/digital/binary_slicer.h>
1212
#include <dsp/routing/doubler.h>
1313

14+
#include "packet_clock_sync.h"
15+
16+
inline float PATTERN_DSDSDZED[] = {
17+
-1.00000000e+00, -8.00000000e-01, -6.00000000e-01, -4.00000000e-01,
18+
-2.00000000e-01, -2.77555756e-17, 2.00000000e-01, 4.00000000e-01,
19+
6.00000000e-01, 8.00000000e-01, 1.00000000e+00, 1.00000000e+00,
20+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
21+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
22+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
23+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
24+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
25+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
26+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
27+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
28+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
29+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 8.00000000e-01,
30+
6.00000000e-01, 4.00000000e-01, 2.00000000e-01, 2.77555756e-17,
31+
-2.00000000e-01, -4.00000000e-01, -6.00000000e-01, -8.00000000e-01,
32+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
33+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
34+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -8.00000000e-01,
35+
-6.00000000e-01, -4.00000000e-01, -2.00000000e-01, -2.77555756e-17,
36+
2.00000000e-01, 4.00000000e-01, 6.00000000e-01, 8.00000000e-01,
37+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
38+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
39+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 8.00000000e-01,
40+
6.00000000e-01, 4.00000000e-01, 2.00000000e-01, 2.77555756e-17,
41+
-2.00000000e-01, -4.00000000e-01, -6.00000000e-01, -8.00000000e-01,
42+
-1.00000000e+00, -8.00000000e-01, -6.00000000e-01, -4.00000000e-01,
43+
-2.00000000e-01, -2.77555756e-17, 2.00000000e-01, 4.00000000e-01,
44+
6.00000000e-01, 8.00000000e-01, 1.00000000e+00, 8.00000000e-01,
45+
6.00000000e-01, 4.00000000e-01, 2.00000000e-01, 2.77555756e-17,
46+
-2.00000000e-01, -4.00000000e-01, -6.00000000e-01, -8.00000000e-01,
47+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
48+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
49+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -8.00000000e-01,
50+
-6.00000000e-01, -4.00000000e-01, -2.00000000e-01, -2.77555756e-17,
51+
2.00000000e-01, 4.00000000e-01, 6.00000000e-01, 8.00000000e-01,
52+
1.00000000e+00, 8.00000000e-01, 6.00000000e-01, 4.00000000e-01,
53+
2.00000000e-01, 2.77555756e-17, -2.00000000e-01, -4.00000000e-01,
54+
-6.00000000e-01, -8.00000000e-01, -1.00000000e+00, -1.00000000e+00,
55+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
56+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
57+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
58+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
59+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
60+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
61+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
62+
-1.00000000e+00, -8.00000000e-01, -6.00000000e-01, -4.00000000e-01,
63+
-2.00000000e-01, -2.77555756e-17, 2.00000000e-01, 4.00000000e-01,
64+
6.00000000e-01, 8.00000000e-01, 1.00000000e+00, 8.00000000e-01,
65+
6.00000000e-01, 4.00000000e-01, 2.00000000e-01, 2.77555756e-17,
66+
-2.00000000e-01, -4.00000000e-01, -6.00000000e-01, -8.00000000e-01,
67+
-1.00000000e+00, -8.00000000e-01, -6.00000000e-01, -4.00000000e-01,
68+
-2.00000000e-01, -2.77555756e-17, 2.00000000e-01, 4.00000000e-01,
69+
6.00000000e-01, 8.00000000e-01, 1.00000000e+00, 8.00000000e-01,
70+
6.00000000e-01, 4.00000000e-01, 2.00000000e-01, 2.77555756e-17,
71+
-2.00000000e-01, -4.00000000e-01, -6.00000000e-01, -8.00000000e-01,
72+
-1.00000000e+00, -8.00000000e-01, -6.00000000e-01, -4.00000000e-01,
73+
-2.00000000e-01, -2.77555756e-17, 2.00000000e-01, 4.00000000e-01,
74+
6.00000000e-01, 8.00000000e-01, 1.00000000e+00, 1.00000000e+00,
75+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
76+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
77+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
78+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
79+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 8.00000000e-01,
80+
6.00000000e-01, 4.00000000e-01, 2.00000000e-01, 2.77555756e-17,
81+
-2.00000000e-01, -4.00000000e-01, -6.00000000e-01, -8.00000000e-01,
82+
-1.00000000e+00, -8.00000000e-01, -6.00000000e-01, -4.00000000e-01,
83+
-2.00000000e-01, -2.77555756e-17, 2.00000000e-01, 4.00000000e-01,
84+
6.00000000e-01, 8.00000000e-01, 1.00000000e+00, 1.00000000e+00,
85+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
86+
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
87+
1.00000000e+00, 8.00000000e-01, 6.00000000e-01, 4.00000000e-01,
88+
2.00000000e-01, 2.77555756e-17, -2.00000000e-01, -4.00000000e-01,
89+
-6.00000000e-01, -8.00000000e-01, -1.00000000e+00, -1.00000000e+00,
90+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
91+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
92+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
93+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
94+
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00
95+
};
96+
1497
class POCSAGDSP : public dsp::Processor<dsp::complex_t, uint8_t> {
1598
using base_type = dsp::Processor<dsp::complex_t, uint8_t>;
1699
public:
@@ -27,7 +110,9 @@ class POCSAGDSP : public dsp::Processor<dsp::complex_t, uint8_t> {
27110
float taps[] = { 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f };
28111
shape = dsp::taps::fromArray<float>(10, taps);
29112
fir.init(NULL, shape);
30-
recov.init(NULL, samplerate/baudrate, 1e-4, 1.0, 0.05);
113+
//recov.init(NULL, samplerate/baudrate, 1e-4, 1.0, 0.05);
114+
115+
cs.init(NULL, PATTERN_DSDSDZED, sizeof(PATTERN_DSDSDZED)/sizeof(float), 0, 10);
31116

32117
// Free useless buffers
33118
// dcBlock.out.free();
@@ -42,8 +127,11 @@ class POCSAGDSP : public dsp::Processor<dsp::complex_t, uint8_t> {
42127
count = demod.process(count, in, demod.out.readBuf);
43128
//count = dcBlock.process(count, demod.out.readBuf, demod.out.readBuf);
44129
count = fir.process(count, demod.out.readBuf, demod.out.readBuf);
45-
count = recov.process(count, demod.out.readBuf, softOut);
46-
dsp::digital::BinarySlicer::process(count, softOut, out);
130+
//count = recov.process(count, demod.out.readBuf, softOut);
131+
132+
count = cs.process(count, demod.out.readBuf, softOut);
133+
134+
//dsp::digital::BinarySlicer::process(count, softOut, out);
47135
return count;
48136
}
49137

@@ -58,8 +146,10 @@ class POCSAGDSP : public dsp::Processor<dsp::complex_t, uint8_t> {
58146
count = process(count, base_type::_in->readBuf, soft.writeBuf, base_type::out.writeBuf);
59147

60148
base_type::_in->flush();
61-
if (!base_type::out.swap(count)) { return -1; }
62-
if (!soft.swap(count)) { return -1; }
149+
//if (!base_type::out.swap(count)) { return -1; }
150+
151+
if (count) { if (!soft.swap(count)) { return -1; } }
152+
63153
return count;
64154
}
65155

@@ -72,4 +162,6 @@ class POCSAGDSP : public dsp::Processor<dsp::complex_t, uint8_t> {
72162
dsp::filter::FIR<float, float> fir;
73163
dsp::clock_recovery::MM<float> recov;
74164

165+
dsp::PacketClockSync cs;
166+
75167
};

‎decoder_modules/pager_decoder/src/pocsag/packet_clock_sync.h‎

Lines changed: 164 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,164 @@
1+
#pragma once
2+
#include <dsp/stream.h>
3+
#include <dsp/buffer/reshaper.h>
4+
#include <dsp/multirate/rational_resampler.h>
5+
#include <dsp/sink/handler_sink.h>
6+
#include <dsp/demod/quadrature.h>
7+
#include <dsp/clock_recovery/mm.h>
8+
#include <dsp/taps/root_raised_cosine.h>
9+
#include <dsp/correction/dc_blocker.h>
10+
#include <dsp/loop/fast_agc.h>
11+
#include <dsp/digital/binary_slicer.h>
12+
#include <dsp/routing/doubler.h>
13+
#include <utils/flog.h>
14+
#include <fftw3.h>
15+
#include <dsp/math/conjugate.h>
16+
#include <dsp/math/normalize_phase.h>
17+
18+
namespace dsp {
19+
class PacketClockSync : public dsp::Processor<float, float> {
20+
using base_type = dsp::Processor<float, float>;
21+
public:
22+
PacketClockSync() {}
23+
PacketClockSync(dsp::stream<float>* in, float* pattern, int patternLen, int frameLen, float sampsPerSym, float threshold = 0.4f) { init(in, pattern, patternLen, frameLen, sampsPerSym, threshold); }
24+
25+
// TODO: Free in destroyer
26+
27+
void init(dsp::stream<float>* in, float* pattern, int patternLen, int frameLen, float sampsPerSym, float threshold = 0.4f) {
28+
// Compute the required FFT size and associated delay length
29+
fftSize = 512;// TODO: Find smallest power of 2 that fits patternLen
30+
delayLen = fftSize - 1;
31+
32+
// Allocate buffers
33+
buffer = dsp::buffer::alloc<float>(STREAM_BUFFER_SIZE+delayLen);
34+
bufferStart = &buffer[delayLen];
35+
this->pattern = dsp::buffer::alloc<float>(patternLen);
36+
patternFFT = dsp::buffer::alloc<complex_t>(fftSize);
37+
patternFFTAmps = dsp::buffer::alloc<float>(fftSize);
38+
fftIn = fftwf_alloc_real(fftSize);
39+
fftOut = (complex_t*)fftwf_alloc_complex(fftSize);
40+
41+
// Copy parameters
42+
memcpy(this->pattern, pattern, patternLen*sizeof(float));
43+
this->sampsPerSym = sampsPerSym;
44+
this->threshold = threshold;
45+
this->patternLen = patternLen;
46+
47+
// Plan FFT
48+
plan = fftwf_plan_dft_r2c_1d(fftSize, fftIn, (fftwf_complex*)fftOut, FFTW_ESTIMATE);
49+
50+
// Pre-compute pattern conjugated FFT
51+
// TODO: Offset the pattern to avoid it being cut off (EXTREMELY IMPORTANT)
52+
memcpy(fftIn, pattern, patternLen*sizeof(float));
53+
memset(&fftIn[patternLen], 0, (fftSize-patternLen)*sizeof(float));
54+
fftwf_execute(plan);
55+
volk_32fc_conjugate_32fc((lv_32fc_t*)patternFFT, (lv_32fc_t*)fftOut, fftSize);
56+
57+
// Compute amplitudes of the pattern FFT
58+
volk_32fc_magnitude_32f(patternFFTAmps, (lv_32fc_t*)patternFFT, fftSize);
59+
60+
// Normalize the amplitudes
61+
float maxAmp = 0.0f;
62+
for (int i = 0; i < patternLen; i++) {
63+
if (patternFFTAmps[i] > maxAmp) { maxAmp = patternFFTAmps[i]; }
64+
}
65+
volk_32f_s32f_multiply_32f(patternFFTAmps, patternFFTAmps, 1.0f/maxAmp, fftSize);
66+
67+
// Init base
68+
base_type::init(in);
69+
}
70+
71+
int process(int count, float* in, float* out) {
72+
// Copy to buffer
73+
memcpy(bufferStart, in, count * sizeof(float));
74+
75+
int outCount = 0;
76+
bool first = true;
77+
78+
for (int i = 0; i < count; i++) {
79+
// Measure correlation to the sync pattern
80+
float corr;
81+
volk_32f_x2_dot_prod_32f(&corr, &buffer[i], pattern, patternLen);
82+
83+
// If not correlated enough, go to next sample. Otherwise continue with fine detection
84+
if (corr/(float)patternLen < threshold) { continue; }
85+
86+
// Copy samples into FFT input (only the part where we think the pattern is located)
87+
// TODO: Instead, check the interval onto which correlation occurs to determine where the pattern is located (IMPORTANT)
88+
memcpy(fftIn, &buffer[i], patternLen*sizeof(float));
89+
memset(&fftIn[patternLen], 0, (fftSize-patternLen)*sizeof(float)); // TODO, figure out why we need this
90+
91+
// Compute FFT
92+
fftwf_execute(plan);
93+
94+
// Multiply with the conjugated pattern FFT to get the phase offset at each frequency
95+
volk_32fc_x2_multiply_32fc((lv_32fc_t*)fftOut, (lv_32fc_t*)fftOut, (lv_32fc_t*)patternFFT, fftSize);
96+
97+
// Compute the average phase delay rate
98+
float last = 0;
99+
float rateIntegral = 0;
100+
for (int j = 1; j < fftSize/2; j++) {
101+
// Compute instantanous rate
102+
float currentPhase = fftOut[j].phase();
103+
float instantRate = dsp::math::normalizePhase(currentPhase - last);
104+
last = currentPhase;
105+
106+
// Compute current rate guess
107+
float rateGuess = rateIntegral / (float)j;
108+
109+
// Update the rate integral as a weighted average of the current guess and measured rate depending on pattern amplitude
110+
rateIntegral += patternFFTAmps[j]*instantRate + (1.0f-patternFFTAmps[j])*rateGuess;
111+
}
112+
float avgRate = 1.14f*rateIntegral/(float)(fftSize/2);
113+
114+
// Compute the total offset
115+
float offset = (float)i - avgRate*(float)fftSize/(2.0f*FL_M_PI);
116+
117+
if (first) {
118+
outCount = 320;
119+
memcpy(out, &buffer[(int)roundf(offset)], 320*sizeof(float));
120+
first = false;
121+
}
122+
123+
124+
flog::debug("Detected: {} -> {} ({})", i, offset, avgRate);
125+
}
126+
127+
// Move unused data
128+
memmove(buffer, &buffer[count], delayLen * sizeof(float));
129+
130+
return outCount;
131+
}
132+
133+
int run() {
134+
int count = base_type::_in->read();
135+
if (count < 0) { return -1; }
136+
137+
count = process(count, base_type::_in->readBuf, base_type::out.writeBuf);
138+
139+
base_type::_in->flush();
140+
//if (!base_type::out.swap(count)) { return -1; }
141+
return count;
142+
}
143+
144+
private:
145+
int delayLen;
146+
float* buffer = NULL;
147+
float* bufferStart = NULL;
148+
float* pattern = NULL;
149+
int patternLen;
150+
bool locked;
151+
int fftSize;
152+
153+
float threshold;
154+
155+
float* fftIn = NULL;
156+
complex_t* fftOut = NULL;
157+
fftwf_plan plan;
158+
159+
complex_t* patternFFT;
160+
float* patternFFTAmps;
161+
162+
float sampsPerSym;
163+
};
164+
}

0 commit comments

Comments
 (0)