/* * Aptdec * Copyright (c) 2004 by Thierry Leconte (F4DWV) * * $Id$ * * This library is free software; you can redistribute it and/or modify * it under the terms of the GNU Library 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 Library General Public License for more details. * * You should have received a copy of the GNU Library General Public * License along with this library; if not, write to the Free Software * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. * */ #include #include #include #include #ifndef M_PI #define M_PI 3.1415926535 // For OS's that don't include it #endif #include "filter.h" #include "filtercoeff.h" /* WARNING * The comments in this file are at best educated guesses. * I am not a DSP god and can not figure out what a complicated * math function does just by looking at it. */ extern int getsample(float *inbuff, int nb); #define BLKAMP 1024 // Block size #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) static double Fe; static double offset = 0.0; static double FreqLine = 1.0; static double FreqOsc; static double K1, K2; // Check if the input sample rate is correct 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; 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 // Used to get value from an IQ sample with noise reduction 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 audio samples into a pixel static int getamp(double *ambuff, int nb) { static float inbuff[BLKIN]; static int idxin = 0; static int nin = 0; int n; for (n = 0; n < nb; n++) { double I, Q; // If the amount of samples is small enough to be processed 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; // If we haven't read any more samples, return how far we got if (nin < IQFilterLen * 2 + 2) return(n); } // Process read samples into a brightness value iqfir(&inbuff[idxin], iqfilter, IQFilterLen, &I, &Q); ambuff[n] = pll(I, Q); idxin += 1; nin -= 1; } return(n); } // Get an entire row of pixels, without alignment int getpixelv(float *pvbuff, int nb) { // Amplitude buffer static double ambuff[BLKAMP]; static int nam = 0; static int idxam = 0; int n, m; double mult; mult = (double) Fi / Fe * FreqLine; m = RSFilterLen / mult + 1; for (n = 0; n < nb; n++) { int shift; if (nam < m) { int res; memmove(ambuff, &(ambuff[idxam]), nam * sizeof(double)); idxam = 0; res = getamp(&(ambuff[nam]), BLKAMP - nam); nam += res; if (nam < m) return(n); } // Denoise pvbuff[n] = rsfir(&(ambuff[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(nb); } // Align this line based off of the synchronisation markers int getpixelrow(float *pixelv) { // Create an array for this row static float pixels[PixelLine + SyncFilterLen]; static int npv = 0; static int synced = 0; static double max = 0.0; double corr, ecorr, lcorr; int res; if (npv > 0) memmove(pixelv, pixels, npv * sizeof(float)); if (npv < SyncFilterLen + 2) { res = getpixelv(&(pixelv[npv]), SyncFilterLen + 2 - npv); npv += res; if (npv < SyncFilterLen + 2) return(0); } // Test current synchronisation ecorr = fir(pixelv, Sync, SyncFilterLen); corr = fir(&(pixelv[1]), Sync, SyncFilterLen - 1); lcorr = fir(&(pixelv[2]), Sync, SyncFilterLen - 2); // Calculate the per pixel offset FreqLine = 1.0+((ecorr-lcorr) / corr / PixelLine / 4.0); // Maximum acceptable offset 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); } // Shift this line until we see the best results mshift = 0; for (int shift = 1; shift < PixelLine; shift++) { double corr; corr = fir(&(pixelv[shift + 1]), Sync, SyncFilterLen); if (corr > max) { mshift = shift; max = corr; } } // Shift memory, shifting this row if (mshift != 0) { memmove(pixelv, &(pixelv[mshift]), (npv - mshift) * sizeof(float)); npv -= mshift; synced = 0; FreqLine = 1.0; } else synced += 1; } // If there are not enough pixels try to grab some more if (npv < PixelLine) { res = getpixelv(&(pixelv[npv]), PixelLine - npv); npv += res; // If we fail this then exit with 0 (which breaks the loop at main.c:338) if (npv < PixelLine) return(0); } // If we're finished reset npv to 0 if (npv == PixelLine) { npv = 0; } else { // Move the pixel build buffer to the output buffer memmove(pixels, &(pixelv[PixelLine]), (npv - PixelLine) * sizeof(float)); npv -= PixelLine; } return(1); }