Browse Source

Rewrite PLL

tags/v1.8.0
Xerbo 2 years ago
parent
commit
e390f8737f
No known key found for this signature in database GPG Key ID: 34103F6D8F11CEB0
3 changed files with 77 additions and 82 deletions
  1. +50
    -73
      src/dsp.c
  2. +12
    -9
      src/main.c
  3. +15
    -0
      src/util.h

+ 50
- 73
src/dsp.c View File

@@ -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;


+ 12
- 9
src/main.c View File

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

+ 15
- 0
src/util.h View File

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

Loading…
Cancel
Save