From 9ef8cd4b45ae5a2a1f5846aa255ca9f7b66fa11e Mon Sep 17 00:00:00 2001
From: ha7ilm <retzlerandras@gmail.com>
Date: Sun, 19 Feb 2017 15:41:33 +0100
Subject: [PATCH] Almost there at working interpolator

---
 csdr.c    | 17 +++++++++++------
 libcsdr.c | 26 +++++++++++++++++---------
 libcsdr.h |  1 +
 3 files changed, 29 insertions(+), 15 deletions(-)

diff --git a/csdr.c b/csdr.c
index 31d8ddc..c135c61 100644
--- a/csdr.c
+++ b/csdr.c
@@ -48,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "ima_adpcm.h"
 #include <sched.h>
 #include <math.h>
+#include <assert.h>
 
 char usage[]=
 "csdr - a simple commandline tool for Software Defined Radio receiver DSP.\n\n"
@@ -857,9 +858,11 @@ int main(int argc, char *argv[])
 
 		int factor;
 		sscanf(argv[2],"%d",&factor);
+		assert(factor >= 1);
 
 		float transition_bw = 0.05;
 		if(argc>=4) sscanf(argv[3],"%g",&transition_bw);
+		assert(transition_bw >= 0 && transition_bw < 1.);
 
 		window_t window = WINDOW_DEFAULT;
 		if(argc>=5)
@@ -870,31 +873,33 @@ int main(int argc, char *argv[])
 
 		int taps_length=firdes_filter_len(transition_bw);
 		fprintf(stderr,"fir_interpolate_cc: taps_length = %d\n",taps_length);
+		assert(taps_length > 0);
 
 		while (env_csdr_fixed_big_bufsize < taps_length*2) env_csdr_fixed_big_bufsize*=2; //temporary fix for buffer size if [transition_bw] is low
 		//fprintf(stderr, "env_csdr_fixed_big_bufsize = %d\n", env_csdr_fixed_big_bufsize);
 
 		if(!initialize_buffers()) return -2;
-		sendbufsize(the_bufsize/factor);
-
+		sendbufsize(the_bufsize*factor);
+		assert(the_bufsize > 0);
 
 		float *taps;
 		taps=(float*)malloc(taps_length*sizeof(float));
+		assert(taps);
 
 		firdes_lowpass_f(taps,taps_length,0.5/(float)factor,window);
 
 		int input_skip=0;
 		int output_size=0;
-		FREAD_C;
+		float* interp_output_buffer = (float*)malloc(sizeof(float)*2*the_bufsize*factor);
 		for(;;)
 		{
 			FEOF_CHECK;
-			output_size=fir_interpolate_cc((complexf*)input_buffer, (complexf*)output_buffer, the_bufsize, factor, taps, taps_length);
+			output_size=fir_interpolate_cc((complexf*)input_buffer, (complexf*)interp_output_buffer, the_bufsize, factor, taps, taps_length);
 			//fprintf(stderr, "os %d\n",output_size);
-			fwrite(output_buffer, sizeof(complexf), output_size, stdout);
+			fwrite(interp_output_buffer, sizeof(complexf), output_size, stdout);
 			fflush(stdout);
 			TRY_YIELD;
-			input_skip=factor*output_size;
+			input_skip=output_size/factor;
 			memmove((complexf*)input_buffer,((complexf*)input_buffer)+input_skip,(the_bufsize-input_skip)*sizeof(complexf)); //memmove lets the source and destination overlap
 			fread(((complexf*)input_buffer)+(the_bufsize-input_skip), sizeof(complexf), input_skip, stdin);
 			//fprintf(stderr,"iskip=%d output_size=%d start=%x target=%x skipcount=%x \n",input_skip,output_size,input_buffer, ((complexf*)input_buffer)+(BIG_BUFSIZE-input_skip),(BIG_BUFSIZE-input_skip));
diff --git a/libcsdr.c b/libcsdr.c
index f5da71c..2e02c8b 100644
--- a/libcsdr.c
+++ b/libcsdr.c
@@ -381,17 +381,25 @@ int fir_decimate_cc(complexf *input, complexf *output, int input_size, int decim
 
 int fir_interpolate_cc(complexf *input, complexf *output, int input_size, int interpolation, float *taps, int taps_length)
 {
+	//i:  input index
+	//oi: output index
+	//ti: tap index
+	//ti: secondary index (inside filter function)
+	//ip: interpolation phase (0 <= ip < interpolation)
 	int oi=0;
-	for(int i=0; i<input_size*interpolation; i++) //@fir_decimate_cc: outer loop
+	for(int i=0; i<input_size; i++) //@fir_interpolate_cc: outer loop
 	{
-		if(i+taps_length>input_size) break;
-		float acci=0;
-		for(int ti=0; ti<taps_length; ti++) acci += (iof(input,i+ti)) * taps[ti]; //@fir_interpolate_cc: i loop
-		float accq=0;
-		for(int ti=0; ti<taps_length; ti++) accq += (qof(input,i+ti)) * taps[ti]; //@fir_interpolate_cc: q loop
-		iof(output,oi)=acci;
-		qof(output,oi)=accq;
-		oi++;
+		if(i*interpolation + (interpolation-1) + taps_length > input_size*interpolation) break;
+		for(int ip=0; ip<interpolation; ip++)
+		{
+			float acci=0;
+			float accq=0;
+			for(int ti=ip, si=0; ti<taps_length; ti+=interpolation, si++) acci += (iof(input,i+si)) * taps[ti]; //@fir_interpolate_cc: i loop
+			for(int ti=ip, si=0; ti<taps_length; ti+=interpolation, si++) accq += (qof(input,i+si)) * taps[ti]; //@fir_interpolate_cc: q loop
+			iof(output,oi)=acci;
+			qof(output,oi)=accq;
+			oi++;
+		}
 	}
 	return oi;
 }
diff --git a/libcsdr.h b/libcsdr.h
index 8bcbc2a..9ef26db 100644
--- a/libcsdr.h
+++ b/libcsdr.h
@@ -99,6 +99,7 @@ void limit_ff(float* input, float* output, int input_size, float max_amplitude);
 //filters, decimators, resamplers, shift, etc.
 float fir_one_pass_ff(float* input, float* taps, int taps_length);
 int fir_decimate_cc(complexf *input, complexf *output, int input_size, int decimation, float *taps, int taps_length);
+int fir_interpolate_cc(complexf *input, complexf *output, int input_size, int interpolation, float *taps, int taps_length);
 int deemphasis_nfm_ff (float* input, float* output, int input_size, int sample_rate);
 float deemphasis_wfm_ff (float* input, float* output, int input_size, float tau, int sample_rate, float last_output);
 float shift_math_cc(complexf *input, complexf* output, int input_size, float rate, float starting_phase);