Working on 2nd order IIR loop filter
This commit is contained in:
parent
c0b4706592
commit
e084341ca2
3 changed files with 21 additions and 45 deletions
13
csdr.c
13
csdr.c
|
@ -1972,11 +1972,12 @@ int main(int argc, char *argv[])
|
|||
{
|
||||
float bandwidth = 0.01, 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]);
|
||||
if(argc>4) sscanf(argv[4],"%f",&damping_factor);
|
||||
if(argc>5) sscanf(argv[5],"%f",&ko);
|
||||
if(argc>6) sscanf(argv[6],"%f",&kd);
|
||||
pll_cc_init_2nd_order_IIR(&pll, bandwidth, ko, kd, 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");
|
||||
|
||||
|
@ -1986,7 +1987,7 @@ int main(int argc, char *argv[])
|
|||
{
|
||||
FEOF_CHECK;
|
||||
FREAD_C;
|
||||
fprintf(stderr, "| ai");
|
||||
fprintf(stderr, "| i");
|
||||
pll_cc(&pll, (complexf*)input_buffer, output_buffer, NULL, the_bufsize);
|
||||
fwrite(output_buffer, sizeof(float), the_bufsize, stdout);
|
||||
// pll_cc(&pll, (complexf*)input_buffer, NULL, (complexf*)output_buffer, the_bufsize);
|
||||
|
|
44
libcsdr.c
44
libcsdr.c
|
@ -1336,23 +1336,13 @@ 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)
|
||||
void pll_cc_init_2nd_order_IIR(pll_t* p, float bandwidth, float ko, float kd, float float damping_factor)
|
||||
{
|
||||
float k=0.9;
|
||||
p->filter_taps_a[0] = k*1;
|
||||
p->filter_taps_a[1] = k*(-2);
|
||||
p->filter_taps_a[2] = k*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;
|
||||
|
||||
p->filter_taps_b[0] = 0.02868000;
|
||||
p->filter_taps_b[1] = 0.00080000;
|
||||
p->filter_taps_b[2] = -0.02788000;
|
||||
float bandwidth_omega 2*M_PI*bandwidth;
|
||||
p->alpha = (damping_factor*2*bandwidth_omega)/(ko*kd);
|
||||
float sampling_rate = 1; //the bandwidth is normalized to the sampling rate
|
||||
p->beta = (bandwidth_omega*bandwidth_omega)/(sampling_rate*ko*kd);
|
||||
p->dphase = p->output_phase=0;
|
||||
// s=tf([0.02868000,0.00080000,-0.02788000],[1 -2 1]); pzmap(s)
|
||||
}
|
||||
|
||||
|
@ -1376,30 +1366,20 @@ void pll_cc(pll_t* p, complexf* input, float* output_dphase, complexf* output_nc
|
|||
qof(output_nco,i) = cos(p->output_phase);
|
||||
}
|
||||
|
||||
//ket komplex szam szorzataval inkabb
|
||||
|
||||
//calculating error from phase offset
|
||||
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]))
|
||||
while(new_dphase>PI) new_dphase-=2*PI;
|
||||
while(new_dphase<-PI) new_dphase+=2*PI;
|
||||
|
||||
//calculating error from two complex samples
|
||||
|
||||
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;
|
||||
//static float lasttemp;
|
||||
//lasttemp = p->beta *
|
||||
//p->dphase = new_dphase * p->alpha;
|
||||
static float lasttemp;
|
||||
p->dphase = new_dphase * p->alpha + lasttemp;
|
||||
lasttemp += new_dphase * p->beta;
|
||||
|
||||
while(p->dphase>PI) p->dphase-=2*PI; //ez nem fog kelleni
|
||||
while(p->dphase<-PI) p->dphase+=2*PI;
|
||||
|
|
|
@ -251,15 +251,10 @@ typedef struct pll_s
|
|||
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;
|
||||
float beta;
|
||||
} pll_t;
|
||||
|
||||
void pll_cc_init_2nd_order_IIR(pll_t* p, float bandwidth, float gain, float dampling_factor);
|
||||
void pll_cc_init_2nd_order_IIR(pll_t* p, float bandwidth, float ko, float kd, float float damping_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_nco, int input_size);
|
||||
|
|
Loading…
Reference in a new issue