您最多选择25个主题 主题必须以字母或数字开头,可以包含连字符 (-),并且长度不得超过35个字符

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202
  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. #ifndef M_PI
  24. #define M_PI 3.14159265358979323846 /* for OS that don't know it */
  25. #endif
  26. #include "filter.h"
  27. #include "filtercoeff.h"
  28. #define Fe 11025
  29. #define PixelLine 2080
  30. #define Fp (2*PixelLine)
  31. #define RSMULT 10
  32. #define Fi (Fp*RSMULT)
  33. static double FreqOsc=2400.0/Fe;
  34. static double FreqLine=1.0;
  35. extern int getsample(float *inbuff ,int nb);
  36. static float pll(float In)
  37. {
  38. /* pll coeff */
  39. #define K1 5e-3
  40. #define K2 3e-6
  41. static double PhaseOsc=0.0;
  42. static iirbuff_t Ifilterbuff,Qfilterbuff;
  43. double Io,Qo;
  44. double Ip,Qp;
  45. double DPhi;
  46. double DF;
  47. /* quadrature oscillator */
  48. Io=cos(PhaseOsc); Qo=sin(PhaseOsc);
  49. /* phase detector */
  50. Ip=iir(In*Io,&Ifilterbuff,&PhaseFilterCf);
  51. Qp=iir(In*Qo,&Qfilterbuff,&PhaseFilterCf);
  52. DPhi=-atan2(Qp,Ip)/M_PI;
  53. /* loop filter */
  54. DF=K1*DPhi+FreqOsc;
  55. FreqOsc+=K2*DPhi;
  56. PhaseOsc+=2.0*M_PI*DF;
  57. if (PhaseOsc > M_PI) PhaseOsc-=2.0*M_PI;
  58. if (PhaseOsc <= -M_PI) PhaseOsc+=2.0*M_PI;
  59. return (float)(In*Io);
  60. }
  61. static double fr=2400.0/Fe;
  62. static double offset=0.0;
  63. getamp(float *ambuff,int nb)
  64. {
  65. #define BLKIN 1024
  66. float inbuff[BLKIN];
  67. int n;
  68. int res;
  69. res=getsample(inbuff,nb>BLKIN?BLKIN:nb);
  70. for(n=0;n<res;n++) {
  71. ambuff[n]=pll(inbuff[n]);
  72. fr=0.25*FreqOsc+0.75*fr;
  73. }
  74. return(res);
  75. }
  76. int getpixelv(float *pvbuff,int nb)
  77. {
  78. #define BLKAMP 256
  79. static float ambuff[BLKAMP];
  80. static int nam=0;
  81. static int idxam=0;
  82. int n;
  83. for(n=0;n<nb;n++) {
  84. double mult;
  85. int shift;
  86. if(nam<BLKAMP) {
  87. int res;
  88. memmove(ambuff,&(ambuff[idxam]),nam*sizeof(float));
  89. idxam=0;
  90. res=getamp(&(ambuff[nam]),BLKAMP-nam);
  91. nam+=res;
  92. if(nam<BLKAMP) return(n);
  93. }
  94. mult=(double)Fi*fr/2400.0*FreqLine;
  95. pvbuff[n]=rsfir(&(ambuff[idxam]),rsfilter,RSFilterLen,offset,mult)*mult*3*256.0;
  96. shift=(int)((RSMULT-offset+mult-1)/mult);
  97. offset=shift*mult+offset-RSMULT;
  98. idxam+=shift;nam-=shift;
  99. }
  100. return(nb);
  101. }
  102. int getpixelrow(float *pixelv)
  103. {
  104. static float pixels[PixelLine+SyncFilterLen];
  105. static int npv=0;
  106. static int synced=0;
  107. static double max=0.0;
  108. double corr,ecorr,lcorr;
  109. double ph;
  110. int n,res;
  111. if(npv>0)
  112. memmove(pixelv,pixels,npv*sizeof(float));
  113. if(npv<SyncFilterLen+2) {
  114. res=getpixelv(&(pixelv[npv]),SyncFilterLen+2-npv);
  115. npv+=res;
  116. if(npv<SyncFilterLen+2)
  117. return(0);
  118. }
  119. /* test sync */
  120. corr=fir(&(pixelv[1]),Sync,SyncFilterLen);
  121. ecorr=fir(pixelv,Sync,SyncFilterLen);
  122. lcorr=fir(&(pixelv[2]),Sync,SyncFilterLen);
  123. FreqLine=1.0+(ecorr-lcorr)/corr/PixelLine/4.0;
  124. if(corr < 0.75*max) {
  125. synced=0;
  126. FreqLine=1.0;
  127. }
  128. max=corr;
  129. if(synced<8) {
  130. int shift,mshift;
  131. if(npv<PixelLine+SyncFilterLen) {
  132. res=getpixelv(&(pixelv[npv]),PixelLine+SyncFilterLen-npv);
  133. npv+=res;
  134. if(npv<PixelLine+SyncFilterLen)
  135. return(0);
  136. }
  137. /* lookup sync start */
  138. mshift=0;
  139. for(shift=1;shift<PixelLine;shift++) {
  140. double corr;
  141. corr=fir(&(pixelv[shift+1]),Sync,SyncFilterLen);
  142. if(corr>max) {
  143. mshift=shift;
  144. max=corr;
  145. }
  146. }
  147. if(mshift !=0) {
  148. memmove(pixelv,&(pixelv[mshift]),(npv-mshift)*sizeof(float));
  149. npv-=mshift;
  150. synced=0;
  151. FreqLine=1.0;
  152. } else
  153. synced+=1;
  154. }
  155. if(npv<PixelLine) {
  156. res=getpixelv(&(pixelv[npv]),PixelLine-npv);
  157. npv+=res;
  158. if(npv<PixelLine) return(0);
  159. }
  160. if(npv==PixelLine) {
  161. npv=0;
  162. } else {
  163. memmove(pixels,&(pixelv[PixelLine]),(npv-PixelLine)*sizeof(float));
  164. npv-=PixelLine;
  165. }
  166. return (1);
  167. }