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.

dsp.c 4.4 KiB

21 years ago
21 years ago
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214
  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 265
  29. #define Fi (Fp*RSMULT)
  30. static double FreqOsc=2.0*2400.0/Fe;
  31. static double FreqLine=1.0;
  32. extern int getsample(float *inbuff ,int nb);
  33. static float pll(float Iv, float Qv)
  34. {
  35. /* pll coeff */
  36. #define K1 0.01
  37. #define K2 5e-6
  38. static double PhaseOsc=0.0;
  39. double DPhi;
  40. double DF;
  41. double Phi;
  42. double I,Q;
  43. double Io,Qo;
  44. /* quadrature oscillator */
  45. Phi=M_PI*PhaseOsc;
  46. Io=cos(Phi); Qo=sin(Phi);
  47. /* phase detector */
  48. I=Iv*Io+Qv*Qo;
  49. Q=Qv*Io-Iv*Qo;
  50. DPhi=atan2(Q,I)/M_PI;
  51. /* update */
  52. DF=K1*DPhi+FreqOsc;
  53. FreqOsc+=K2*DPhi;
  54. PhaseOsc+=DF;
  55. if (PhaseOsc > 1.0) PhaseOsc-=2.0;
  56. if (PhaseOsc <= -1.0) PhaseOsc+=2.0;
  57. return (float)I;
  58. }
  59. static double fr=4800.0/Fe;
  60. static double offset=0.0;
  61. getamp(float *ambuff,int nb)
  62. {
  63. #define BLKIN 4096
  64. static float inbuff[BLKIN];
  65. static int nin=0;
  66. static int idxin=0;
  67. int n;
  68. for(n=0;n<nb;n++) {
  69. float I,Q;
  70. if(nin<IQFilterLen) {
  71. int res;
  72. memmove(inbuff,&(inbuff[idxin]),nin*sizeof(float));
  73. idxin=0;
  74. res=getsample(&(inbuff[nin]),BLKIN-nin);
  75. nin+=res;
  76. if(nin<IQFilterLen) return(n);
  77. }
  78. iqfir(&(inbuff[idxin]),Icoeff,Qcoeff,IQFilterLen,&I,&Q);
  79. ambuff[n]=pll(I,Q);
  80. fr=0.25*FreqOsc+0.75*fr;
  81. idxin++;nin--;
  82. }
  83. return(nb);
  84. }
  85. int getpixelv(float *pvbuff,int nb)
  86. {
  87. #define BLKAMP 256
  88. static float ambuff[BLKAMP];
  89. static int nam=0;
  90. static int idxam=0;
  91. int n;
  92. for(n=0;n<nb;n++) {
  93. double mult;
  94. int shift;
  95. if(nam<15) {
  96. int res;
  97. memmove(ambuff,&(ambuff[idxam]),nam*sizeof(float));
  98. idxam=0;
  99. res=getamp(&(ambuff[nam]),BLKAMP-nam);
  100. nam+=res;
  101. if(nam<15) return(n);
  102. }
  103. mult=(double)Fi/4800.0*fr*FreqLine;
  104. pvbuff[n]=rsfir(&(ambuff[idxam]),rsfilter,RSFilterLen,offset,mult)*RSMULT*128.0;
  105. shift=(int)((RSMULT-offset+mult-1)/mult);
  106. offset=shift*mult+offset-RSMULT;
  107. idxam+=shift;nam-=shift;
  108. }
  109. return(nb);
  110. }
  111. int getpixelrow(float *pixelv)
  112. {
  113. static float pixels[PixelLine+SyncFilterLen];
  114. static int npv=0;
  115. static int synced=0;
  116. static double max=0.0;
  117. float corr,qcorr;
  118. double ph;
  119. int n,res;
  120. if(npv>0)
  121. memmove(pixelv,pixels,npv*sizeof(float));
  122. if(npv<SyncFilterLen) {
  123. res=getpixelv(&(pixelv[npv]),SyncFilterLen-npv);
  124. npv+=res;
  125. if(npv<SyncFilterLen)
  126. return(0);
  127. }
  128. iqfir(pixelv,ISync,QSync,SyncFilterLen,&corr,&qcorr);
  129. ph=atan2((double)qcorr,(double)corr)/M_PI*2.0;
  130. max=corr;
  131. FreqLine=(double)(PixelLine-SyncFilterLen-ph)/(double)(PixelLine-SyncFilterLen);
  132. if(corr < 0.75*max) {
  133. synced=0;
  134. FreqLine=1.0;
  135. }
  136. if(ph >=1.0 || ph <= -1.0) {
  137. synced=0;
  138. FreqLine=1.0;
  139. }
  140. if(synced<8) {
  141. int shift,mshift;
  142. if(npv<PixelLine+SyncFilterLen) {
  143. res=getpixelv(&(pixelv[npv]),PixelLine+SyncFilterLen-npv);
  144. npv+=res;
  145. if(npv<PixelLine+SyncFilterLen)
  146. return(0);
  147. }
  148. mshift=0;
  149. for(shift=1;shift<PixelLine;shift++) {
  150. double corr;
  151. corr=fir(&(pixelv[shift]),ISync,SyncFilterLen);
  152. if(corr>max) {
  153. mshift=shift;
  154. max=corr;
  155. }
  156. }
  157. if(mshift !=0) {
  158. memmove(pixelv,&(pixelv[mshift]),(npv-mshift)*sizeof(float));
  159. npv-=mshift;
  160. synced=0;
  161. FreqLine=1.0;
  162. } else
  163. synced+=1;
  164. }
  165. if(npv<PixelLine) {
  166. res=getpixelv(&(pixelv[npv]),PixelLine-npv);
  167. npv+=res;
  168. if(npv<PixelLine) return(0);
  169. }
  170. if(npv==PixelLine) {
  171. npv=0;
  172. } else {
  173. memmove(pixels,&(pixelv[PixelLine]),(npv-PixelLine)*sizeof(float));
  174. npv-=PixelLine;
  175. }
  176. return (1);
  177. }