daily pastebin goal
29%
SHARE
TWEET

Untitled

a guest Aug 21st, 2016 8 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <stdio.h>
  2. #include <stdint.h>
  3.  
  4. static int samp;
  5. static int nco_out;
  6.  
  7. static uint16_t phacc=0; //phase accumulator
  8. static uint16_t control_word=1;
  9.  
  10. static float f1=0,f2=0; //frequencies
  11.  
  12.  
  13. void nco(void){  //advance NCO
  14.   phacc+=control_word;
  15.   nco_out=((phacc&0x8000)!=0);
  16. }
  17.  
  18. int div10(int a){  //frequency divider
  19.   static int prev=0,n,output=0;
  20.   if(a!=prev) { //edge detect a
  21.     n++;
  22.     if(n==10){
  23.       n=0;
  24.       output=1-output;
  25.     }
  26.   }
  27.   prev=a;
  28.   return output;
  29. }
  30.  
  31. static int Qa=0,Qb=0; //phase comparator flip flops
  32.  
  33. void comp(int a,int b){ //phase comparator
  34.   static int prev_a,prev_b;
  35.  
  36.   int rst=!(Qa&&Qb);
  37.   Qa=((a&&!prev_a)||Qa)&&rst;
  38.   Qb=((b&&!prev_b)||Qb)&&rst;
  39.   prev_a=a;
  40.   prev_b=b;
  41.  
  42.   //  printf("Qa: %d\t Qb: %d\n",Qa,Qb);
  43.  
  44. }
  45.  
  46. void input_counter(void){  //input frequency counter
  47.   static int prev_samp=0,t=0;
  48.  
  49.   if(samp&&!prev_samp){ //edge detect input samples
  50.     f1=8000.0f/t;
  51.     t=0;
  52.   }
  53.   else t++;
  54.   prev_samp=samp;
  55. }
  56.  
  57. void output_counter(void){ //NCO frequency counter
  58.   static int prev_nco=0,t=0;
  59.  
  60.   if(nco_out&&!prev_nco){ //edge detect NCO
  61.     f2=8000.f/t;
  62.     t=0;
  63.   }
  64.   else t++;
  65.   prev_nco=nco_out;
  66. }
  67.  
  68.  
  69. void run(void){
  70.  
  71.   comp(samp,div10(nco_out));
  72.   //comp(samp,nco_out);
  73.   nco();
  74.   input_counter();
  75.   output_counter();
  76.  
  77.   //Qa=1: fNCO<fWAV
  78.   //Qb=1: fNCO>fWAV
  79.  
  80.   float filt(float);
  81.   static float freq=0;
  82.  
  83.   freq=freq-filt(Qa-Qb)/128;
  84.   control_word=freq*8.192;
  85.  
  86.  done:  printf("f1:%2.3f\t f2:%2.3f\t\n",f1,f2);
  87.  
  88. }
  89.  
  90. float filt(float a){
  91.   static float last;
  92.   static float deriv=200;
  93.  
  94.   float res= a+(a-last)*deriv;
  95.  
  96.   last=a;
  97.   return res;
  98. }
  99.  
  100. int main(void){
  101.  
  102.   FILE *f=fopen("1.wav","rb"); //must be 8 kHz, 16 bit signed
  103.  
  104.  
  105.   for(int i=0;i<44;i++) //skip header
  106.     fgetc(f);
  107.  
  108.   int16_t t;
  109.   while(fread(&t,1,2,f)){
  110.     samp=(t>0);
  111.     run();
  112.   }
  113.   fclose(f);
  114.  
  115.  
  116. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top