您最多选择25个主题 主题必须以字母或数字开头,可以包含连字符 (-),并且长度不得超过35个字符
 
 
 
 
 

303 行
6.9 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 per 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. double minDoppler = 100;
  173. int getpixelrow(float *pixelv, int nrow, int *zenith) {
  174. static float pixels[PixelLine + SyncFilterLen];
  175. static int npv;
  176. static int synced = 0;
  177. static double max = 0.0;
  178. double corr, ecorr, lcorr;
  179. int res;
  180. // Move the row buffer into the the image buffer
  181. if (npv > 0)
  182. memmove(pixelv, pixels, npv * sizeof(float));
  183. // Get the sync line
  184. if (npv < SyncFilterLen + 2) {
  185. res = getpixelv(&(pixelv[npv]), SyncFilterLen + 2 - npv);
  186. npv += res;
  187. // Exit if there are no pixels left
  188. if (npv < SyncFilterLen + 2) return(0);
  189. }
  190. // Calculate the frequency offset
  191. ecorr = fir(pixelv, Sync, SyncFilterLen);
  192. corr = fir(&(pixelv[1]), Sync, SyncFilterLen - 1);
  193. lcorr = fir(&(pixelv[2]), Sync, SyncFilterLen - 2);
  194. FreqLine = 1.0+((ecorr-lcorr) / corr / PixelLine / 4.0);
  195. // Find the point in which ecorr and lcorr intercept, make sure that it's not too perfect
  196. if(fabs(lcorr - ecorr) < minDoppler && fabs(lcorr - ecorr) > 1){
  197. minDoppler = fabs(lcorr - ecorr);
  198. *zenith = nrow;
  199. }
  200. // The point in which the pixel offset is recalculated
  201. if (corr < 0.75 * max) {
  202. synced = 0;
  203. FreqLine = 1.0;
  204. }
  205. max = corr;
  206. if (synced < 8) {
  207. int mshift;
  208. if (npv < PixelLine + SyncFilterLen) {
  209. res = getpixelv(&(pixelv[npv]), PixelLine + SyncFilterLen - npv);
  210. npv += res;
  211. if (npv < PixelLine + SyncFilterLen)
  212. return(0);
  213. }
  214. // Test every possible position until we get the best result
  215. mshift = 0;
  216. for (int shift = 0; shift < PixelLine; shift++) {
  217. double corr;
  218. corr = fir(&(pixelv[shift + 1]), Sync, SyncFilterLen);
  219. if (corr > max) {
  220. mshift = shift;
  221. max = corr;
  222. }
  223. }
  224. // If we are already as aligned as we can get, just continue
  225. if (mshift == 0) {
  226. synced++;
  227. } else {
  228. memmove(pixelv, &(pixelv[mshift]), (npv - mshift) * sizeof(float));
  229. npv -= mshift;
  230. synced = 0;
  231. FreqLine = 1.0;
  232. }
  233. }
  234. // Get the rest of this row
  235. if (npv < PixelLine) {
  236. res = getpixelv(&(pixelv[npv]), PixelLine - npv);
  237. npv += res;
  238. if (npv < PixelLine)
  239. return(0);
  240. }
  241. // Move the sync lines into the output buffer with the calculated offset
  242. if (npv == PixelLine) {
  243. npv = 0;
  244. } else {
  245. memmove(pixels, &(pixelv[PixelLine]), (npv - PixelLine) * sizeof(float));
  246. npv -= PixelLine;
  247. }
  248. return(1);
  249. }