Added pll_cc
This commit is contained in:
parent
d79807a67c
commit
a89d174ec5
3 changed files with 128 additions and 0 deletions
39
csdr.c
39
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;
|
||||
|
|
62
libcsdr.c
62
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;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
_____ _ _
|
||||
| __ \ | | (_)
|
||||
|
|
27
libcsdr.h
27
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);
|
||||
|
|
Loading…
Reference in a new issue