Rewrite PLL

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;

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);
 	}
 }

src/util.h

@@ -16,5 +16,20 @@
  * along with this program.  If not, see <https://www.gnu.org/licenses/>.
  */
 
+#include <stdio.h>
+
+#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);
+}