/* * This file is part of Aptdec. * Copyright (c) 2004-2009 Thierry Leconte (F4DWV), Xerbo (xerbo@protonmail.com) 2019-2020 * * Aptdec is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * */ #include #include #include #include #include "filter.h" #include "filtercoeff.h" // In case your C compiler is so old that Pi hadn't been invented yet #ifndef M_PI #define M_PI 3.141592653589793 #endif // Block sizes #define BLKAMP 1024 #define BLKIN 1024 #define Fc 2400.0 #define DFc 50.0 #define PixelLine 2080 #define Fp (2 * PixelLine) #define RSMULT 15 #define Fi (Fp * RSMULT) extern int getsample(float *inbuff, int count); static double Fe; static double offset = 0.0; static double FreqLine = 1.0; static double FreqOsc; static double K1, K2; // Check the input sample rate and calculate some constants int init_dsp(double F) { if(F > Fi) return(1); if(F < Fp) return(-1); Fe = F; K1 = DFc / Fe; K2 = K1 * K1 / 2.0; // Number of samples per cycle FreqOsc = Fc / Fe; return(0); } /* Fast phase estimator * Calculates the phase angle of a signal from a IQ sample */ static inline double Phase(double I, double Q) { double angle, r; int s; if(I == 0.0 && Q == 0.0) return(0.0); if (Q < 0) { s = -1; Q = -Q; } else { s = 1; } if (I >= 0) { r = (I - Q) / (I + Q); angle = 0.25 - 0.25 * r; } else { r = (I + Q) / (Q - I); angle = 0.75 - 0.25 * r; } if(s > 0) return(angle); else return(-angle); } /* Phase locked loop * https://en.wikipedia.org/wiki/Phase-locked_loop * https://arachnoid.com/phase_locked_loop/ * https://simple.wikipedia.org/wiki/Phase-locked_loop */ static double pll(double I, double Q) { // PLL coefficient static double PhaseOsc = 0.0; double Io, Qo; double Ip, Qp; double DPhi; // Quadrature oscillator / reference Io = cos(PhaseOsc); Qo = sin(PhaseOsc); // Phase detector Ip = I * Io + Q * Qo; Qp = Q * Io - I * Qo; DPhi = Phase(Ip, Qp); // Loop filter PhaseOsc += 2.0 * M_PI * (K1 * DPhi + FreqOsc); if (PhaseOsc > M_PI) PhaseOsc -= 2.0 * M_PI; if (PhaseOsc <= -M_PI) PhaseOsc += 2.0 * M_PI; FreqOsc += K2 * DPhi; if (FreqOsc > ((Fc + DFc) / Fe)) FreqOsc = (Fc + DFc) / Fe; if (FreqOsc < ((Fc - DFc) / Fe)) FreqOsc = (Fc - DFc) / Fe; return(Ip); } // Convert samples into pixels static int getamp(double *ampbuff, int count) { static float inbuff[BLKIN]; static int idxin = 0; static int nin = 0; for (int n = 0; n < count; n++) { double I, Q; // Get some more samples when needed if (nin < IQFilterLen * 2 + 2) { // Number of samples read int res; memmove(inbuff, &(inbuff[idxin]), nin * sizeof(float)); idxin = 0; // Read some samples res = getsample(&(inbuff[nin]), BLKIN - nin); nin += res; // Make sure there is enough samples to continue if (nin < IQFilterLen * 2 + 2) return(n); } // Process read samples into a brightness value iqfir(&inbuff[idxin], iqfilter, IQFilterLen, &I, &Q); ampbuff[n] = pll(I, Q); // Increment current sample idxin++; nin--; } return(count); } // Sub-pixel offsetting + FIR compensation int getpixelv(float *pvbuff, int count) { // Amplitude buffer static double ampbuff[BLKAMP]; static int nam = 0; static int idxam = 0; double mult; // Sub-pixel offset mult = (double) Fi / Fe * FreqLine; int m = RSFilterLen / mult + 1; for (int n = 0; n < count; n++) { int shift; if (nam < m) { int res; memmove(ampbuff, &(ampbuff[idxam]), nam * sizeof(double)); idxam = 0; res = getamp(&(ampbuff[nam]), BLKAMP - nam); nam += res; if (nam < m) return(n); } // Gaussian FIR compensation filter pvbuff[n] = rsfir(&(ampbuff[idxam]), rsfilter, RSFilterLen, offset, mult) * mult * 256.0; shift = ((int) floor((RSMULT - offset) / mult)) + 1; offset = shift * mult + offset - RSMULT; idxam += shift; nam -= shift; } return(count); } // Get an entire row of pixels, aligned with sync markers double minDoppler = 100; int getpixelrow(float *pixelv, int nrow, int *zenith) { static float pixels[PixelLine + SyncFilterLen]; static int npv; static int synced = 0; static double max = 0.0; double corr, ecorr, lcorr; int res; // Move the row buffer into the the image buffer if (npv > 0) memmove(pixelv, pixels, npv * sizeof(float)); // Get the sync line if (npv < SyncFilterLen + 2) { res = getpixelv(&(pixelv[npv]), SyncFilterLen + 2 - npv); npv += res; // Exit if there are no pixels left if (npv < SyncFilterLen + 2) return(0); } // Calculate the frequency offset ecorr = fir(pixelv, Sync, SyncFilterLen); corr = fir(&(pixelv[1]), Sync, SyncFilterLen - 1); lcorr = fir(&(pixelv[2]), Sync, SyncFilterLen - 2); FreqLine = 1.0+((ecorr-lcorr) / corr / PixelLine / 4.0); // Find the point in which ecorr and lcorr intercept, make sure that it's not too perfect if(fabs(lcorr - ecorr) < minDoppler && fabs(lcorr - ecorr) > 1){ minDoppler = fabs(lcorr - ecorr); *zenith = nrow; } // The point in which the pixel offset is recalculated if (corr < 0.75 * max) { synced = 0; FreqLine = 1.0; } max = corr; if (synced < 8) { int mshift; if (npv < PixelLine + SyncFilterLen) { res = getpixelv(&(pixelv[npv]), PixelLine + SyncFilterLen - npv); npv += res; if (npv < PixelLine + SyncFilterLen) return(0); } // Test every possible position until we get the best result mshift = 0; for (int shift = 0; shift < PixelLine; shift++) { double corr; corr = fir(&(pixelv[shift + 1]), Sync, SyncFilterLen); if (corr > max) { mshift = shift; max = corr; } } // If we are already as aligned as we can get, just continue if (mshift == 0) { synced++; } else { memmove(pixelv, &(pixelv[mshift]), (npv - mshift) * sizeof(float)); npv -= mshift; synced = 0; FreqLine = 1.0; } } // Get the rest of this row if (npv < PixelLine) { res = getpixelv(&(pixelv[npv]), PixelLine - npv); npv += res; if (npv < PixelLine) return(0); } // Move the sync lines into the output buffer with the calculated offset if (npv == PixelLine) { npv = 0; } else { memmove(pixels, &(pixelv[PixelLine]), (npv - PixelLine) * sizeof(float)); npv -= PixelLine; } return(1); }