You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 

200 lines
4.2 KiB

  1. /*
  2. * Atpdec
  3. * Copyright (c) 2003 by Thierry Leconte (F4DWV)
  4. *
  5. * $Id$
  6. *
  7. * This library is free software; you can redistribute it and/or modify
  8. * it under the terms of the GNU Library General Public License as
  9. * published by the Free Software Foundation; either version 2 of
  10. * the License, or (at your option) any later version.
  11. *
  12. * This program is distributed in the hope that it will be useful,
  13. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  15. * GNU Library General Public License for more details.
  16. *
  17. * You should have received a copy of the GNU Library General Public
  18. * License along with this library; if not, write to the Free Software
  19. * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20. *
  21. */
  22. #include <math.h>
  23. #include <filter.h>
  24. #include <filtercoeff.h>
  25. #define Fe 11025
  26. #define PixelLine 2080
  27. #define Fp (2*PixelLine)
  28. #define RSMULT 10
  29. #define Fi (Fp*RSMULT)
  30. static double FreqOsc=2400.0/Fe;
  31. static double FreqLine=1.0;
  32. extern int getsample(float *inbuff ,int nb);
  33. static float pll(float In)
  34. {
  35. /* pll coeff */
  36. #define K1 5e-3
  37. #define K2 3e-6
  38. static double PhaseOsc=0.0;
  39. static iirbuff_t Ifilterbuff,Qfilterbuff;
  40. double Io,Qo;
  41. double Ip,Qp;
  42. double DPhi;
  43. double DF;
  44. /* quadrature oscillator */
  45. Io=cos(PhaseOsc); Qo=sin(PhaseOsc);
  46. /* phase detector */
  47. Ip=iir(In*Io,&Ifilterbuff,&PhaseFilterCf);
  48. Qp=iir(In*Qo,&Qfilterbuff,&PhaseFilterCf);
  49. DPhi=-atan2(Qp,Ip)/M_PI;
  50. /* loop filter */
  51. DF=K1*DPhi+FreqOsc;
  52. FreqOsc+=K2*DPhi;
  53. PhaseOsc+=2.0*M_PI*DF;
  54. if (PhaseOsc > M_PI) PhaseOsc-=2.0*M_PI;
  55. if (PhaseOsc <= -M_PI) PhaseOsc+=2.0*M_PI;
  56. return (float)(In*Io);
  57. }
  58. static double fr=2400.0/Fe;
  59. static double offset=0.0;
  60. getamp(float *ambuff,int nb)
  61. {
  62. #define BLKIN 1024
  63. float inbuff[BLKIN];
  64. int n;
  65. int res;
  66. res=getsample(inbuff,nb>BLKIN?BLKIN:nb);
  67. for(n=0;n<res;n++) {
  68. ambuff[n]=pll(inbuff[n]);
  69. fr=0.25*FreqOsc+0.75*fr;
  70. }
  71. return(res);
  72. }
  73. int getpixelv(float *pvbuff,int nb)
  74. {
  75. #define BLKAMP 256
  76. static float ambuff[BLKAMP];
  77. static int nam=0;
  78. static int idxam=0;
  79. int n;
  80. for(n=0;n<nb;n++) {
  81. double mult;
  82. int shift;
  83. if(nam<BLKAMP) {
  84. int res;
  85. memmove(ambuff,&(ambuff[idxam]),nam*sizeof(float));
  86. idxam=0;
  87. res=getamp(&(ambuff[nam]),BLKAMP-nam);
  88. nam+=res;
  89. if(nam<BLKAMP) return(n);
  90. }
  91. mult=(double)Fi*fr/2400.0*FreqLine;
  92. pvbuff[n]=rsfir(&(ambuff[idxam]),rsfilter,RSFilterLen,offset,mult)*mult*3*256.0;
  93. shift=(int)((RSMULT-offset+mult-1)/mult);
  94. offset=shift*mult+offset-RSMULT;
  95. idxam+=shift;nam-=shift;
  96. }
  97. return(nb);
  98. }
  99. int getpixelrow(float *pixelv)
  100. {
  101. static float pixels[PixelLine+SyncFilterLen];
  102. static int npv=0;
  103. static int synced=0;
  104. static double max=0.0;
  105. double corr,ecorr,lcorr;
  106. double ph;
  107. int n,res;
  108. if(npv>0)
  109. memmove(pixelv,pixels,npv*sizeof(float));
  110. if(npv<SyncFilterLen+2) {
  111. res=getpixelv(&(pixelv[npv]),SyncFilterLen+2-npv);
  112. npv+=res;
  113. if(npv<SyncFilterLen+2)
  114. return(0);
  115. }
  116. /* test sync */
  117. corr=fir(&(pixelv[1]),Sync,SyncFilterLen);
  118. ecorr=fir(pixelv,Sync,SyncFilterLen);
  119. lcorr=fir(&(pixelv[2]),Sync,SyncFilterLen);
  120. FreqLine=1.0+(ecorr-lcorr)/corr/PixelLine/4.0;
  121. if(corr < 0.75*max) {
  122. synced=0;
  123. FreqLine=1.0;
  124. }
  125. max=corr;
  126. if(synced<8) {
  127. int shift,mshift;
  128. if(npv<PixelLine+SyncFilterLen) {
  129. res=getpixelv(&(pixelv[npv]),PixelLine+SyncFilterLen-npv);
  130. npv+=res;
  131. if(npv<PixelLine+SyncFilterLen)
  132. return(0);
  133. }
  134. /* lookup sync start */
  135. mshift=0;
  136. for(shift=1;shift<PixelLine;shift++) {
  137. double corr;
  138. corr=fir(&(pixelv[shift+1]),Sync,SyncFilterLen);
  139. if(corr>max) {
  140. mshift=shift;
  141. max=corr;
  142. }
  143. }
  144. if(mshift !=0) {
  145. memmove(pixelv,&(pixelv[mshift]),(npv-mshift)*sizeof(float));
  146. npv-=mshift;
  147. synced=0;
  148. FreqLine=1.0;
  149. } else
  150. synced+=1;
  151. }
  152. if(npv<PixelLine) {
  153. res=getpixelv(&(pixelv[npv]),PixelLine-npv);
  154. npv+=res;
  155. if(npv<PixelLine) return(0);
  156. }
  157. if(npv==PixelLine) {
  158. npv=0;
  159. } else {
  160. memmove(pixels,&(pixelv[PixelLine]),(npv-PixelLine)*sizeof(float));
  161. npv-=PixelLine;
  162. }
  163. return (1);
  164. }