diff --git a/decoder_modules/pager_decoder/src/pocsag/decoder.h b/decoder_modules/pager_decoder/src/pocsag/decoder.h index 672bed3d..9807e6d3 100644 --- a/decoder_modules/pager_decoder/src/pocsag/decoder.h +++ b/decoder_modules/pager_decoder/src/pocsag/decoder.h @@ -13,7 +13,7 @@ class POCSAGDecoder : public Decoder { public: - POCSAGDecoder(const std::string& name, VFOManager::VFO* vfo) : diag(0.6, 544) { + POCSAGDecoder(const std::string& name, VFOManager::VFO* vfo) : diag(0.6, BAUDRATE) { 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, 544, 0); + reshape.init(&dsp.soft, BAUDRATE, (BAUDRATE / 30.0) - BAUDRATE); dataHandler.init(&dsp.out, _dataHandler, this); diagHandler.init(&reshape.out, _diagHandler, this); diff --git a/decoder_modules/pager_decoder/src/pocsag/dsp.h b/decoder_modules/pager_decoder/src/pocsag/dsp.h index f5d10a6f..20a1b8f8 100644 --- a/decoder_modules/pager_decoder/src/pocsag/dsp.h +++ b/decoder_modules/pager_decoder/src/pocsag/dsp.h @@ -11,89 +11,6 @@ #include #include -#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 { using base_type = dsp::Processor; public: @@ -106,16 +23,12 @@ public: // Configure blocks demod.init(NULL, -4500.0, samplerate); - //dcBlock.init(NULL, 0.001); // NOTE: DC blocking causes issues because no scrambling, think more about it 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(10, taps); fir.init(NULL, shape); - //recov.init(NULL, samplerate/baudrate, 1e-4, 1.0, 0.05); - - cs.init(NULL, PATTERN_DSDSDZED, sizeof(PATTERN_DSDSDZED)/sizeof(float), 544, 10); + recov.init(NULL, samplerate/baudrate, 1e-4, 1.0, 0.05); // Free useless buffers - // dcBlock.out.free(); fir.out.free(); recov.out.free(); @@ -125,13 +38,9 @@ public: int process(int count, dsp::complex_t* in, float* softOut, uint8_t* out) { 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); - - count = cs.process(count, demod.out.readBuf, softOut); - - //dsp::digital::BinarySlicer::process(count, softOut, out); + count = recov.process(count, demod.out.readBuf, softOut); + dsp::digital::BinarySlicer::process(count, softOut, out); return count; } @@ -146,10 +55,8 @@ 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 (!base_type::out.swap(count)) { return -1; } if (count) { if (!soft.swap(count)) { return -1; } } - return count; } @@ -157,11 +64,8 @@ public: private: dsp::demod::Quadrature demod; - //dsp::correction::DCBlocker dcBlock; dsp::tap shape; dsp::filter::FIR fir; dsp::clock_recovery::MM recov; - dsp::PacketClockSync cs; - }; \ No newline at end of file diff --git a/decoder_modules/pager_decoder/src/pocsag/packet_clock_sync.h b/decoder_modules/pager_decoder/src/pocsag/packet_clock_sync.h deleted file mode 100644 index e5367f57..00000000 --- a/decoder_modules/pager_decoder/src/pocsag/packet_clock_sync.h +++ /dev/null @@ -1,221 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace dsp { - class PacketClockSync : public dsp::Processor { - using base_type = dsp::Processor; - public: - PacketClockSync() {} - PacketClockSync(dsp::stream* 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* 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(STREAM_BUFFER_SIZE+delayLen); - bufferStart = &buffer[delayLen]; - this->pattern = dsp::buffer::alloc(patternLen); - patternFFT = dsp::buffer::alloc(fftSize); - patternFFTAmps = dsp::buffer::alloc(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; - this->frameLen = frameLen; - - // 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 < fftSize/2; i++) { - if (patternFFTAmps[i] > maxAmp) { maxAmp = patternFFTAmps[i]; } - } - volk_32f_s32f_multiply_32f(patternFFTAmps, patternFFTAmps, 1.0f/maxAmp, fftSize); - - // Initialize the phase control loop - float omegaRelLimit = 0.05; - pcl.init(1, 10e-4, 0.0, 0.0, 1.0, sampsPerSym, sampsPerSym * (1.0 - omegaRelLimit), sampsPerSym * (1.0 + omegaRelLimit)); - generateInterpTaps(); - - // 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; - - for (int i = 0; i < count;) { - // Run clock recovery if needed - while (toRead) { - // Interpolate symbol - float symbol; - int phase = std::clamp(floorf(pcl.phase * (float)interpPhaseCount), 0, interpPhaseCount - 1); - volk_32f_x2_dot_prod_32f(&symbol, &buffer[offsetInt], interpBank.phases[phase], interpTapCount); - out[outCount++] = symbol; - - // Compute symbol phase error - float error = (math::step(lastSymbol) * symbol) - (lastSymbol * math::step(symbol)); - lastSymbol = symbol; - - // Clamp symbol phase error - if (error > 1.0f) { error = 1.0f; } - if (error < -1.0f) { error = -1.0f; } - - // Advance symbol offset and phase - pcl.advance(error); - float delta = floorf(pcl.phase); - offsetInt += delta; - i = offsetInt; - pcl.phase -= delta; - - // Decrement read counter - toRead--; - - if (offsetInt >= count) { - offsetInt -= count; - break; - } - } - - - // 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) { - i++; - 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)); - - // 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); - flog::debug("Detected: {} -> {}", i, offset); - - // Initialize clock recovery - offsetInt = floorf(offset) - 3; // TODO: Will be negative sometimes, has to be taken into account - pcl.phase = offset - (float)floorf(offset); - pcl.freq = sampsPerSym; - - // Start reading symbols - toRead = frameLen; - } - - // 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 (count) { - if (!base_type::out.swap(count)) { return -1; } - } - return count; - } - - private: - void generateInterpTaps() { - double bw = 0.5 / (double)interpPhaseCount; - dsp::tap lp = dsp::taps::windowedSinc(interpPhaseCount * interpTapCount, dsp::math::hzToRads(bw, 1.0), dsp::window::nuttall, interpPhaseCount); - interpBank = dsp::multirate::buildPolyphaseBank(interpPhaseCount, lp); - taps::free(lp); - } - - int delayLen; - float* buffer = NULL; - float* bufferStart = NULL; - float* pattern = NULL; - int patternLen; - bool locked; - int fftSize; - int frameLen; - float threshold; - - float* fftIn = NULL; - complex_t* fftOut = NULL; - fftwf_plan plan; - - complex_t* patternFFT; - float* patternFFTAmps; - - float sampsPerSym; - int toRead = 0; - - loop::PhaseControlLoop pcl; - dsp::multirate::PolyphaseBank interpBank; - int interpTapCount = 8; - int interpPhaseCount = 128; - float lastSymbol = 0.0f; - int offsetInt; - }; -} \ No newline at end of file