From a89d174ec598b8b77ffb6445645bf85a5671c80f Mon Sep 17 00:00:00 2001 From: ha7ilm Date: Mon, 9 May 2016 12:59:08 +0200 Subject: [PATCH] Added pll_cc --- csdr.c | 39 ++++++++++++++++++++++++++++++++++ libcsdr.c | 62 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ libcsdr.h | 27 ++++++++++++++++++++++++ 3 files changed, 128 insertions(+) diff --git a/csdr.c b/csdr.c index 50e8e82..17d5b64 100644 --- a/csdr.c +++ b/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; diff --git a/libcsdr.c b/libcsdr.c index 3126440..2bfbd93 100644 --- a/libcsdr.c +++ b/libcsdr.c @@ -1336,6 +1336,68 @@ void binary_slicer_f_u8(float* input, unsigned char* output, int input_size) for(int i=0;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;ioutput_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; + } +} + /* _____ _ _ | __ \ | | (_) diff --git a/libcsdr.h b/libcsdr.h index b1b116d..ff89568 100644 --- a/libcsdr.h +++ b/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);