From 1446b6295998c49e5d376f43e1ab97b35b6186fe Mon Sep 17 00:00:00 2001
From: Tatu Peltola <tatu.peltola@aalto.fi>
Date: Sun, 23 Oct 2016 20:25:34 +0300
Subject: [PATCH] Implement shift_addition_fc for downconversion of real signal

---
 csdr.c        | 49 +++++++++++++++++++++++++++++++++++++++++++++++++
 libcsdr_gpl.c | 27 +++++++++++++++++++++++++++
 libcsdr_gpl.h |  1 +
 3 files changed, 77 insertions(+)

diff --git a/csdr.c b/csdr.c
index 1f31c81..a87e29e 100644
--- a/csdr.c
+++ b/csdr.c
@@ -717,6 +717,55 @@ int main(int argc, char *argv[])
 		return 0;
 	}
 
+	if(!strcmp(argv[1],"shift_addition_fc"))
+	{
+		bigbufs=1;
+
+		float starting_phase=0;
+		float rate;
+
+		int fd;
+		if(fd=init_fifo(argc,argv))
+		{
+			while(!read_fifo_ctl(fd,"%g\n",&rate)) usleep(10000);
+		}
+		else
+		{
+			if(argc<=2) return badsyntax("need required parameter (rate)");
+			sscanf(argv[2],"%g",&rate);
+		}
+
+		if(!sendbufsize(initialize_buffers())) return -2;
+		for(;;)
+		{
+			shift_addition_data_t data=shift_addition_init(rate);
+			fprintf(stderr,"shift_addition_fc: reinitialized to %g\n",rate);
+			int remain, current_size;
+			float* ibufptr;
+			float* obufptr;
+			for(;;)
+			{
+				FEOF_CHECK;
+				if(!FREAD_R) break;
+				remain=the_bufsize;
+				ibufptr=input_buffer;
+				obufptr=output_buffer;
+				while(remain)
+				{
+					current_size=(remain>1024)?1024:remain;
+					starting_phase=shift_addition_fc(ibufptr, (complexf*)obufptr, current_size, data, starting_phase);
+					ibufptr+=current_size;
+					obufptr+=current_size*2;
+					remain-=current_size;
+				}
+				FWRITE_C;
+				if(read_fifo_ctl(fd,"%g\n",&rate)) break;
+				TRY_YIELD;
+			}
+		}
+		return 0;
+	}
+
 	if(!strcmp(argv[1],"shift_addition_cc_test"))
 	{
 		if(argc<=2) return badsyntax("need required parameter (rate)");
diff --git a/libcsdr_gpl.c b/libcsdr_gpl.c
index b895e00..f9a651e 100644
--- a/libcsdr_gpl.c
+++ b/libcsdr_gpl.c
@@ -51,6 +51,33 @@ float shift_addition_cc(complexf *input, complexf* output, int input_size, shift
 	return starting_phase;
 }
 
+float shift_addition_fc(float *input, complexf* output, int input_size, shift_addition_data_t d, float starting_phase)
+{
+	//The original idea was taken from wdsp:
+	//http://svn.tapr.org/repos_sdr_hpsdr/trunk/W5WC/PowerSDR_HPSDR_mRX_PS/Source/wdsp/shift.c
+
+	//However, this method introduces noise (from floating point rounding errors), which increases until the end of the buffer.
+	//fprintf(stderr, "cosd=%g sind=%g\n", d.cosdelta, d.sindelta);
+	float cosphi=cos(starting_phase);
+	float sinphi=sin(starting_phase);
+	float cosphi_last, sinphi_last;
+	for(int i=0;i<input_size;i++) //@shift_addition_cc: work
+	{
+		iof(output,i)=cosphi*input[i];
+		qof(output,i)=sinphi*input[i];
+		//using the trigonometric addition formulas
+		//cos(phi+delta)=cos(phi)cos(delta)-sin(phi)*sin(delta)
+		cosphi_last=cosphi;
+		sinphi_last=sinphi;
+		cosphi=cosphi_last*d.cosdelta-sinphi_last*d.sindelta;
+		sinphi=sinphi_last*d.cosdelta+cosphi_last*d.sindelta;
+	}
+	starting_phase+=d.rate*PI*input_size;
+	while(starting_phase>PI) starting_phase-=2*PI; //@shift_addition_cc: normalize starting_phase
+	while(starting_phase<-PI) starting_phase+=2*PI;
+	return starting_phase;
+}
+
 shift_addition_data_t shift_addition_init(float rate)
 {
 	rate*=2;
diff --git a/libcsdr_gpl.h b/libcsdr_gpl.h
index a4bfd23..2e3e4da 100644
--- a/libcsdr_gpl.h
+++ b/libcsdr_gpl.h
@@ -31,6 +31,7 @@ typedef struct shift_addition_data_s
 } shift_addition_data_t;
 shift_addition_data_t shift_addition_init(float rate);
 float shift_addition_cc(complexf *input, complexf* output, int input_size, shift_addition_data_t d, float starting_phase);
+float shift_addition_fc(float *input, complexf* output, int input_size, shift_addition_data_t d, float starting_phase);
 void shift_addition_cc_test(shift_addition_data_t d);
 
 float agc_ff(float* input, float* output, int input_size, float reference, float attack_rate, float decay_rate, float max_gain, short hang_time, short attack_wait_time, float gain_filter_alpha, float last_gain);