diff --git a/csdr.c b/csdr.c index e1dc7e1..cb02ef7 100644 --- a/csdr.c +++ b/csdr.c @@ -1970,7 +1970,7 @@ int main(int argc, char *argv[]) } else if(pll.pll_type == PLL_2ND_ORDER_IIR_LOOP_FILTER) { - float bandwidth = 0.01, gain = 1000, damping_factor = 0.707; + float bandwidth = 0.01, ko = 10, kd=100, damping_factor = 0.707; if(argc>3) sscanf(argv[3],"%f",&bandwidth); if(argc>4) sscanf(argv[4],"%f",&damping_factor); if(argc>5) sscanf(argv[5],"%f",&ko); @@ -1988,10 +1988,10 @@ int main(int argc, char *argv[]) FEOF_CHECK; FREAD_C; 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); - // fwrite(output_buffer, sizeof(complexf), the_bufsize, stdout); + // 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); + fwrite(output_buffer, sizeof(complexf), the_bufsize, stdout); fprintf(stderr, "| o"); TRY_YIELD; } diff --git a/libcsdr.c b/libcsdr.c index 16f95a8..4cb3f57 100644 --- a/libcsdr.c +++ b/libcsdr.c @@ -1336,13 +1336,13 @@ 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 ko, float kd, float float damping_factor) +void pll_cc_init_2nd_order_IIR(pll_t* p, float bandwidth, float ko, float kd, float damping_factor) { - float bandwidth_omega 2*M_PI*bandwidth; - p->alpha = (damping_factor*2*bandwidth_omega)/(ko*kd); + 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->iir_temp = p->dphase = p->output_phase = 0; // s=tf([0.02868000,0.00080000,-0.02788000],[1 -2 1]); pzmap(s) } @@ -1361,27 +1361,27 @@ void pll_cc(pll_t* p, complexf* input, float* output_dphase, complexf* output_nc while(p->output_phase>PI) p->output_phase-=2*PI; while(p->output_phase<-PI) p->output_phase+=2*PI; complexf current_nco; - iof(current_nco,i) = sin(p->output_phase); - qof(current_nco,i) = cos(p->output_phase); + iof(¤t_nco,0) = sin(p->output_phase); + qof(¤t_nco,0) = cos(p->output_phase); if(output_nco) output_nco[i] = current_nco; //we don't output anything if it is a NULL pointer //accurate phase detector: calculating error from phase offset - float input_phase = atan2(iof(input,i),qof(input,i)); - float new_dphase = input_phase - p->output_phase; - while(new_dphase>PI) new_dphase-=2*PI; - while(new_dphase<-PI) new_dphase+=2*PI; + // float input_phase = atan2(iof(input,i),qof(input,i)); + // float new_dphase = input_phase - p->output_phase; + // while(new_dphase>PI) new_dphase-=2*PI; + // while(new_dphase<-PI) new_dphase+=2*PI; //modeling analog phase detector: abs(input[i] * conj(current_nco)) - //qof(¤t_nco,0)=-qof(¤t_nco,0); //calculate conjugate - //complexf multiply_result; - //cmult(&multiply_result, &input[i], ¤t_nco); - //float new_dphase = absof(&multiply_result,0); + qof(¤t_nco,0)=-qof(¤t_nco,0); //calculate conjugate + complexf multiply_result; + cmult(&multiply_result, &input[i], ¤t_nco); + output_nco[i] = multiply_result; + float new_dphase = absof(&multiply_result,0); if(p->pll_type == PLL_2ND_ORDER_IIR_LOOP_FILTER) { - static float lasttemp; - p->dphase = new_dphase * p->alpha + lasttemp; - lasttemp += new_dphase * p->beta; + p->dphase = new_dphase * p->alpha + p->iir_temp; + p->iir_temp += new_dphase * p->beta; while(p->dphase>PI) p->dphase-=2*PI; //ez nem fog kelleni while(p->dphase<-PI) p->dphase+=2*PI; @@ -1392,7 +1392,7 @@ void pll_cc(pll_t* p, complexf* input, float* output_dphase, complexf* output_nc } else return; if(output_dphase) output_dphase[i] = -p->dphase; - // if(output_dphase) output_dphase[i] = new_dphase/10; + if(output_dphase) output_dphase[i] = new_dphase/10; } } diff --git a/libcsdr.h b/libcsdr.h index c40d5a3..b05abb2 100644 --- a/libcsdr.h +++ b/libcsdr.h @@ -48,9 +48,9 @@ typedef struct complexf_s { float i; float q; } complexf; //apply to pointers: #define iof(complexf_input_p,i) (*(((float*)complexf_input_p)+2*(i))) #define qof(complexf_input_p,i) (*(((float*)complexf_input_p)+2*(i)+1)) -#define absof(complexf_input_p,i) (sqrt(iof(complexf_input_p,i)*iof(complexf_input_p,i)+qof(complexf_input_p,i)*qof(complexf_input_p,i))) +#define absof(complexf_input_p,i) (sqrt((iof(complexf_input_p,i)*iof(complexf_input_p,i))+(qof(complexf_input_p,i)*qof(complexf_input_p,i)))) #define argof(complexf_input_p,i) (atan2(qof(complexf_input_p,i),iof(complexf_input_p,i))) -#define cmult(cfo, cfi1, cfi2) iof(cfo,0)=iof(cfi1,0)*iof(cfi2,0)-qof(cfi1,0)*qof(cfi2,0);qof(cfo,0)=iof(cfi1,0)*qof(cfi2,0)+iof(cfi2,0)*qof(cfi1,0) +#define cmult(cfo, cfi1, cfi2) {iof(cfo,0)=iof(cfi1,0)*iof(cfi2,0)-qof(cfi1,0)*qof(cfi2,0);qof(cfo,0)=iof(cfi1,0)*qof(cfi2,0)+iof(cfi2,0)*qof(cfi1,0);} //(ai+aq*j)*(bi+bq*j)=ai*bi-aq*bq+(aq*bi+ai*bq)*j #define cmultadd(cfo, cfi1, cfi2) { iof(cfo,0)+=iof(cfi1,0)*iof(cfi2,0)-qof(cfi1,0)*qof(cfi2,0);qof(cfo,0)+=iof(cfi1,0)*qof(cfi2,0)+iof(cfi2,0)*qof(cfi1,0); } #define csetnull(cf) { iof(cf,0)=0.0; qof(cf,0)=0.0; } @@ -253,8 +253,9 @@ typedef struct pll_s float frequency; float alpha; float beta; + float iir_temp; } pll_t; -void pll_cc_init_2nd_order_IIR(pll_t* p, float bandwidth, float ko, float kd, float float damping_factor); +void pll_cc_init_2nd_order_IIR(pll_t* p, float bandwidth, float ko, float kd, 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);