Du kan inte välja fler än 25 ämnen Ämnen måste starta med en bokstav eller siffra, kan innehålla bindestreck ('-') och vara max 35 tecken långa.
 
 
 
 
 

211 rader
4.8 KiB

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