Added pll_cc

csdr.c

@@ -1954,6 +1954,45 @@ int main(int argc, char *argv[])
 		}
 	}
 
+	if(!strcmp(argv[1],"pll_cc"))
+	{
+		pll_t pll;
+
+		if(argc<=2) return badsyntax("need required parameter (pll_type)");
+		sscanf(argv[2],"%d",(int*)&pll.pll_type);
+		//if(serial.samples_per_bits<1) return badsyntax("samples_per_bits should be at least 1.");
+		//if(serial.samples_per_bits<5) fprintf(stderr, "serial_line_decoder_sy_u8: warning: this algorithm does not work well if samples_per_bits is too low. It should be at least 5.\n");
+		if(pll.pll_type == PLL_1ST_ORDER_IIR_LOOP_FILTER)
+		{
+				float alpha = 0.01;
+				if(argc>3) sscanf(argv[3],"%f",&alpha);
+				pll_cc_init_1st_order_IIR(&pll, alpha);
+		}
+		else if(pll.pll_type == PLL_2ND_ORDER_IIR_LOOP_FILTER)
+		{
+			float bandwidth = 0.1, gain = 1000, damping_factor = 0.707;
+			if(argc>3) sscanf(argv[3],"%f",&bandwidth);
+			if(argc>4) sscanf(argv[4],"%f",&gain);
+			if(argc>5) sscanf(argv[5],"%f",&damping_factor);
+			pll_cc_init_2nd_order_IIR(&pll, bandwidth, gain, damping_factor);
+			fprintf(stderr, "%f %f %f | a: %f %f %f | b: %f %f %f\n", bandwidth, gain, damping_factor,
+				pll.filter_taps_a[0], pll.filter_taps_a[1], pll.filter_taps_a[2], pll.filter_taps_b[0], pll.filter_taps_b[1], pll.filter_taps_b[2]);
+		}
+		else return badsyntax("invalid pll_type. Valid values are:\n\t1: PLL_1ST_ORDER_IIR_LOOP_FILTER\n\t2: PLL_2ND_ORDER_IIR_LOOP_FILTER");
+
+		if(!sendbufsize(initialize_buffers())) return -2;
+
+		for(;;)
+		{
+			FEOF_CHECK;
+			FREAD_C;
+			//pll_cc(&pll, (complexf*)input_buffer, NULL, (complexf*)output_buffer, the_bufsize);
+			pll_cc(&pll, (complexf*)input_buffer, output_buffer, NULL, the_bufsize);
+			fwrite(output_buffer, sizeof(float), the_bufsize, stdout);
+			TRY_YIELD;
+		}
+	}
+
 	if(!strcmp(argv[1],"none"))
 	{
 		return 0;

libcsdr.c

@@ -1336,6 +1336,68 @@ void binary_slicer_f_u8(float* input, unsigned char* output, int input_size)
 	for(int i=0;i<input_size;i++) output[i] = input[i] > 0;
 }
 
+void pll_cc_init_2nd_order_IIR(pll_t* p, float bandwidth, float gain, float dampling_factor)
+{
+	p->filter_taps_a[0] = 1;
+	p->filter_taps_a[1] = -2;
+	p->filter_taps_a[2] = 1;
+	float tau1 = gain / (bandwidth*bandwidth);
+	float tau2 = (2*dampling_factor) / bandwidth;
+	p->filter_taps_b[0] = 4*(gain/tau1)*(1+tau2/2);
+	p->filter_taps_b[1] = 8*(gain/tau1);
+	p->filter_taps_b[2] = 4*(gain/tau1)*(1-tau2/2);
+	p->last_filter_outputs[0]=p->last_filter_outputs[1]=p->last_filter_inputs[0]=p->last_filter_inputs[1]=0;
+	p->dphase=p->output_phase=0;
+}
+
+void pll_cc_init_1st_order_IIR(pll_t* p, float alpha)
+{
+	p->alpha = alpha;
+	p->dphase=p->output_phase=0;
+}
+
+
+void pll_cc(pll_t* p, complexf* input, float* output_dphase, complexf* output_vco, int input_size)
+{
+	for(int i=0;i<input_size;i++)
+	{
+		p->output_phase += p->dphase;
+		while(p->output_phase>PI) p->output_phase-=2*PI;
+		while(p->output_phase<-PI) p->output_phase+=2*PI;
+		if(output_vco) //we don't output anything if it is a NULL pointer
+		{
+			iof(output_vco,i) = cos(p->output_phase);
+			qof(output_vco,i) = sin(p->output_phase);
+		}
+
+		float input_phase = atan2(iof(input,i),qof(input,i));
+		float new_dphase = input_phase - p->output_phase; //arg(input[i]/abs(input[i]) * conj(current_output_vco[i]))
+
+
+		if(p->pll_type == PLL_2ND_ORDER_IIR_LOOP_FILTER)
+		{
+			p->dphase = 0 //...
+				+ new_dphase                * p->filter_taps_b[0]
+				+ p->last_filter_inputs[1]  * p->filter_taps_b[1]
+				+ p->last_filter_inputs[0]  * p->filter_taps_b[2]
+				- p->last_filter_outputs[1] * p->filter_taps_a[1]
+				- p->last_filter_outputs[0] * p->filter_taps_a[2];
+			//dphase /= filter_taps_a[0]; //The filter taps are already normalized, a[0]==1 always, so it is not necessary.
+
+			p->last_filter_outputs[0]=p->last_filter_outputs[1];
+			p->last_filter_outputs[1]=p->dphase;
+			p->last_filter_inputs[0]=p->last_filter_inputs[1];
+			p->last_filter_inputs[1]=new_dphase;
+		}
+		else if(p->pll_type == PLL_1ST_ORDER_IIR_LOOP_FILTER)
+		{
+			p->dphase = p->dphase * (1-p->alpha) + new_dphase * p->alpha;
+		}
+		else return;
+		if(output_dphase) output_dphase[i] = p->dphase;
+	}
+}
+
 /*
   _____        _                                            _
  |  __ \      | |                                          (_)

libcsdr.h

@@ -236,3 +236,30 @@ typedef struct serial_line_s
 
 void serial_line_decoder_f_u8(serial_line_t* s, float* input, unsigned char* output, int input_size);
 void binary_slicer_f_u8(float* input, unsigned char* output, int input_size);
+
+
+typedef enum pll_type_e
+{
+	PLL_1ST_ORDER_IIR_LOOP_FILTER=1,
+	PLL_2ND_ORDER_IIR_LOOP_FILTER=2
+} pll_type_t;
+
+typedef struct pll_s
+{
+	pll_type_t pll_type;
+	//common:
+	float output_phase;
+	float dphase;
+	float frequency;
+	//2nd order IIR:
+	float last_filter_outputs[2];
+	float last_filter_inputs[2];
+	float filter_taps_a[3];
+	float filter_taps_b[3];
+	//1st order IIR:
+	float alpha;
+} pll_t;
+
+void pll_cc_init_2nd_order_IIR(pll_t* p, float bandwidth, float gain, float dampling_factor);
+void pll_cc_init_1st_order_IIR(pll_t* p, float alpha);
+void pll_cc(pll_t* p, complexf* input, float* output_dphase, complexf* output_vco, int input_size);