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