Current status (PLL modified)

This commit is contained in:
ha7ilm 2016-05-18 14:56:05 +02:00
parent ed35bb96e7
commit b36b01e9cf
3 changed files with 27 additions and 26 deletions

10
csdr.c
View file

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

View file

@ -1336,13 +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 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;
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(&current_nco,0) = sin(p->output_phase);
qof(&current_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(&current_nco,0)=-qof(&current_nco,0); //calculate conjugate
//complexf multiply_result;
//cmult(&multiply_result, &input[i], &current_nco);
//float new_dphase = absof(&multiply_result,0);
qof(&current_nco,0)=-qof(&current_nco,0); //calculate conjugate
complexf multiply_result;
cmult(&multiply_result, &input[i], &current_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;
}
}

View file

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