new halfband filter for downsampling

2 files changed, 88 insertions, 20 deletions

diff --git a/dsp/inthalfbandfilter.cpp b/dsp/inthalfbandfilter.cpp
index f1bd23f..8ff9b07 100644
--- a/dsp/inthalfbandfilter.cpp
+++ b/dsp/inthalfbandfilter.cpp
@@ -2,9 +2,10 @@
 
 IntHalfbandFilter::IntHalfbandFilter()
 {
-	for(int i = 0; i < FILTERLEN; i++) {
+	for(int i = 0; i < FILTERORDER + 1; i++) {
 		m_samples[i][0] = 0;
 		m_samples[i][1] = 0;
 	}
 	m_ptr = 0;
+	m_state = 0;
 }
diff --git a/dsp/inthalfbandfilter.h b/dsp/inthalfbandfilter.h
index 2ac9384..629a1e3 100644
--- a/dsp/inthalfbandfilter.h
+++ b/dsp/inthalfbandfilter.h
@@ -5,8 +5,12 @@
 
 // uses Q1.14 format internally, input and output are S16
 
+/*
+ * supported filter orders: 64, 48, 32
+ */
+#define FILTERORDER 48
+
 #define SHIFT 14
-#define FILTERLEN 16
 
 class IntHalfbandFilter {
 public:
@@ -19,49 +23,102 @@ public:
 		m_samples[m_ptr][0] = *i;
 		m_samples[m_ptr][1] = *q;
 
-		if((m_ptr & 1) != 0) {
+		if(m_state == 0) {
+			m_state = 1;
+
 			// save result
 			doFIR(i, q);
 
 			// advance write-pointer
-			m_ptr = (m_ptr + (FILTERLEN - 1)) % FILTERLEN;
+			m_ptr = (m_ptr + FILTERORDER);
+			if(m_ptr >= (FILTERORDER + 1))
+				m_ptr -= (FILTERORDER + 1);
 
 			// tell caller we have a new sample
 			return true;
 		} else {
+			m_state = 0;
+
 			// advance write-pointer
-			m_ptr = (m_ptr + (FILTERLEN - 1)) % FILTERLEN;
+			m_ptr = (m_ptr + FILTERORDER);
+			if(m_ptr >= (FILTERORDER + 1))
+				m_ptr -= (FILTERORDER + 1);
 
 			return false;
 		}
 	}
 
 protected:
-	qint16 m_samples[FILTERLEN][2];
+	qint16 m_samples[FILTERORDER + 1][2];
 	int m_ptr;
+	int m_state;
 
 	void doFIR(qint16* iResult, qint16* qResult)
 	{
 		// coefficents
-		static const qint32 COEFF[FILTERLEN / 2] = {
-			-0.026129089617881795515330622947658412158 * (1 << SHIFT),
-			-0.012950412689549032144165074953434668714 * (1 << SHIFT),
-			 0.041565523271949324224383559567286283709 * (1 << SHIFT),
-			 0.03266607144386817623837870883107825648  * (1 << SHIFT),
-			-0.068224549707270459864005829331290442497 * (1 << SHIFT),
-			-0.076102167644322413209145850032655289397 * (1 << SHIFT),
-			 0.157303124195510402039133168727857992053 * (1 << SHIFT),
-			 0.43955491415211112027350282005500048399  * (1 << SHIFT)
+
+#if FILTERORDER == 64
+		static const qint32 COEFF[16] = {
+			-0.001114417441601693505720538368564120901 * (1 << SHIFT),
+			 0.001268007827185253051302527005361753254 * (1 << SHIFT),
+			-0.001959831378850490895410230152151598304 * (1 << SHIFT),
+			 0.002878308307661380308073439948657323839 * (1 << SHIFT),
+			-0.004071361818258721100571850826099762344 * (1 << SHIFT),
+			 0.005597288494657440618973431867289036745 * (1 << SHIFT),
+			-0.007532345003308904551886371336877346039 * (1 << SHIFT),
+			 0.009980346844667375288961963519795972388 * (1 << SHIFT),
+			-0.013092614174300500062830820979797863401 * (1 << SHIFT),
+			 0.01710934914871829748417297878404497169  * (1 << SHIFT),
+			-0.022443558692997273018576720460259821266 * (1 << SHIFT),
+			 0.029875811511593811098386197500076377764 * (1 << SHIFT),
+			-0.041086352085710403647667021687084343284 * (1 << SHIFT),
+			 0.060465467462665789533104998554335907102 * (1 << SHIFT),
+			-0.104159517495977321788203084906854201108 * (1 << SHIFT),
+			 0.317657589850154464805598308885237202048 * (1 << SHIFT),
 		};
+#elif FILTERORDER == 48
+		static const qint32 COEFF[12] = {
+		   -0.004102576237611492253332112767338912818 * (1 << SHIFT),
+			0.003950551047979387886410762575906119309 * (1 << SHIFT),
+		   -0.005807875789391703583164350277456833282 * (1 << SHIFT),
+			0.00823497890520805998770814682075069868  * (1 << SHIFT),
+		   -0.011372226513199541059195851744334504474 * (1 << SHIFT),
+			0.015471557140973646315984524335362948477 * (1 << SHIFT),
+		   -0.020944996398689276484450516591095947661 * (1 << SHIFT),
+			0.028568078132034283034279553703527199104 * (1 << SHIFT),
+		   -0.040015143905614086738964374490024056286 * (1 << SHIFT),
+			0.059669519431831075095828964549582451582 * (1 << SHIFT),
+		   -0.103669138691865420076609893840213771909 * (1 << SHIFT),
+			0.317491986549921390015072120149852707982 * (1 << SHIFT)
+		};
+#elif FILTERORDER == 32
+		static const qint32 COEFF[8] = {
+		   -0.015956912844043127236437484839370881673 * (1 << SHIFT),
+			0.013023031678944928940522274274371739011 * (1 << SHIFT),
+		   -0.01866942273717486777684371190844103694  * (1 << SHIFT),
+			0.026550887571157304190005987720724078827 * (1 << SHIFT),
+		   -0.038350314277854319344740474662103224546 * (1 << SHIFT),
+			0.058429248652825838128421764849917963147 * (1 << SHIFT),
+		   -0.102889802028955756885153505209018476307 * (1 << SHIFT),
+			0.317237706405931241260276465254719369113 * (1 << SHIFT)
+		};
+#else
+#error unsupported filter order
+#endif
+
 
 		// init read-pointer
-		int a = m_ptr;
-		int b = (m_ptr + (FILTERLEN - 1)) % FILTERLEN;
+		int a = (m_ptr + 1);
+		if(a >= (FILTERORDER + 1))
+			a -= (FILTERORDER + 1);
+		int b = (m_ptr + (FILTERORDER - 1));
+		if(b >= (FILTERORDER + 1))
+			b -= (FILTERORDER + 1);
 
 		// go through samples in buffer
 		qint32 iAcc = 0;
 		qint32 qAcc = 0;
-		for(int i = 0; i < (FILTERLEN / 2); i++) {
+		for(int i = 0; i < FILTERORDER / 4; i++) {
 			// do multiply-accumulate
 			qint32 iTmp = m_samples[a][0] + m_samples[b][0];
 			qint32 qTmp = m_samples[a][1] + m_samples[b][1];
@@ -69,10 +126,20 @@ protected:
 			qAcc += qTmp * COEFF[i];
 
 			// update read-pointer
-			a = (a + 1) % FILTERLEN;
-			b = (b + (FILTERLEN - 1)) % FILTERLEN;
+			a = (a + 2);
+			if(a >= (FILTERORDER + 1))
+				a -= (FILTERORDER + 1);
+			b = b + (FILTERORDER - 1);
+			if(b >= (FILTERORDER + 1))
+				b -= (FILTERORDER + 1);
 		}
 
+		a = (a + FILTERORDER);
+		if(a >= (FILTERORDER + 1))
+			a -= (FILTERORDER + 1);
+		iAcc += m_samples[a][0] * (qint32)(0.5 * (1 << SHIFT));
+		qAcc += m_samples[a][1] * (qint32)(0.5 * (1 << SHIFT));
+
 		// done, save result
 		*iResult = iAcc >> SHIFT;
 		*qResult = qAcc >> SHIFT;