diff --git a/decoder_modules/dab_decoder/src/ofdm.h b/decoder_modules/dab_decoder/src/ofdm.h new file mode 100644 index 00000000..4934bd5a --- /dev/null +++ b/decoder_modules/dab_decoder/src/ofdm.h @@ -0,0 +1,161 @@ +#pragma once +#include +#include +#include +#include +#include + +namespace dsp::ofdm { + class MM : public Processor { + using base_type = Processor ; + public: + MM() {} + + MM(stream* in, double omega, double omegaGain, double muGain, double omegaRelLimit, int interpPhaseCount = 128, int interpTapCount = 8) { init(in, omega, omegaGain, muGain, omegaRelLimit, interpPhaseCount, interpTapCount); } + + ~MM() { + if (!base_type::_block_init) { return; } + base_type::stop(); + dsp::multirate::freePolyphaseBank(interpBank); + buffer::free(buffer); + } + + void init(stream* in, double omega, double omegaGain, double muGain, double omegaRelLimit, int interpPhaseCount = 128, int interpTapCount = 8) { + _omega = omega; + _omegaGain = omegaGain; + _muGain = muGain; + _omegaRelLimit = omegaRelLimit; + _interpPhaseCount = interpPhaseCount; + _interpTapCount = interpTapCount; + + pcl.init(_muGain, _omegaGain, 0.0, 0.0, 1.0, _omega, _omega * (1.0 - omegaRelLimit), _omega * (1.0 + omegaRelLimit)); + generateInterpTaps(); + buffer = buffer::alloc(STREAM_BUFFER_SIZE + _interpTapCount); + bufStart = &buffer[_interpTapCount - 1]; + + base_type::init(in); + } + + void setOmega(double omega) { + assert(base_type::_block_init); + std::lock_guard lck(base_type::ctrlMtx); + base_type::tempStop(); + _omega = omega; + offset = 0; + pcl.phase = 0.0f; + pcl.freq = _omega; + pcl.setFreqLimits(_omega * (1.0 - _omegaRelLimit), _omega * (1.0 + _omegaRelLimit)); + base_type::tempStart(); + } + + void setOmegaGain(double omegaGain) { + assert(base_type::_block_init); + std::lock_guard lck(base_type::ctrlMtx); + _omegaGain = omegaGain; + pcl.setCoefficients(_muGain, _omegaGain); + } + + void setMuGain(double muGain) { + assert(base_type::_block_init); + std::lock_guard lck(base_type::ctrlMtx); + _muGain = muGain; + pcl.setCoefficients(_muGain, _omegaGain); + } + + void setOmegaRelLimit(double omegaRelLimit) { + assert(base_type::_block_init); + std::lock_guard lck(base_type::ctrlMtx); + _omegaRelLimit = omegaRelLimit; + pcl.setFreqLimits(_omega * (1.0 - _omegaRelLimit), _omega * (1.0 + _omegaRelLimit)); + } + + void setInterpParams(int interpPhaseCount, int interpTapCount) { + assert(base_type::_block_init); + std::lock_guard lck(base_type::ctrlMtx); + base_type::tempStop(); + _interpPhaseCount = interpPhaseCount; + _interpTapCount = interpTapCount; + dsp::multirate::freePolyphaseBank(interpBank); + buffer::free(buffer); + generateInterpTaps(); + buffer = buffer::alloc(STREAM_BUFFER_SIZE + _interpTapCount); + bufStart = &buffer[_interpTapCount - 1]; + base_type::tempStart(); + } + + void reset() { + assert(base_type::_block_init); + std::lock_guard lck(base_type::ctrlMtx); + base_type::tempStop(); + offset = 0; + pcl.phase = 0.0f; + pcl.freq = _omega; + base_type::tempStart(); + } + + inline int process(int count, const complex_t* in, complex_t* out) { + // Copy data to work buffer + memcpy(bufStart, in, count * sizeof(complex_t)); + + // Process all samples + int outCount = 0; + while (offset < count) { + float error = 0; // TODO + complex_t outVal; + + // Calculate new output value + int phase = std::clamp(floorf(pcl.phase * (float)_interpPhaseCount), 0, _interpPhaseCount - 1); + volk_32fc_32f_dot_prod_32fc((lv_32fc_t*)&outVal, (lv_32fc_t*)&buffer[offset], interpBank.phases[phase], _interpTapCount); + out[outCount++] = outVal; + + // Advance symbol offset and phase + pcl.advance(error); + float delta = floorf(pcl.phase); + offset += delta; + pcl.phase -= delta; + } + offset -= count; + + // Update delay buffer + memmove(buffer, &buffer[count], (_interpTapCount - 1) * sizeof(complex_t)); + + return outCount; + } + + int run() { + int count = base_type::_in->read(); + if (count < 0) { return -1; } + + int outCount = process(count, base_type::_in->readBuf, base_type::out.writeBuf); + + // Swap if some data was generated + base_type::_in->flush(); + if (outCount) { + if (!base_type::out.swap(outCount)) { return -1; } + } + return outCount; + } + + protected: + 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); + } + + dsp::multirate::PolyphaseBank interpBank; + loop::PhaseControlLoop pcl; + + double _omega; + double _omegaGain; + double _muGain; + double _omegaRelLimit; + int _interpPhaseCount; + int _interpTapCount; + + int offset = 0; + complex_t* buffer; + complex_t* bufStart; + }; +}; \ No newline at end of file