Added pll_cc

This commit is contained in:
ha7ilm 2016-05-09 12:59:08 +02:00
parent d79807a67c
commit a89d174ec5
3 changed files with 128 additions and 0 deletions

39
csdr.c
View file

@ -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;

View file

@ -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;
}
}
/*
_____ _ _
| __ \ | | (_)

View file

@ -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);