From e390f8737f05fde049afc9ef79f8987b358e68bf Mon Sep 17 00:00:00 2001 From: Xerbo Date: Tue, 19 Jul 2022 14:01:28 +0100 Subject: [PATCH] Rewrite PLL --- src/dsp.c | 123 ++++++++++++++++++++++------------------------------- src/main.c | 21 +++++---- src/util.h | 15 +++++++ 3 files changed, 77 insertions(+), 82 deletions(-) diff --git a/src/dsp.c b/src/dsp.c index bddcf3a..0bac94e 100755 --- a/src/dsp.c +++ b/src/dsp.c @@ -24,79 +24,58 @@ #include "apt.h" #include "filter.h" #include "taps.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 +#include "util.h" // Block sizes #define BLKAMP 32768 #define BLKIN 32768 -#define Fc 2400.0 -#define DFc 50.0 -#define PixelLine 2080 -#define Fp (2 * PixelLine) +#define CARRIER_FREQ 2400.0 +#define MAX_CARRIER_OFFSET 20.0 + #define RSMULT 15 -#define Fi (Fp * RSMULT) +#define Fi (APT_IMG_WIDTH*2 * RSMULT) -static float Fe; +static float _sample_rate; static float offset = 0.0; static float FreqLine = 1.0; -static float FreqOsc; -static float K1, K2; +static float oscillator_freq; +static float pll_alpha; +static float pll_beta; -// Check the sample rate and calculate some constants -int apt_init(double F) { - if(F > Fi) return 1; - if(F < Fp) return -1; - Fe = F; +// Initalise and configure PLL +int apt_init(double sample_rate) { + if(sample_rate > Fi) return 1; + if(sample_rate < APT_IMG_WIDTH*2) return -1; + _sample_rate = sample_rate; - K1 = DFc / Fe; - K2 = K1 * K1 / 2.0; - // Number of samples per cycle - FreqOsc = Fc / Fe; + // Pll configuration + pll_alpha = 50 / _sample_rate; + pll_beta = pll_alpha * pll_alpha / 2.0; + oscillator_freq = CARRIER_FREQ/sample_rate; return 0; } -/* Phase locked loop - * https://arachnoid.com/phase_locked_loop/ - * Model of this filter here https://www.desmos.com/calculator/m0uadgkoee - */ -static float pll(float I2, float Q) { - // PLL coefficient - static float PhaseOsc = 0.0; - float Io, Qo; - float Ip, Qp; - float DPhi; - - // Quadrature oscillator / reference - Io = cos(PhaseOsc); - Qo = sin(PhaseOsc); - - // Phase detector - Ip = I2 * Io + Q * Qo; - Qp = Q * Io - I2 * Qo; - DPhi = atan2f(Qp, Ip); - - // 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; +static float pll(float complex in) { + static float oscillator_phase = 0.0; + + // Internal oscillator + float complex osc = cos(oscillator_phase) + -sin(oscillator_phase)*I; + in *= osc; + + // Error detector + float error = cargf(in); + + // Adjust frequency and phase + oscillator_freq += pll_beta*error; + oscillator_freq = clamp_half(oscillator_freq, (CARRIER_FREQ + MAX_CARRIER_OFFSET) / _sample_rate); + oscillator_phase += M_TAUf * (pll_alpha*error + oscillator_freq); + oscillator_phase = remainderf(oscillator_phase, M_TAUf); + + return crealf(in); } // Convert samples into pixels @@ -125,10 +104,8 @@ static int getamp(float *ampbuff, int count, apt_getsamples_t getsamples, void * } // Process read samples into a brightness value - float complex tmp = hilbert_transform(&inbuff[idxin], hilbert_filter, HILBERT_FILTER_SIZE); - I2 = crealf(tmp); - Q = cimagf(tmp); - ampbuff[n] = pll(I2, Q); + float complex sample = hilbert_transform(&inbuff[idxin], hilbert_filter, HILBERT_FILTER_SIZE); + ampbuff[n] = pll(sample); // Increment current sample idxin++; @@ -148,7 +125,7 @@ int getpixelv(float *pvbuff, int count, apt_getsamples_t getsamples, void *conte float mult; // Gaussian resampling factor - mult = (float) Fi / Fe * FreqLine; + mult = (float) Fi / _sample_rate * FreqLine; int m = (int)(LOW_PASS_SIZE / mult + 1); for (int n = 0; n < count; n++) { @@ -178,7 +155,7 @@ int getpixelv(float *pvbuff, int count, apt_getsamples_t getsamples, void *conte // Get an entire row of pixels, aligned with sync markers int apt_getpixelrow(float *pixelv, int nrow, int *zenith, int reset, apt_getsamples_t getsamples, void *context) { - static float pixels[PixelLine + SYNC_PATTERN_SIZE]; + static float pixels[APT_IMG_WIDTH + SYNC_PATTERN_SIZE]; static size_t npv; static int synced = 0; static float max = 0.0; @@ -205,7 +182,7 @@ int apt_getpixelrow(float *pixelv, int nrow, int *zenith, int reset, apt_getsamp ecorr = convolve(pixelv, sync_pattern, SYNC_PATTERN_SIZE); corr = convolve(&pixelv[1], sync_pattern, SYNC_PATTERN_SIZE - 1); lcorr = convolve(&pixelv[2], sync_pattern, SYNC_PATTERN_SIZE - 2); - FreqLine = 1.0+((ecorr-lcorr) / corr / PixelLine / 4.0); + FreqLine = 1.0+((ecorr-lcorr) / corr / APT_IMG_WIDTH / 4.0); float val = fabs(lcorr - ecorr)*0.25 + previous*0.75; if(val < minDoppler && nrow > 10){ @@ -225,16 +202,16 @@ int apt_getpixelrow(float *pixelv, int nrow, int *zenith, int reset, apt_getsamp int mshift; static int lastmshift; - if (npv < PixelLine + SYNC_PATTERN_SIZE) { - res = getpixelv(&(pixelv[npv]), PixelLine + SYNC_PATTERN_SIZE - npv, getsamples, context); + if (npv < APT_IMG_WIDTH + SYNC_PATTERN_SIZE) { + res = getpixelv(&(pixelv[npv]), APT_IMG_WIDTH + SYNC_PATTERN_SIZE - npv, getsamples, context); npv += res; - if (npv < PixelLine + SYNC_PATTERN_SIZE) + if (npv < APT_IMG_WIDTH + SYNC_PATTERN_SIZE) return 0; } // Test every possible position until we get the best result mshift = 0; - for (int shift = 0; shift < PixelLine; shift++) { + for (int shift = 0; shift < APT_IMG_WIDTH; shift++) { float corr; corr = convolve(&(pixelv[shift + 1]), sync_pattern, SYNC_PATTERN_SIZE); @@ -263,19 +240,19 @@ int apt_getpixelrow(float *pixelv, int nrow, int *zenith, int reset, apt_getsamp } // Get the rest of this row - if (npv < PixelLine) { - res = getpixelv(&(pixelv[npv]), PixelLine - npv, getsamples, context); + if (npv < APT_IMG_WIDTH) { + res = getpixelv(&(pixelv[npv]), APT_IMG_WIDTH - npv, getsamples, context); npv += res; - if (npv < PixelLine) + if (npv < APT_IMG_WIDTH) return 0; } // Move the sync lines into the output buffer with the calculated offset - if (npv == PixelLine) { + if (npv == APT_IMG_WIDTH) { npv = 0; } else { - memmove(pixels, &(pixelv[PixelLine]), (npv - PixelLine) * sizeof(float)); - npv -= PixelLine; + memmove(pixels, &(pixelv[APT_IMG_WIDTH]), (npv - APT_IMG_WIDTH) * sizeof(float)); + npv -= APT_IMG_WIDTH; } return 1; diff --git a/src/main.c b/src/main.c index e74a4d3..742b24e 100644 --- a/src/main.c +++ b/src/main.c @@ -269,6 +269,7 @@ static int processAudio(char *filename, options_t *opts){ return 1; } +float *samplebuf; static int initsnd(char *filename) { SF_INFO infwav; int res; @@ -293,6 +294,7 @@ static int initsnd(char *filename) { printf("Input sample rate: %d\n", infwav.samplerate); channels = infwav.channels; + samplebuf = (float *)malloc(sizeof(float) * 32768 * channels); return 1; } @@ -300,16 +302,17 @@ static int initsnd(char *filename) { // Read samples from the audio file int getsamples(void *context, float *samples, int nb) { (void) context; - if(channels == 1){ + if (channels == 1){ return (int)sf_read_float(audioFile, samples, nb); - }else{ - /* Multi channel audio is encoded such as: - * Ch1,Ch2,Ch1,Ch2,Ch1,Ch2 - */ - float *buf = malloc(sizeof(float) * nb * channels); // Something like BLKIN*2 could also be used - int samplesRead = (int)sf_read_float(audioFile, buf, nb * channels); - for(int i = 0; i < nb; i++) samples[i] = buf[i * channels]; - free(buf); + } else if (channels == 2) { + // Stereo channels are interleaved + int samplesRead = (int)sf_read_float(audioFile, samplebuf, nb * channels); + for(int i = 0; i < nb; i++) { + samples[i] = samplebuf[i * channels]; + } return samplesRead / channels; + } else { + printf("Only mono and stereo input files are supported\n"); + exit(1); } } diff --git a/src/util.h b/src/util.h index edcfeb9..6a54d84 100644 --- a/src/util.h +++ b/src/util.h @@ -16,5 +16,20 @@ * along with this program. If not, see . */ +#include + +#define M_PIf 3.14159265358979323846f +#define M_TAUf (M_PIf * 2.0f) + #define MIN(a,b) (((a) < (b)) ? (a) : (b)) #define MAX(a,b) (((a) > (b)) ? (a) : (b)) + +static float clamp(float x, float hi, float lo) { + if (x > hi) return hi; + if (x < lo) return lo; + return x; +} + +static float clamp_half(float x, float hi) { + return clamp(x, hi, -hi); +}