Nie możesz wybrać więcej, niż 25 tematów Tematy muszą się zaczynać od litery lub cyfry, mogą zawierać myślniki ('-') i mogą mieć do 35 znaków.
 
 
 
 
 

276 wiersze
5.5 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 <stdlib.h>
  23. #include <string.h>
  24. #include <math.h>
  25. #ifndef M_PI
  26. #define M_PI 3.14159265358979323846 /* for OS that don't know it */
  27. #endif /* */
  28. #include "filter.h"
  29. #include "filtercoeff.h"
  30. extern int getsample(float *inbuff, int nb);
  31. #define Fc 2400.0
  32. #define DFc 50.0
  33. #define PixelLine 2080
  34. #define Fp (2*PixelLine)
  35. #define RSMULT 15
  36. #define Fi (Fp*RSMULT)
  37. static double Fe;
  38. static double offset = 0.0;
  39. static double FreqLine = 1.0;
  40. static double FreqOsc;
  41. static double K1,K2;
  42. int init_dsp(double F)
  43. {
  44. if(F > Fi) return (1);
  45. if(F < Fp) return (-1);
  46. Fe=F;
  47. K1=DFc/Fe;
  48. K2=K1*K1/2.0;
  49. FreqOsc=Fc/Fe;
  50. return(0);
  51. }
  52. /* fast phase estimator */
  53. static inline double Phase(double I,double Q)
  54. {
  55. double angle,r;
  56. int s;
  57. if(I==0.0 && Q==0.0)
  58. return(0.0);
  59. if (Q < 0) {
  60. s=-1;
  61. Q=-Q;
  62. } else {
  63. s=1;
  64. }
  65. if (I>=0) {
  66. r = (I - Q) / (I + Q);
  67. angle = 0.25 - 0.25 * r;
  68. } else {
  69. r = (I + Q) / (Q - I);
  70. angle = 0.75 - 0.25 * r;
  71. }
  72. if(s>0)
  73. return(angle);
  74. else
  75. return(-angle);
  76. }
  77. static double pll(double I, double Q)
  78. {
  79. /* pll coeff */
  80. static double PhaseOsc = 0.0;
  81. double Io, Qo;
  82. double Ip, Qp;
  83. double DPhi;
  84. /* quadrature oscillator */
  85. Io = cos(PhaseOsc);
  86. Qo = sin(PhaseOsc);
  87. /* phase detector */
  88. Ip = I*Io+Q*Qo;
  89. Qp = Q*Io-I*Qo;
  90. DPhi = Phase(Ip,Qp);
  91. /* loop filter */
  92. PhaseOsc += 2.0 * M_PI * (K1 * DPhi + FreqOsc);
  93. if (PhaseOsc > M_PI)
  94. PhaseOsc -= 2.0 * M_PI;
  95. if (PhaseOsc <= -M_PI)
  96. PhaseOsc += 2.0 * M_PI;
  97. FreqOsc += K2 * DPhi;
  98. if (FreqOsc > ((Fc + DFc) / Fe))
  99. FreqOsc = (Fc + DFc) / Fe;
  100. if (FreqOsc < ((Fc - DFc) / Fe))
  101. FreqOsc = (Fc - DFc) / Fe;
  102. return (Ip);
  103. }
  104. static int getamp(double *ambuff, int nb)
  105. {
  106. #define BLKIN 1024
  107. static float inbuff[BLKIN];
  108. static int idxin=0;
  109. static int nin=0;
  110. int n;
  111. for (n = 0; n < nb; n++) {
  112. double I,Q;
  113. if (nin < IQFilterLen*2+2) {
  114. int res;
  115. memmove(inbuff, &(inbuff[idxin]), nin * sizeof(float));
  116. idxin = 0;
  117. res = getsample(&(inbuff[nin]), BLKIN - nin);
  118. nin += res;
  119. if (nin < IQFilterLen*2+2)
  120. return (n);
  121. }
  122. iqfir(&inbuff[idxin],iqfilter,IQFilterLen,&I,&Q);
  123. ambuff[n] = pll(I,Q);
  124. idxin += 1;
  125. nin -= 1;
  126. }
  127. return (n);
  128. }
  129. int getpixelv(float *pvbuff, int nb)
  130. {
  131. #define BLKAMP 1024
  132. static double ambuff[BLKAMP];
  133. static int nam = 0;
  134. static int idxam = 0;
  135. int n,m;
  136. double mult;
  137. mult = (double) Fi/Fe*FreqLine;
  138. m=RSFilterLen/mult+1;
  139. for (n = 0; n < nb; n++) {
  140. int shift;
  141. if (nam < m) {
  142. int res;
  143. memmove(ambuff, &(ambuff[idxam]), nam * sizeof(double));
  144. idxam = 0;
  145. res = getamp(&(ambuff[nam]), BLKAMP - nam);
  146. nam += res;
  147. if (nam < m)
  148. return (n);
  149. }
  150. pvbuff[n] = rsfir(&(ambuff[idxam]), rsfilter, RSFilterLen, offset, mult) * mult * 256.0;
  151. //printf("%g\n",pvbuff[n]);
  152. shift = ((int) floor((RSMULT - offset) / mult))+1;
  153. offset = shift*mult+offset-RSMULT ;
  154. idxam += shift;
  155. nam -= shift;
  156. }
  157. return (nb);
  158. }
  159. int getpixelrow(float *pixelv)
  160. {
  161. static float pixels[PixelLine + SyncFilterLen];
  162. static int npv = 0;
  163. static int synced = 0;
  164. static double max = 0.0;
  165. double corr, ecorr, lcorr;
  166. int res;
  167. if (npv > 0)
  168. memmove(pixelv, pixels, npv * sizeof(float));
  169. if (npv < SyncFilterLen + 2) {
  170. res = getpixelv(&(pixelv[npv]), SyncFilterLen + 2 - npv);
  171. npv += res;
  172. if (npv < SyncFilterLen + 2)
  173. return (0);
  174. }
  175. /* test sync */
  176. ecorr = fir(pixelv, Sync, SyncFilterLen);
  177. corr = fir(&(pixelv[1]), Sync, SyncFilterLen);
  178. lcorr = fir(&(pixelv[2]), Sync, SyncFilterLen);
  179. FreqLine = 1.0+((ecorr-lcorr) / corr / PixelLine / 4.0);
  180. if (corr < 0.75 * max) {
  181. synced = 0;
  182. FreqLine = 1.0;
  183. }
  184. max = corr;
  185. if (synced < 8) {
  186. int shift, mshift;
  187. if (npv < PixelLine + SyncFilterLen) {
  188. res =
  189. getpixelv(&(pixelv[npv]), PixelLine + SyncFilterLen - npv);
  190. npv += res;
  191. if (npv < PixelLine + SyncFilterLen)
  192. return (0);
  193. }
  194. /* lookup sync start */
  195. mshift = 0;
  196. for (shift = 1; shift < PixelLine; shift++) {
  197. double corr;
  198. corr = fir(&(pixelv[shift + 1]), Sync, SyncFilterLen);
  199. if (corr > max) {
  200. mshift = shift;
  201. max = corr;
  202. }
  203. }
  204. if (mshift != 0) {
  205. memmove(pixelv, &(pixelv[mshift]),
  206. (npv - mshift) * sizeof(float));
  207. npv -= mshift;
  208. synced = 0;
  209. FreqLine = 1.0;
  210. } else
  211. synced += 1;
  212. }
  213. if (npv < PixelLine) {
  214. res = getpixelv(&(pixelv[npv]), PixelLine - npv);
  215. npv += res;
  216. if (npv < PixelLine)
  217. return (0);
  218. }
  219. if (npv == PixelLine) {
  220. npv = 0;
  221. } else {
  222. memmove(pixels, &(pixelv[PixelLine]),
  223. (npv - PixelLine) * sizeof(float));
  224. npv -= PixelLine;
  225. }
  226. return (1);
  227. }