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

307 строки
6.7 KiB

  1. /*
  2. * This file is part of Aptdec.
  3. * Copyright (c) 2004-2009 Thierry Leconte (F4DWV), Xerbo (xerbo@protonmail.com) 2019-2020
  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 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) return 0.0;
  62. if (Q < 0) {
  63. s = -1;
  64. Q = -Q;
  65. } else {
  66. s = 1;
  67. }
  68. if (I >= 0) {
  69. r = (I - Q) / (I + Q);
  70. angle = 0.25 - 0.25 * r;
  71. } else {
  72. r = (I + Q) / (Q - I);
  73. angle = 0.75 - 0.25 * r;
  74. }
  75. if(s > 0){
  76. return angle;
  77. }else{
  78. return -angle;
  79. }
  80. }
  81. /* Phase locked loop
  82. * https://arachnoid.com/phase_locked_loop/
  83. * Model of this filter here https://www.desmos.com/calculator/m0uadgkoee
  84. */
  85. static double pll(double I, double Q) {
  86. // PLL coefficient
  87. static double PhaseOsc = 0.0;
  88. double Io, Qo;
  89. double Ip, Qp;
  90. double DPhi;
  91. // Quadrature oscillator / reference
  92. Io = cos(PhaseOsc);
  93. Qo = sin(PhaseOsc);
  94. // Phase detector
  95. Ip = I * Io + Q * Qo;
  96. Qp = Q * Io - I * Qo;
  97. DPhi = Phase(Ip, Qp);
  98. // Loop filter
  99. PhaseOsc += 2.0 * M_PI * (K1 * DPhi + FreqOsc);
  100. if (PhaseOsc > M_PI)
  101. PhaseOsc -= 2.0 * M_PI;
  102. if (PhaseOsc <= -M_PI)
  103. PhaseOsc += 2.0 * M_PI;
  104. FreqOsc += K2 * DPhi;
  105. if (FreqOsc > ((Fc + DFc) / Fe))
  106. FreqOsc = (Fc + DFc) / Fe;
  107. if (FreqOsc < ((Fc - DFc) / Fe))
  108. FreqOsc = (Fc - DFc) / Fe;
  109. return Ip;
  110. }
  111. // Convert samples into pixels
  112. static int getamp(double *ampbuff, int count) {
  113. static float inbuff[BLKIN];
  114. static int idxin = 0;
  115. static int nin = 0;
  116. for (int n = 0; n < count; n++) {
  117. double I, Q;
  118. // Get some more samples when needed
  119. if (nin < IQFilterLen * 2 + 2) {
  120. // Number of samples read
  121. int res;
  122. memmove(inbuff, &(inbuff[idxin]), nin * sizeof(float));
  123. idxin = 0;
  124. // Read some samples
  125. res = getsample(&(inbuff[nin]), BLKIN - nin);
  126. nin += res;
  127. // Make sure there is enough samples to continue
  128. if (nin < IQFilterLen * 2 + 2)
  129. return n;
  130. }
  131. // Process read samples into a brightness value
  132. iqfir(&inbuff[idxin], iqfilter, IQFilterLen, &I, &Q);
  133. ampbuff[n] = pll(I, Q);
  134. // Increment current sample
  135. idxin++;
  136. nin--;
  137. }
  138. return count;
  139. }
  140. // Sub-pixel offsetting + FIR compensation
  141. int getpixelv(float *pvbuff, int count) {
  142. // Amplitude buffer
  143. static double ampbuff[BLKAMP];
  144. static int nam = 0;
  145. static int idxam = 0;
  146. double mult;
  147. // Gaussian resampling factor
  148. mult = (double) Fi / Fe * FreqLine;
  149. int m = RSFilterLen / mult + 1;
  150. for (int n = 0; n < count; n++) {
  151. int shift;
  152. if (nam < m) {
  153. int res;
  154. memmove(ampbuff, &(ampbuff[idxam]), nam * sizeof(double));
  155. idxam = 0;
  156. res = getamp(&(ampbuff[nam]), BLKAMP - nam);
  157. nam += res;
  158. if (nam < m)
  159. return n;
  160. }
  161. // Gaussian FIR compensation filter
  162. pvbuff[n] = rsfir(&(ampbuff[idxam]), rsfilter, RSFilterLen, offset, mult) * mult * 256.0;
  163. shift = ((int) floor((RSMULT - offset) / mult)) + 1;
  164. offset = shift * mult + offset - RSMULT;
  165. idxam += shift;
  166. nam -= shift;
  167. }
  168. return count;
  169. }
  170. // Get an entire row of pixels, aligned with sync markers
  171. // FIXME: loses sync easily
  172. int getpixelrow(float *pixelv, int nrow, int *zenith, int reset) {
  173. static float pixels[PixelLine + SyncFilterLen];
  174. static int npv;
  175. static int synced = 0;
  176. static double max = 0.0;
  177. static double minDoppler = 100;
  178. if(reset) synced = 0;
  179. double corr, ecorr, lcorr;
  180. int res;
  181. // Move the row buffer into the the image buffer
  182. if (npv > 0)
  183. memmove(pixelv, pixels, npv * sizeof(float));
  184. // Get the sync line
  185. if (npv < SyncFilterLen + 2) {
  186. res = getpixelv(&(pixelv[npv]), SyncFilterLen + 2 - npv);
  187. npv += res;
  188. if (npv < SyncFilterLen + 2)
  189. return 0;
  190. }
  191. // Calculate the frequency offset
  192. ecorr = fir(pixelv, Sync, SyncFilterLen);
  193. corr = fir(&pixelv[1], Sync, SyncFilterLen - 1);
  194. lcorr = fir(&pixelv[2], Sync, SyncFilterLen - 2);
  195. FreqLine = 1.0+((ecorr-lcorr) / corr / PixelLine / 4.0);
  196. // Find the point in which ecorr and lcorr intercept
  197. if(fabs(lcorr - ecorr) < minDoppler && fabs(lcorr - ecorr) > 2){
  198. minDoppler = fabs(lcorr - ecorr);
  199. *zenith = nrow;
  200. }
  201. // The point in which the pixel offset is recalculated
  202. if (corr < 0.75 * max) {
  203. synced = 0;
  204. FreqLine = 1.0;
  205. }
  206. max = corr;
  207. if (synced < 8) {
  208. int mshift;
  209. if (npv < PixelLine + SyncFilterLen) {
  210. res = getpixelv(&(pixelv[npv]), PixelLine + SyncFilterLen - npv);
  211. npv += res;
  212. if (npv < PixelLine + SyncFilterLen)
  213. return 0;
  214. }
  215. // Test every possible position until we get the best result
  216. mshift = 0;
  217. for (int shift = 0; shift < PixelLine; shift++) {
  218. double corr;
  219. corr = fir(&(pixelv[shift + 1]), Sync, SyncFilterLen);
  220. if (corr > max) {
  221. mshift = shift;
  222. max = corr;
  223. }
  224. }
  225. // If we are already as aligned as we can get, just continue
  226. if (mshift == 0) {
  227. synced++;
  228. } else {
  229. memmove(pixelv, &(pixelv[mshift]), (npv - mshift) * sizeof(float));
  230. npv -= mshift;
  231. synced = 0;
  232. FreqLine = 1.0;
  233. }
  234. }
  235. // Get the rest of this row
  236. if (npv < PixelLine) {
  237. res = getpixelv(&(pixelv[npv]), PixelLine - npv);
  238. npv += res;
  239. if (npv < PixelLine)
  240. return 0;
  241. }
  242. // Move the sync lines into the output buffer with the calculated offset
  243. if (npv == PixelLine) {
  244. npv = 0;
  245. } else {
  246. memmove(pixels, &(pixelv[PixelLine]), (npv - PixelLine) * sizeof(float));
  247. npv -= PixelLine;
  248. }
  249. return 1;
  250. }