From daf0f8c159c00f1535022107305a9157a681d94f Mon Sep 17 00:00:00 2001 From: AlexandreRouma Date: Thu, 8 Feb 2024 14:17:35 +0100 Subject: [PATCH] more work on new clock recovery --- .../pager_decoder/src/pocsag/decoder.h | 4 +- .../pager_decoder/src/pocsag/dsp.h | 2 +- .../src/pocsag/packet_clock_sync.h | 87 +++++++++++++++---- 3 files changed, 75 insertions(+), 18 deletions(-) diff --git a/decoder_modules/pager_decoder/src/pocsag/decoder.h b/decoder_modules/pager_decoder/src/pocsag/decoder.h index d861f00a..672bed3d 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, 320) { + POCSAGDecoder(const std::string& name, VFOManager::VFO* vfo) : diag(0.6, 544) { 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, 320, 0); + reshape.init(&dsp.soft, 544, 0); 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 ae4ad949..f5d10a6f 100644 --- a/decoder_modules/pager_decoder/src/pocsag/dsp.h +++ b/decoder_modules/pager_decoder/src/pocsag/dsp.h @@ -112,7 +112,7 @@ public: 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), 0, 10); + cs.init(NULL, PATTERN_DSDSDZED, sizeof(PATTERN_DSDSDZED)/sizeof(float), 544, 10); // Free useless buffers // dcBlock.out.free(); diff --git a/decoder_modules/pager_decoder/src/pocsag/packet_clock_sync.h b/decoder_modules/pager_decoder/src/pocsag/packet_clock_sync.h index 42a57f5b..e5367f57 100644 --- a/decoder_modules/pager_decoder/src/pocsag/packet_clock_sync.h +++ b/decoder_modules/pager_decoder/src/pocsag/packet_clock_sync.h @@ -43,6 +43,7 @@ namespace dsp { 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); @@ -59,11 +60,16 @@ namespace dsp { // Normalize the amplitudes float maxAmp = 0.0f; - for (int i = 0; i < patternLen; i++) { + 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); } @@ -72,21 +78,55 @@ namespace dsp { // Copy to buffer memcpy(bufferStart, in, count * sizeof(float)); - int outCount = 0; - bool first = true; + 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; + } + } + - 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; } + 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)); - memset(&fftIn[patternLen], 0, (fftSize-patternLen)*sizeof(float)); // TODO, figure out why we need this // Compute FFT fftwf_execute(plan); @@ -113,15 +153,15 @@ namespace dsp { // Compute the total offset float offset = (float)i - avgRate*(float)fftSize/(2.0f*FL_M_PI); + flog::debug("Detected: {} -> {}", i, offset); - if (first) { - outCount = 320; - memcpy(out, &buffer[(int)roundf(offset)], 320*sizeof(float)); - first = false; - } - + // 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; - flog::debug("Detected: {} -> {} ({})", i, offset, avgRate); + // Start reading symbols + toRead = frameLen; } // Move unused data @@ -137,11 +177,20 @@ namespace dsp { count = process(count, base_type::_in->readBuf, base_type::out.writeBuf); base_type::_in->flush(); - //if (!base_type::out.swap(count)) { return -1; } + 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; @@ -149,7 +198,7 @@ namespace dsp { int patternLen; bool locked; int fftSize; - + int frameLen; float threshold; float* fftIn = NULL; @@ -160,5 +209,13 @@ namespace dsp { 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