beginning of cleaner OFDM demod code

This commit is contained in:
AlexandreRouma 2024-09-19 05:47:58 +02:00
parent 17eccf5156
commit d87ae23560

View File

@ -0,0 +1,161 @@
#pragma once
#include <dsp/processor.h>
#include <dsp/loop/phase_control_loop.h>
#include <dsp/taps/windowed_sinc.h>
#include <dsp/multirate/polyphase_bank.h>
#include <dsp/math/step.h>
namespace dsp::ofdm {
class MM : public Processor<complex_t, complex_t> {
using base_type = Processor<complex_t, complex_t> ;
public:
MM() {}
MM(stream<complex_t>* 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<complex_t>* 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<complex_t>(STREAM_BUFFER_SIZE + _interpTapCount);
bufStart = &buffer[_interpTapCount - 1];
base_type::init(in);
}
void setOmega(double omega) {
assert(base_type::_block_init);
std::lock_guard<std::recursive_mutex> 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<std::recursive_mutex> lck(base_type::ctrlMtx);
_omegaGain = omegaGain;
pcl.setCoefficients(_muGain, _omegaGain);
}
void setMuGain(double muGain) {
assert(base_type::_block_init);
std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
_muGain = muGain;
pcl.setCoefficients(_muGain, _omegaGain);
}
void setOmegaRelLimit(double omegaRelLimit) {
assert(base_type::_block_init);
std::lock_guard<std::recursive_mutex> 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<std::recursive_mutex> lck(base_type::ctrlMtx);
base_type::tempStop();
_interpPhaseCount = interpPhaseCount;
_interpTapCount = interpTapCount;
dsp::multirate::freePolyphaseBank(interpBank);
buffer::free(buffer);
generateInterpTaps();
buffer = buffer::alloc<complex_t>(STREAM_BUFFER_SIZE + _interpTapCount);
bufStart = &buffer[_interpTapCount - 1];
base_type::tempStart();
}
void reset() {
assert(base_type::_block_init);
std::lock_guard<std::recursive_mutex> 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<int>(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<float> lp = dsp::taps::windowedSinc<float>(_interpPhaseCount * _interpTapCount, dsp::math::hzToRads(bw, 1.0), dsp::window::nuttall, _interpPhaseCount);
interpBank = dsp::multirate::buildPolyphaseBank<float>(_interpPhaseCount, lp);
taps::free(lp);
}
dsp::multirate::PolyphaseBank<float> interpBank;
loop::PhaseControlLoop<float, false> pcl;
double _omega;
double _omegaGain;
double _muGain;
double _omegaRelLimit;
int _interpPhaseCount;
int _interpTapCount;
int offset = 0;
complex_t* buffer;
complex_t* bufStart;
};
};