Working on 2nd order IIR loop filter

This commit is contained in:
ha7ilm 2016-05-11 08:48:31 +02:00
parent c0b4706592
commit e084341ca2
3 changed files with 21 additions and 45 deletions

13
csdr.c
View file

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

View file

@ -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;
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;
p->filter_taps_b[0] = 0.02868000;
p->filter_taps_b[1] = 0.00080000;
p->filter_taps_b[2] = -0.02788000;
// 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;

View file

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