Вы не можете выбрать более 25 тем Темы должны начинаться с буквы или цифры, могут содержать дефисы(-) и должны содержать не более 35 символов.
 
 
 
 
 

296 строки
6.6 KiB

  1. /*
  2. * This file is part of Aptdec.
  3. * Copyright (c) 2004-2009 Thierry Leconte (F4DWV), Xerbo (xerbo@protonmail.com) 2019
  4. *
  5. * Aptdec is free software: you can redistribute it and/or modify
  6. * it under the terms of the GNU General Public License as published by
  7. * the Free Software Foundation, either version 2 of the License, or
  8. * (at your option) any later version.
  9. *
  10. * This program is distributed in the hope that it will be useful,
  11. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  13. * GNU General Public License for more details.
  14. *
  15. * You should have received a copy of the GNU General Public License
  16. * along with this program. If not, see <https://www.gnu.org/licenses/>.
  17. *
  18. */
  19. #include <stdlib.h>
  20. #include <stdio.h>
  21. #include <string.h>
  22. #include <math.h>
  23. #include "filter.h"
  24. #include "filtercoeff.h"
  25. // In case your C compiler is so old that Pi hadn't been invented yet
  26. #ifndef M_PI
  27. #define M_PI 3.141592653589793
  28. #endif
  29. // Block sizes
  30. #define BLKAMP 1024
  31. #define BLKIN 1024
  32. #define Fc 2400.0
  33. #define DFc 50.0
  34. #define PixelLine 2080
  35. #define Fp (2 * PixelLine)
  36. #define RSMULT 15
  37. #define Fi (Fp * RSMULT)
  38. extern int getsample(float *inbuff, int count);
  39. static double Fe;
  40. static double offset = 0.0;
  41. static double FreqLine = 1.0;
  42. static double FreqOsc;
  43. static double K1, K2;
  44. // Check the input sample rate and calculate some constants
  45. int init_dsp(double F) {
  46. if(F > Fi) return(1);
  47. if(F < Fp) return(-1);
  48. Fe = F;
  49. K1 = DFc / Fe;
  50. K2 = K1 * K1 / 2.0;
  51. // Number of samples for each cycle
  52. FreqOsc = Fc / Fe;
  53. return(0);
  54. }
  55. /* Fast phase estimator
  56. * Calculates the phase angle of a signal from a IQ sample
  57. */
  58. static inline double Phase(double I, double Q) {
  59. double angle, r;
  60. int s;
  61. if(I == 0.0 && Q == 0.0)
  62. return(0.0);
  63. if (Q < 0) {
  64. s = -1;
  65. Q = -Q;
  66. } else {
  67. s = 1;
  68. }
  69. if (I >= 0) {
  70. r = (I - Q) / (I + Q);
  71. angle = 0.25 - 0.25 * r;
  72. } else {
  73. r = (I + Q) / (Q - I);
  74. angle = 0.75 - 0.25 * r;
  75. }
  76. if(s > 0)
  77. return(angle);
  78. else
  79. return(-angle);
  80. }
  81. /* Phase locked loop
  82. * https://en.wikipedia.org/wiki/Phase-locked_loop
  83. * https://arachnoid.com/phase_locked_loop/
  84. * https://simple.wikipedia.org/wiki/Phase-locked_loop
  85. */
  86. static double pll(double I, double Q) {
  87. // PLL coefficient
  88. static double PhaseOsc = 0.0;
  89. double Io, Qo;
  90. double Ip, Qp;
  91. double DPhi;
  92. // Quadrature oscillator / reference
  93. Io = cos(PhaseOsc);
  94. Qo = sin(PhaseOsc);
  95. // Phase detector
  96. Ip = I * Io + Q * Qo;
  97. Qp = Q * Io - I * Qo;
  98. DPhi = Phase(Ip, Qp);
  99. // Loop filter
  100. PhaseOsc += 2.0 * M_PI * (K1 * DPhi + FreqOsc);
  101. if (PhaseOsc > M_PI)
  102. PhaseOsc -= 2.0 * M_PI;
  103. if (PhaseOsc <= -M_PI)
  104. PhaseOsc += 2.0 * M_PI;
  105. FreqOsc += K2 * DPhi;
  106. if (FreqOsc > ((Fc + DFc) / Fe))
  107. FreqOsc = (Fc + DFc) / Fe;
  108. if (FreqOsc < ((Fc - DFc) / Fe))
  109. FreqOsc = (Fc - DFc) / Fe;
  110. return(Ip);
  111. }
  112. // Convert samples into pixels
  113. static int getamp(double *ampbuff, int count) {
  114. static float inbuff[BLKIN];
  115. static int idxin = 0;
  116. static int nin = 0;
  117. for (int n = 0; n < count; n++) {
  118. double I, Q;
  119. // Get some more samples when needed
  120. if (nin < IQFilterLen * 2 + 2) {
  121. // Number of samples read
  122. int res;
  123. memmove(inbuff, &(inbuff[idxin]), nin * sizeof(float));
  124. idxin = 0;
  125. // Read some samples
  126. res = getsample(&(inbuff[nin]), BLKIN - nin);
  127. nin += res;
  128. // Make sure there is enough samples to continue
  129. if (nin < IQFilterLen * 2 + 2)
  130. return(n);
  131. }
  132. // Process read samples into a brightness value
  133. iqfir(&inbuff[idxin], iqfilter, IQFilterLen, &I, &Q);
  134. ampbuff[n] = pll(I, Q);
  135. // Increment current sample
  136. idxin++;
  137. nin--;
  138. }
  139. return(count);
  140. }
  141. // Sub-pixel offsetting + FIR compensation
  142. int getpixelv(float *pvbuff, int count) {
  143. // Amplitude buffer
  144. static double ampbuff[BLKAMP];
  145. static int nam = 0;
  146. static int idxam = 0;
  147. double mult;
  148. // Sub-pixel offset
  149. mult = (double) Fi / Fe * FreqLine;
  150. int m = RSFilterLen / mult + 1;
  151. for (int n = 0; n < count; n++) {
  152. int shift;
  153. if (nam < m) {
  154. int res;
  155. memmove(ampbuff, &(ampbuff[idxam]), nam * sizeof(double));
  156. idxam = 0;
  157. res = getamp(&(ampbuff[nam]), BLKAMP - nam);
  158. nam += res;
  159. if (nam < m)
  160. return(n);
  161. }
  162. // Gaussian FIR compensation filter
  163. pvbuff[n] = rsfir(&(ampbuff[idxam]), rsfilter, RSFilterLen, offset, mult) * mult * 256.0;
  164. shift = ((int) floor((RSMULT - offset) / mult)) + 1;
  165. offset = shift * mult + offset - RSMULT;
  166. idxam += shift;
  167. nam -= shift;
  168. }
  169. return(count);
  170. }
  171. // Get an entire row of pixels, aligned with sync markers
  172. int getpixelrow(float *pixelv) {
  173. static float pixels[PixelLine + SyncFilterLen];
  174. static int npv;
  175. static int synced = 0;
  176. static double max = 0.0;
  177. double corr, ecorr, lcorr;
  178. int res;
  179. // Move the row buffer into the the image buffer
  180. if (npv > 0)
  181. memmove(pixelv, pixels, npv * sizeof(float));
  182. // Get the sync line
  183. if (npv < SyncFilterLen + 2) {
  184. res = getpixelv(&(pixelv[npv]), SyncFilterLen + 2 - npv);
  185. npv += res;
  186. // Exit if there are no pixels left
  187. if (npv < SyncFilterLen + 2) return(0);
  188. }
  189. // Calculate the sub-pixel offset
  190. ecorr = fir(pixelv, Sync, SyncFilterLen);
  191. corr = fir(&(pixelv[1]), Sync, SyncFilterLen - 1);
  192. lcorr = fir(&(pixelv[2]), Sync, SyncFilterLen - 2);
  193. FreqLine = 1.0+((ecorr-lcorr) / corr / PixelLine / 4.0);
  194. // The point in which the pixel offset is recalculated
  195. if (corr < 0.75 * max) {
  196. synced = 0;
  197. FreqLine = 1.0;
  198. }
  199. max = corr;
  200. if (synced < 8) {
  201. int mshift;
  202. if (npv < PixelLine + SyncFilterLen) {
  203. res = getpixelv(&(pixelv[npv]), PixelLine + SyncFilterLen - npv);
  204. npv += res;
  205. if (npv < PixelLine + SyncFilterLen)
  206. return(0);
  207. }
  208. // Test every possible position until we get the best result
  209. mshift = 0;
  210. for (int shift = 0; shift < PixelLine; shift++) {
  211. double corr;
  212. corr = fir(&(pixelv[shift + 1]), Sync, SyncFilterLen);
  213. if (corr > max) {
  214. mshift = shift;
  215. max = corr;
  216. }
  217. }
  218. // If we are already as aligned as we can get, just continue
  219. if (mshift == 0) {
  220. synced++;
  221. } else {
  222. memmove(pixelv, &(pixelv[mshift]), (npv - mshift) * sizeof(float));
  223. npv -= mshift;
  224. synced = 0;
  225. FreqLine = 1.0;
  226. }
  227. }
  228. // Get the rest of this row
  229. if (npv < PixelLine) {
  230. res = getpixelv(&(pixelv[npv]), PixelLine - npv);
  231. npv += res;
  232. if (npv < PixelLine)
  233. return(0);
  234. }
  235. // Move the sync lines into the output buffer with the calculated offset
  236. if (npv == PixelLine) {
  237. npv = 0;
  238. } else {
  239. memmove(pixels, &(pixelv[PixelLine]), (npv - PixelLine) * sizeof(float));
  240. npv -= PixelLine;
  241. }
  242. return(1);
  243. }