kopia lustrzana https://github.com/AlexandreRouma/SDRPlusPlus
beginning of new pager clock recovery
AlexandreRouma
rodzic
c0a84f8703
commit
63aa45de9e
|
|
@ -13,7 +13,7 @@
|
|||
|
||||
class POCSAGDecoder : public Decoder {
|
||||
public:
|
||||
POCSAGDecoder(const std::string& name, VFOManager::VFO* vfo) : diag(0.6, BAUDRATE) {
|
||||
POCSAGDecoder(const std::string& name, VFOManager::VFO* vfo) : diag(0.6, 320) {
|
||||
this->name = name;
|
||||
this->vfo = vfo;
|
||||
|
||||
|
|
@ -26,7 +26,7 @@ public:
|
|||
vfo->setBandwidthLimits(12500, 12500, true);
|
||||
vfo->setSampleRate(SAMPLERATE, 12500);
|
||||
dsp.init(vfo->output, SAMPLERATE, BAUDRATE);
|
||||
reshape.init(&dsp.soft, BAUDRATE, (BAUDRATE / 30.0) - BAUDRATE);
|
||||
reshape.init(&dsp.soft, 320, 0);
|
||||
dataHandler.init(&dsp.out, _dataHandler, this);
|
||||
diagHandler.init(&reshape.out, _diagHandler, this);
|
||||
|
||||
|
|
|
|||
|
|
@ -11,6 +11,89 @@
|
|||
#include <dsp/digital/binary_slicer.h>
|
||||
#include <dsp/routing/doubler.h>
|
||||
|
||||
#include "packet_clock_sync.h"
|
||||
|
||||
inline float PATTERN_DSDSDZED[] = {
|
||||
-1.00000000e+00, -8.00000000e-01, -6.00000000e-01, -4.00000000e-01,
|
||||
-2.00000000e-01, -2.77555756e-17, 2.00000000e-01, 4.00000000e-01,
|
||||
6.00000000e-01, 8.00000000e-01, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 8.00000000e-01,
|
||||
6.00000000e-01, 4.00000000e-01, 2.00000000e-01, 2.77555756e-17,
|
||||
-2.00000000e-01, -4.00000000e-01, -6.00000000e-01, -8.00000000e-01,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -8.00000000e-01,
|
||||
-6.00000000e-01, -4.00000000e-01, -2.00000000e-01, -2.77555756e-17,
|
||||
2.00000000e-01, 4.00000000e-01, 6.00000000e-01, 8.00000000e-01,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 8.00000000e-01,
|
||||
6.00000000e-01, 4.00000000e-01, 2.00000000e-01, 2.77555756e-17,
|
||||
-2.00000000e-01, -4.00000000e-01, -6.00000000e-01, -8.00000000e-01,
|
||||
-1.00000000e+00, -8.00000000e-01, -6.00000000e-01, -4.00000000e-01,
|
||||
-2.00000000e-01, -2.77555756e-17, 2.00000000e-01, 4.00000000e-01,
|
||||
6.00000000e-01, 8.00000000e-01, 1.00000000e+00, 8.00000000e-01,
|
||||
6.00000000e-01, 4.00000000e-01, 2.00000000e-01, 2.77555756e-17,
|
||||
-2.00000000e-01, -4.00000000e-01, -6.00000000e-01, -8.00000000e-01,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -8.00000000e-01,
|
||||
-6.00000000e-01, -4.00000000e-01, -2.00000000e-01, -2.77555756e-17,
|
||||
2.00000000e-01, 4.00000000e-01, 6.00000000e-01, 8.00000000e-01,
|
||||
1.00000000e+00, 8.00000000e-01, 6.00000000e-01, 4.00000000e-01,
|
||||
2.00000000e-01, 2.77555756e-17, -2.00000000e-01, -4.00000000e-01,
|
||||
-6.00000000e-01, -8.00000000e-01, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -8.00000000e-01, -6.00000000e-01, -4.00000000e-01,
|
||||
-2.00000000e-01, -2.77555756e-17, 2.00000000e-01, 4.00000000e-01,
|
||||
6.00000000e-01, 8.00000000e-01, 1.00000000e+00, 8.00000000e-01,
|
||||
6.00000000e-01, 4.00000000e-01, 2.00000000e-01, 2.77555756e-17,
|
||||
-2.00000000e-01, -4.00000000e-01, -6.00000000e-01, -8.00000000e-01,
|
||||
-1.00000000e+00, -8.00000000e-01, -6.00000000e-01, -4.00000000e-01,
|
||||
-2.00000000e-01, -2.77555756e-17, 2.00000000e-01, 4.00000000e-01,
|
||||
6.00000000e-01, 8.00000000e-01, 1.00000000e+00, 8.00000000e-01,
|
||||
6.00000000e-01, 4.00000000e-01, 2.00000000e-01, 2.77555756e-17,
|
||||
-2.00000000e-01, -4.00000000e-01, -6.00000000e-01, -8.00000000e-01,
|
||||
-1.00000000e+00, -8.00000000e-01, -6.00000000e-01, -4.00000000e-01,
|
||||
-2.00000000e-01, -2.77555756e-17, 2.00000000e-01, 4.00000000e-01,
|
||||
6.00000000e-01, 8.00000000e-01, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 8.00000000e-01,
|
||||
6.00000000e-01, 4.00000000e-01, 2.00000000e-01, 2.77555756e-17,
|
||||
-2.00000000e-01, -4.00000000e-01, -6.00000000e-01, -8.00000000e-01,
|
||||
-1.00000000e+00, -8.00000000e-01, -6.00000000e-01, -4.00000000e-01,
|
||||
-2.00000000e-01, -2.77555756e-17, 2.00000000e-01, 4.00000000e-01,
|
||||
6.00000000e-01, 8.00000000e-01, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 1.00000000e+00, 1.00000000e+00, 1.00000000e+00,
|
||||
1.00000000e+00, 8.00000000e-01, 6.00000000e-01, 4.00000000e-01,
|
||||
2.00000000e-01, 2.77555756e-17, -2.00000000e-01, -4.00000000e-01,
|
||||
-6.00000000e-01, -8.00000000e-01, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00, -1.00000000e+00,
|
||||
-1.00000000e+00, -1.00000000e+00, -1.00000000e+00
|
||||
};
|
||||
|
||||
class POCSAGDSP : public dsp::Processor<dsp::complex_t, uint8_t> {
|
||||
using base_type = dsp::Processor<dsp::complex_t, uint8_t>;
|
||||
public:
|
||||
|
|
@ -27,7 +110,9 @@ public:
|
|||
float taps[] = { 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f };
|
||||
shape = dsp::taps::fromArray<float>(10, taps);
|
||||
fir.init(NULL, shape);
|
||||
recov.init(NULL, samplerate/baudrate, 1e-4, 1.0, 0.05);
|
||||
//recov.init(NULL, samplerate/baudrate, 1e-4, 1.0, 0.05);
|
||||
|
||||
cs.init(NULL, PATTERN_DSDSDZED, sizeof(PATTERN_DSDSDZED)/sizeof(float), 0, 10);
|
||||
|
||||
// Free useless buffers
|
||||
// dcBlock.out.free();
|
||||
|
|
@ -42,8 +127,11 @@ public:
|
|||
count = demod.process(count, in, demod.out.readBuf);
|
||||
//count = dcBlock.process(count, demod.out.readBuf, demod.out.readBuf);
|
||||
count = fir.process(count, demod.out.readBuf, demod.out.readBuf);
|
||||
count = recov.process(count, demod.out.readBuf, softOut);
|
||||
dsp::digital::BinarySlicer::process(count, softOut, out);
|
||||
//count = recov.process(count, demod.out.readBuf, softOut);
|
||||
|
||||
count = cs.process(count, demod.out.readBuf, softOut);
|
||||
|
||||
//dsp::digital::BinarySlicer::process(count, softOut, out);
|
||||
return count;
|
||||
}
|
||||
|
||||
|
|
@ -58,8 +146,10 @@ public:
|
|||
count = process(count, base_type::_in->readBuf, soft.writeBuf, base_type::out.writeBuf);
|
||||
|
||||
base_type::_in->flush();
|
||||
if (!base_type::out.swap(count)) { return -1; }
|
||||
if (!soft.swap(count)) { return -1; }
|
||||
//if (!base_type::out.swap(count)) { return -1; }
|
||||
|
||||
if (count) { if (!soft.swap(count)) { return -1; } }
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
|
|
@ -72,4 +162,6 @@ private:
|
|||
dsp::filter::FIR<float, float> fir;
|
||||
dsp::clock_recovery::MM<float> recov;
|
||||
|
||||
dsp::PacketClockSync cs;
|
||||
|
||||
};
|
||||
|
|
@ -0,0 +1,164 @@
|
|||
#pragma once
|
||||
#include <dsp/stream.h>
|
||||
#include <dsp/buffer/reshaper.h>
|
||||
#include <dsp/multirate/rational_resampler.h>
|
||||
#include <dsp/sink/handler_sink.h>
|
||||
#include <dsp/demod/quadrature.h>
|
||||
#include <dsp/clock_recovery/mm.h>
|
||||
#include <dsp/taps/root_raised_cosine.h>
|
||||
#include <dsp/correction/dc_blocker.h>
|
||||
#include <dsp/loop/fast_agc.h>
|
||||
#include <dsp/digital/binary_slicer.h>
|
||||
#include <dsp/routing/doubler.h>
|
||||
#include <utils/flog.h>
|
||||
#include <fftw3.h>
|
||||
#include <dsp/math/conjugate.h>
|
||||
#include <dsp/math/normalize_phase.h>
|
||||
|
||||
namespace dsp {
|
||||
class PacketClockSync : public dsp::Processor<float, float> {
|
||||
using base_type = dsp::Processor<float, float>;
|
||||
public:
|
||||
PacketClockSync() {}
|
||||
PacketClockSync(dsp::stream<float>* in, float* pattern, int patternLen, int frameLen, float sampsPerSym, float threshold = 0.4f) { init(in, pattern, patternLen, frameLen, sampsPerSym, threshold); }
|
||||
|
||||
// TODO: Free in destroyer
|
||||
|
||||
void init(dsp::stream<float>* in, float* pattern, int patternLen, int frameLen, float sampsPerSym, float threshold = 0.4f) {
|
||||
// Compute the required FFT size and associated delay length
|
||||
fftSize = 512;// TODO: Find smallest power of 2 that fits patternLen
|
||||
delayLen = fftSize - 1;
|
||||
|
||||
// Allocate buffers
|
||||
buffer = dsp::buffer::alloc<float>(STREAM_BUFFER_SIZE+delayLen);
|
||||
bufferStart = &buffer[delayLen];
|
||||
this->pattern = dsp::buffer::alloc<float>(patternLen);
|
||||
patternFFT = dsp::buffer::alloc<complex_t>(fftSize);
|
||||
patternFFTAmps = dsp::buffer::alloc<float>(fftSize);
|
||||
fftIn = fftwf_alloc_real(fftSize);
|
||||
fftOut = (complex_t*)fftwf_alloc_complex(fftSize);
|
||||
|
||||
// Copy parameters
|
||||
memcpy(this->pattern, pattern, patternLen*sizeof(float));
|
||||
this->sampsPerSym = sampsPerSym;
|
||||
this->threshold = threshold;
|
||||
this->patternLen = patternLen;
|
||||
|
||||
// Plan FFT
|
||||
plan = fftwf_plan_dft_r2c_1d(fftSize, fftIn, (fftwf_complex*)fftOut, FFTW_ESTIMATE);
|
||||
|
||||
// Pre-compute pattern conjugated FFT
|
||||
// TODO: Offset the pattern to avoid it being cut off (EXTREMELY IMPORTANT)
|
||||
memcpy(fftIn, pattern, patternLen*sizeof(float));
|
||||
memset(&fftIn[patternLen], 0, (fftSize-patternLen)*sizeof(float));
|
||||
fftwf_execute(plan);
|
||||
volk_32fc_conjugate_32fc((lv_32fc_t*)patternFFT, (lv_32fc_t*)fftOut, fftSize);
|
||||
|
||||
// Compute amplitudes of the pattern FFT
|
||||
volk_32fc_magnitude_32f(patternFFTAmps, (lv_32fc_t*)patternFFT, fftSize);
|
||||
|
||||
// Normalize the amplitudes
|
||||
float maxAmp = 0.0f;
|
||||
for (int i = 0; i < patternLen; i++) {
|
||||
if (patternFFTAmps[i] > maxAmp) { maxAmp = patternFFTAmps[i]; }
|
||||
}
|
||||
volk_32f_s32f_multiply_32f(patternFFTAmps, patternFFTAmps, 1.0f/maxAmp, fftSize);
|
||||
|
||||
// Init base
|
||||
base_type::init(in);
|
||||
}
|
||||
|
||||
int process(int count, float* in, float* out) {
|
||||
// Copy to buffer
|
||||
memcpy(bufferStart, in, count * sizeof(float));
|
||||
|
||||
int outCount = 0;
|
||||
bool first = true;
|
||||
|
||||
for (int i = 0; i < count; i++) {
|
||||
// Measure correlation to the sync pattern
|
||||
float corr;
|
||||
volk_32f_x2_dot_prod_32f(&corr, &buffer[i], pattern, patternLen);
|
||||
|
||||
// If not correlated enough, go to next sample. Otherwise continue with fine detection
|
||||
if (corr/(float)patternLen < threshold) { continue; }
|
||||
|
||||
// Copy samples into FFT input (only the part where we think the pattern is located)
|
||||
// TODO: Instead, check the interval onto which correlation occurs to determine where the pattern is located (IMPORTANT)
|
||||
memcpy(fftIn, &buffer[i], patternLen*sizeof(float));
|
||||
memset(&fftIn[patternLen], 0, (fftSize-patternLen)*sizeof(float)); // TODO, figure out why we need this
|
||||
|
||||
// Compute FFT
|
||||
fftwf_execute(plan);
|
||||
|
||||
// Multiply with the conjugated pattern FFT to get the phase offset at each frequency
|
||||
volk_32fc_x2_multiply_32fc((lv_32fc_t*)fftOut, (lv_32fc_t*)fftOut, (lv_32fc_t*)patternFFT, fftSize);
|
||||
|
||||
// Compute the average phase delay rate
|
||||
float last = 0;
|
||||
float rateIntegral = 0;
|
||||
for (int j = 1; j < fftSize/2; j++) {
|
||||
// Compute instantanous rate
|
||||
float currentPhase = fftOut[j].phase();
|
||||
float instantRate = dsp::math::normalizePhase(currentPhase - last);
|
||||
last = currentPhase;
|
||||
|
||||
// Compute current rate guess
|
||||
float rateGuess = rateIntegral / (float)j;
|
||||
|
||||
// Update the rate integral as a weighted average of the current guess and measured rate depending on pattern amplitude
|
||||
rateIntegral += patternFFTAmps[j]*instantRate + (1.0f-patternFFTAmps[j])*rateGuess;
|
||||
}
|
||||
float avgRate = 1.14f*rateIntegral/(float)(fftSize/2);
|
||||
|
||||
// Compute the total offset
|
||||
float offset = (float)i - avgRate*(float)fftSize/(2.0f*FL_M_PI);
|
||||
|
||||
if (first) {
|
||||
outCount = 320;
|
||||
memcpy(out, &buffer[(int)roundf(offset)], 320*sizeof(float));
|
||||
first = false;
|
||||
}
|
||||
|
||||
|
||||
flog::debug("Detected: {} -> {} ({})", i, offset, avgRate);
|
||||
}
|
||||
|
||||
// Move unused data
|
||||
memmove(buffer, &buffer[count], delayLen * sizeof(float));
|
||||
|
||||
return outCount;
|
||||
}
|
||||
|
||||
int run() {
|
||||
int count = base_type::_in->read();
|
||||
if (count < 0) { return -1; }
|
||||
|
||||
count = process(count, base_type::_in->readBuf, base_type::out.writeBuf);
|
||||
|
||||
base_type::_in->flush();
|
||||
//if (!base_type::out.swap(count)) { return -1; }
|
||||
return count;
|
||||
}
|
||||
|
||||
private:
|
||||
int delayLen;
|
||||
float* buffer = NULL;
|
||||
float* bufferStart = NULL;
|
||||
float* pattern = NULL;
|
||||
int patternLen;
|
||||
bool locked;
|
||||
int fftSize;
|
||||
|
||||
float threshold;
|
||||
|
||||
float* fftIn = NULL;
|
||||
complex_t* fftOut = NULL;
|
||||
fftwf_plan plan;
|
||||
|
||||
complex_t* patternFFT;
|
||||
float* patternFFTAmps;
|
||||
|
||||
float sampsPerSym;
|
||||
};
|
||||
}
|
||||
Ładowanie…
Reference in New Issue