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.
 
 
 
 
 

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