- refactored interpolation

git-svn-id: http://moon:8086/svn/software/trunk/libsrc/cpp@927 b431acfa-c32f-4a4a-93f1-934dc6c82436
2022-06-02 06:10:44 +00:00
parent 4297025be3
commit 526c490325
10 changed files with 502 additions and 2 deletions
@@ -8,7 +8,7 @@
 #ifndef _RADIO_FIRCOMPLEX_HPP_
 #define	_RADIO_FIRCOMPLEX_HPP_

-#include "Vector.hpp"
+#include <cpp/radio/Vector.hpp>

 namespace Radio
 {
@@ -11,7 +11,8 @@
 #define VECTOR_USE_EIGEN

 #include <complex>
-#include <radio/radio_types.h>
+#include <radio/cpx.h>
+#include <radio/real.h>

 #ifndef radio_float_t
 #define radio_float_t float
@@ -0,0 +1 @@
+#include "DownSampler.hpp"
@@ -0,0 +1,115 @@
+#ifndef _INTERPOLATION_DOWNSAMPLER_HPP_
+#define _INTERPOLATION_DOWNSAMPLER_HPP_
+
+#pragma once
+
+#include <cpp/radio/Vector.hpp>
+#include <cpp/radio/FirComplex.hpp>
+
+namespace Radio
+{
+namespace Interpolation
+{
+
+// --------------------------------------------------------------
+// Complex FIR-based DownSampler
+// --------------------------------------------------------------
+
+class DownSampler
+{
+public:
+	DownSampler()
+	: m_pFir(nullptr)
+	, m_pCoeff(nullptr)
+	, m_M(0)
+	, m_currStage(0)
+	{
+		
+	}
+	~DownSampler()
+	{
+		if (m_pFir)
+			delete [] m_pFir;
+			
+		if (m_pCoeff)
+			delete [] m_pCoeff;
+		
+	}
+	void init(uint32_t M, RealScalar const *coeff, uint32_t N)
+	{
+		uint32_t i, j;
+		uint32_t stage_N;
+		uint32_t stage;
+		
+		if (m_pFir)
+			delete [] m_pFir;
+			
+		if (m_pCoeff)
+			delete [] m_pCoeff;
+
+		m_pFir = new FirComplex[M];
+		m_pCoeff = new RVec[M];
+		
+		for (stage=0; stage < M; stage++)
+		{
+			stage_N = 0;
+			for (i=stage; i < N; i += M)
+			{
+				stage_N++;
+			}
+			m_pFir[stage].setNumTaps(stage_N);
+			m_pCoeff[stage].resize(stage_N);
+			
+			j=0;
+			for (i=stage; i < N; i += M)
+			{
+				if (coeff)
+				{
+					m_pCoeff[stage](j++) = coeff[i];
+				}
+			}
+		}
+		m_M = M;
+		m_currStage = m_M-1;
+
+	}
+	
+	uint32_t process(CVec &dst, CVec const &src, uint32_t len)
+	{
+		uint32_t i;
+		uint32_t j;
+		
+		j = 0;
+		for (i=0; i < len; i++)
+		{
+			m_pFir[m_currStage].feed(src(i));
+			if (m_currStage == (m_M-1))
+			{
+				dst(j) = m_pFir[m_currStage].processReal(m_pCoeff[m_currStage]);
+			}
+			else
+			{
+				dst(j) += m_pFir[m_currStage].processReal(m_pCoeff[m_currStage]);
+			}
+			if (--m_currStage >= m_M)
+			{
+				m_currStage = m_M-1;
+				j++;
+			}
+			
+		}
+		return j;
+	}
+	
+private:
+	FirComplex *m_pFir;
+	RVec *m_pCoeff;
+	uint32_t m_M;
+	uint32_t m_currStage;
+	
+};
+
+} // Radio::Interpolation
+} // ::Radio
+
+#endif // _INTERPOLATION_DOWNSAMPLER_HPP_
@@ -0,0 +1,2 @@
+#include "Farrow.hpp"
+
@@ -0,0 +1,171 @@
+#ifndef _INTERPOLATION_FARROW_HPP_
+#define _INTERPOLATION_FARROW_HPP_
+
+#pragma once
+
+#include <cpp/radio/Vector.hpp>
+#include <cpp/radio/FirComplex.hpp>
+
+namespace Radio
+{
+namespace Interpolation
+{
+
+// --------------------------------------------------------------
+// Complex FIR-based polyphase interpolation filter (Farrow structure)
+// From:
+// "PERFORMANCE AND DESIGN OF FARROW FILTER USED FOR ARBITRARY RESAMPLING"
+// [unknown date], Fred Harris, Signal Processing ChairCommunication Systems and Signal Processing Institute
+// College of Engineering, San Diego State University, San Diego, CA 92182-0190 USA
+// --------------------------------------------------------------
+class Farrow : public FirComplex
+{
+	typedef struct _ml_farrow_coef_hdr_t
+	{
+		uint32_t M;
+		uint32_t N;
+	} ml_farrow_coef_hdr_t;
+	
+public:
+	Farrow(uint32_t M=0, uint32_t N=0)
+	: FirComplex(N)
+	, m_M(M)
+	, m_N(N)
+	, m_w(0)
+	, m_r(0)
+	, m_coeff(N, M)
+	, m_b(M)
+	, m_h(M)
+	, m_fifo(N)
+	{
+		init(M, N);
+	}
+	
+	~Farrow()
+	{
+		
+	}
+	
+	void init(uint32_t M, uint32_t N)
+	{
+		FirComplex::setNumTaps(N);
+		m_M = M;
+		m_N = N;
+		m_coeff.resize(N, M);
+		m_b.resize(M);
+		m_h.resize(M);
+		m_fifo.resize(N);
+		m_fifo.setZero();
+	}
+
+	void feed(ComplexScalar const &x, uint32_t push)
+	{
+		m_w = (m_w + push) % m_N;
+		m_fifo[m_w] = x;
+	}
+	
+	ComplexScalar process(RealScalar mu, uint32_t pop)
+	{
+		uint32_t i, r;
+		int32_t j;
+
+		m_r = (m_r + pop) % m_N;
+
+		if (pop)
+		{
+			r = m_r;
+			j = m_N-1;
+			while(r < m_N)
+			{
+				m_state[j--] = m_fifo[r++];
+			}
+			r = 0;
+			while(j >= 0)
+			{
+				m_state[j--] = m_fifo[r++];
+			}
+		}
+
+//		Eigen::internal::set_is_malloc_allowed(true);
+		// Partial filter responses
+		for (i=0; i < m_M; i++)
+		{
+			m_h[i] = FirComplex::processReal(m_coeff.col(i));
+		}
+//		Eigen::internal::set_is_malloc_allowed(false);
+
+		// Combine
+		return horner(mu);
+	}	
+
+	void load(const char *pFilename)
+	{
+		int i, j;
+		FILE *pFile;
+		ml_farrow_coef_hdr_t hdr;
+		RealScalar *pCoeff;
+		
+		pFile = fopen(pFilename, "rb");
+		if (!pFile)
+		{
+			printf("Can't open %s\n", pFilename);
+			return;
+		}
+		if (EOF == fread(&hdr, sizeof(hdr), 1, pFile))
+		{
+			printf("Can't read %s\n", pFilename);
+			return;
+		}
+
+		init(hdr.M, hdr.N);
+
+		pCoeff = new RealScalar[hdr.M*hdr.N];
+		for (i=0; i < (int)hdr.M; i++)
+		{
+			if (EOF == fread(pCoeff, sizeof(RealScalar), hdr.N, pFile))
+			{
+				printf("Can't read %s\n", pFilename);
+				return;
+			}
+				
+			for (j=0; j < hdr.N; j++)
+			{
+				m_coeff(j,i) = pCoeff[j];
+			}
+		}
+		printf("Farrow filter coefficients loaded (M=%d, N=%d)\n", m_M, m_N);
+		
+		delete [] pCoeff;
+		fclose(pFile);
+
+	}
+	
+private:
+	uint32_t m_M;
+	uint32_t m_N;
+	uint32_t m_r;
+	uint32_t m_w;
+	RMat m_coeff;
+	CVec m_b;
+	CVec m_h;
+	CVec m_fifo;
+	
+	ComplexScalar horner(RealScalar mu)
+	{
+		uint32_t i;
+
+		m_b[0] = m_h[0];
+		for (i=1; i < m_M; i++)
+		{
+			m_b[i] = ComplexScalar(m_h[i].real() + m_b[i-1].real() * mu, m_h[i].imag() + m_b[i-1].imag() * mu);
+		}
+		
+		return m_b[m_M-1];
+	}
+	
+};
+
+} // Radio::Interpolation
+} // ::Radio
+
+#endif // _INTERPOLATION_FARROW_HPP_
@@ -0,0 +1,2 @@
+#include "PolyPhase.hpp"
+
@@ -0,0 +1,104 @@
+#ifndef _INTERPOLATION_POLYPHASE_HPP_
+#define _INTERPOLATION_POLYPHASE_HPP_
+
+#pragma once
+
+#include <cpp/radio/Vector.hpp>
+#include <cpp/radio/FirComplex.hpp>
+#include <fir/fir2.h>
+
+namespace Radio
+{
+namespace Interpolation
+{
+// --------------------------------------------------------------
+// Complex FIR-based discrete step polyphase interpolation filter
+// --------------------------------------------------------------
+class PolyPhase : public FirComplex
+{
+public:
+	PolyPhase()
+	{
+		
+	}
+	
+	~PolyPhase()
+	{
+		
+	}
+	
+	void init(RealScalar fa, RealScalar tsym, RealScalar alpha, uint32_t N, uint32_t M)
+	{
+		uint32_t i, j;
+		fir_float_t *pCoeff;
+		RealScalar k;
+	
+		FirComplex::setNumTaps(N);
+		m_M = M;
+		m_N = N;
+		m_coeff.resize(N, M);
+		m_fifo.resize(N);
+		m_fifo.setZero();
+		
+		pCoeff = (fir_float_t*)malloc((N+1)*M*sizeof(fir_float_t));
+		memset(pCoeff, 0, (N+1)*M*sizeof(fir_float_t));
+
+		k = CalcFirSRRC(pCoeff, M*fa, tsym, alpha, N*M);
+
+		m_coeff.resize(N, M);
+		for(i=0; i < M; i++)
+		{
+			for(j=0; j < N; j++)
+			{
+				m_coeff(j,i) = M*pCoeff[M*j+M/2+i];
+			}
+		}
+		free(pCoeff);
+	}
+
+	void feed(ComplexScalar const &x, uint32_t push)
+	{
+		m_w = (m_w + push) % m_N;
+		m_fifo[m_w] = x;
+	}
+	
+	ComplexScalar process(RealScalar mu, uint32_t pop)
+	{
+		uint32_t i, r, index;
+		int32_t j;
+
+		index = (uint32_t)dmod(dmax(0, mu*m_M), (RealScalar)m_M);
+		
+		m_r = (m_r + pop) % m_N;
+
+		if (pop)
+		{
+			r = m_r;
+			j = m_N-1;
+			while(r < m_N)
+			{
+				m_state[j--] = m_fifo[r++];
+			}
+			r = 0;
+			while(j >= 0)
+			{
+				m_state[j--] = m_fifo[r++];
+			}
+		}
+
+		return FirComplex::processReal(m_coeff.col(index));
+	}	
+	
+private:
+	uint32_t m_M;
+	uint32_t m_N;
+	uint32_t m_r;
+	uint32_t m_w;
+	RMat m_coeff;
+	CVec m_fifo;
+};
+
+} // Radio::Interpolation
+} // ::Radio
+
+#endif // _INTERPOLATION_POLYPHASE_HPP_
@@ -0,0 +1 @@
+#include "UpSampler.hpp"
@@ -0,0 +1,103 @@
+#ifndef _INTERPOLATION_UPSAMPLER_HPP_
+#define _INTERPOLATION_UPSAMPLER_HPP_
+
+#pragma once
+
+#include <cpp/radio/Vector.hpp>
+#include <cpp/radio/FirComplex.hpp>
+
+namespace Radio
+{
+namespace Interpolation
+{
+
+// --------------------------------------------------------------
+// Complex FIR-based UpSampler
+// --------------------------------------------------------------
+
+class UpSampler
+{
+public:
+	UpSampler()
+	: m_pFir(nullptr)
+	, m_pCoeff(nullptr)
+	, m_L(0)
+	{
+		
+	}
+	~UpSampler()
+	{
+		if (m_pFir)
+			delete [] m_pFir;
+			
+		if (m_pCoeff)
+			delete [] m_pCoeff;
+		
+	}
+	void init(uint32_t L, RealScalar const *coeff, uint32_t N)
+	{
+		uint32_t i, j;
+		uint32_t stage_N;
+		uint32_t stage;
+	
+		if (m_pFir)
+			delete [] m_pFir;
+
+		if (m_pCoeff)
+			delete [] m_pCoeff;
+
+		m_pFir = new FirComplex[L];
+		m_pCoeff = new RVec[L];
+		
+		for (stage=0; stage < L; stage++)
+		{
+			stage_N = 0;
+			for (i=L-stage-1; i < N; i += L)
+			{
+				stage_N++;
+			}
+			m_pFir[stage].setNumTaps(stage_N);
+			m_pCoeff[stage].resize(stage_N);
+			
+			j=0;
+			for (i=L-stage-1; i < N; i += L)
+			{
+				if (coeff)
+				{
+					m_pCoeff[stage](j++) = coeff[i];
+				}
+			}
+		}
+		m_L = L;
+	}
+
+	uint32_t process(CVec &dst, CVec const &src, uint32_t len)
+	{
+		uint32_t i;
+		uint32_t j;
+		uint32_t currStage;
+		
+		j = 0;
+		for (i=0; i < len; i++)
+		{
+			for (currStage=0; currStage < m_L; currStage++)
+			{
+				m_pFir[currStage].feed(src(i));
+				dst(m_L-1-currStage+j) = m_pFir[currStage].processReal(m_pCoeff[currStage]);
+			}
+			j += m_L;
+		}
+		return j;
+	}
+
+private:
+	FirComplex *m_pFir;
+	RVec *m_pCoeff;
+	uint32_t m_L;
+	
+};
+
+} // Radio::Interpolation
+} // ::Radio
+
+#endif // _INTERPOLATION_UPSAMPLER_HPP_