diff --git a/radio/interpolation/Farrow.hpp b/radio/interpolation/Farrow.hpp index bcf0fb5..b1922ae 100644 --- a/radio/interpolation/Farrow.hpp +++ b/radio/interpolation/Farrow.hpp @@ -8,6 +8,8 @@ #include #include +#include + namespace Radio { namespace Interpolation @@ -39,8 +41,8 @@ public: , m_b(M) , m_h(M) , m_fifo(N) - , m_bufferIn(1024) - , m_bufferOut(1024) + , m_bufferIn(8*1024) + , m_bufferOut(8*1024) { init(M, N); } @@ -64,36 +66,43 @@ public: // m_bufferOut.fill(ComplexScalar(0,0)); } - void feed(ComplexScalar const &x, uint32_t push) + size_t feed(ComplexScalar const *x, uint32_t size=1) { - m_w = (m_w + push) % m_N; - m_fifo[m_w] = x; + return m_bufferIn.write(x, size); } - ComplexScalar process_new(ComplexScalar const &xin, RealScalar dmu) + bool process_new(RealScalar dmu) { - size_t adv = m_interpolator.process(dmu); - m_bufferIn.write(&xin, 1); - - if (adv) + while(true) { - ComplexScalar xout; - m_bufferIn.read(&xout, 1); - FirComplex::feed(xout); + radio_float_t mu = m_interpolator.getMu(); + size_t adv = m_interpolator.getAdv(); + if (m_bufferIn.len() < adv) + { + std::cout << "----- SKIP -----" << std::endl; + break; + } + if (adv) + { + ComplexScalar xout; + m_bufferIn.consume(adv); + m_bufferIn.read(&xout, 1, false); + FirComplex::feed(xout); + + // Partial filter responses + for (int i=0; i < m_M; i++) + { + m_h[i] = FirComplex::processReal(column(m_coeff, i)); + } + } + m_interpolator.process(dmu); + + // Combine + ComplexScalar yout = horner(mu); + m_bufferOut.write(&yout, 1); + } - - - // Partial filter responses - for (int i=0; i < m_M; i++) - { - m_h[i] = FirComplex::processReal(column(m_coeff, i)); - } - - // Combine - ComplexScalar yout = horner(m_interpolator.getMu()); - m_bufferOut.write(&yout, 1); - - return yout; + return true; } ComplexScalar process(RealScalar mu, uint32_t pop) diff --git a/radio/interpolation/Interpolator.cpp b/radio/interpolation/Interpolator.cpp index e959469..390ec00 100644 --- a/radio/interpolation/Interpolator.cpp +++ b/radio/interpolation/Interpolator.cpp @@ -1,13 +1,15 @@ #include "Interpolator.hpp" +#include namespace Radio { namespace Interpolation { Interpolator::Interpolator() -: m_mu (0) +: m_adv(0) +, m_mu(0) { - + count = 0; } Interpolator::~Interpolator() @@ -15,18 +17,14 @@ Interpolator::~Interpolator() } -void Interpolator::setMu(radio_float_t mu) +void Interpolator::process(radio_float_t dMu) { - m_mu = mu; -} + m_mu += dMu; + m_adv = (size_t)m_mu; + m_mu -= m_adv; - -size_t Interpolator::process(radio_float_t dMu) -{ - radio_float_t nextMu = m_mu + dMu; - size_t adv = nextMu; - nextMu -= adv; - return adv; + std::cout << count << ": adv = " << m_adv << ", mu = " << m_mu << std::endl; + count++; } radio_float_t Interpolator::getMu() @@ -34,5 +32,10 @@ radio_float_t Interpolator::getMu() return m_mu; } +size_t Interpolator::getAdv() +{ + return m_adv; +} + } // Interpolation } // Radio diff --git a/radio/interpolation/Interpolator.hpp b/radio/interpolation/Interpolator.hpp index 8f82f3a..54eef28 100644 --- a/radio/interpolation/Interpolator.hpp +++ b/radio/interpolation/Interpolator.hpp @@ -16,11 +16,13 @@ public: Interpolator(); virtual ~Interpolator(); - void setMu(radio_float_t mu); radio_float_t getMu(); - size_t process(radio_float_t dMu); + size_t getAdv(); + void process(radio_float_t dMu); private: radio_float_t m_mu; + size_t m_adv; + uint32_t count; }; diff --git a/radio/processor/src/Buffer.hpp b/radio/processor/src/Buffer.hpp index 86f065e..b23e4a7 100644 --- a/radio/processor/src/Buffer.hpp +++ b/radio/processor/src/Buffer.hpp @@ -58,9 +58,10 @@ class Buffer : public IBuffer T* data_w(); T* data_r(); + size_t write(const T* pData, size_t len); - size_t read(T* pData, size_t len); - Result readAt(size_t offset, T &x); + size_t read(T* pData, size_t len, bool do_consume=true); + T readAt(size_t offset); Result consume(size_t len); Result produce(size_t len); void print(const char *prefix); diff --git a/radio/processor/src/Buffer.tcc b/radio/processor/src/Buffer.tcc index c6cdda9..8b1ef62 100644 --- a/radio/processor/src/Buffer.tcc +++ b/radio/processor/src/Buffer.tcc @@ -108,7 +108,7 @@ size_t Buffer::write(const T *pData, size_t len) } template -size_t Buffer::read(T* pData, size_t len) +size_t Buffer::read(T* pData, size_t len, bool do_consume) { len = std::min(m_len, len); size_t numRead = 0; @@ -120,27 +120,26 @@ size_t Buffer::read(T* pData, size_t len) memcpy(pData, data_r(), actual_len*sizeof(T)); len -= actual_len; pData += actual_len; - consume(actual_len); + if (do_consume) + { + consume(actual_len); + } } return numRead; } template -typename Buffer::Result Buffer::readAt(size_t offset, T &x) +T Buffer::readAt(size_t offset) { - if (m_len == 0) + if (m_len < offset) { - RETURN_OR_EXCEPT(Result::Err_InvalidSize); + throw Buffer::Exception(Result::Err_InvalidSize); } - size_t n = (m_ri+offset) % m_capacity; - x = data_r()[n]; - if (offset) - { - consume(1); - } - - return Result::Ok; + size_t i = (m_ri + offset) % m_capacity; + T result = bufferActive()[i]; + consume(1); + return result; } template