#include <stdlib.h> | |
#include <math.h> | |
#include <string.h> | |
#include <fftw3.h> | |
#define FFTLEN 2048 | |
#define SRATE 22050 | |
// Return sinusoid power from complex DFT coefficients | |
double power (fftw_complex coeff) { | |
return pow(coeff[0],2) + pow(coeff[1],2); | |
} | |
// Return the FFT bin index matching the given frequency | |
int GetBin (double Freq) { | |
return (Freq / SRATE * FFTLEN); | |
} | |
int main () { | |
double f0 = 2900; | |
double f1 = 4700; | |
double bps = 850; | |
int bin0 = GetBin(f0); | |
int bin1 = GetBin(f1); | |
double w = 2 * M_PI * (1.0 * bps/2 / SRATE); | |
unsigned char *a, *b; | |
a = calloc(1,sizeof(unsigned char)); | |
b = calloc(1,sizeof(unsigned char)); | |
unsigned char pix; | |
short int word=0; | |
unsigned char *Ibuffer; | |
unsigned char *Qbuffer; | |
unsigned char *tbuf; | |
Ibuffer = calloc( FFTLEN, sizeof(unsigned char)); | |
Qbuffer = calloc( FFTLEN, sizeof(unsigned char)); | |
tbuf = calloc( FFTLEN, sizeof(unsigned char)); | |
int lenf = SRATE/bps; | |
int i,bit,n=0,prevbit=0,dbit,sync=0; | |
int MaxBin,prevMB; | |
int acc,nextpll=0; | |
double lo,prevlo; | |
double Hann[FFTLEN] = {0}; | |
double Power[FFTLEN], lo_phase; | |
fftw_complex *in; | |
fftw_complex *out; | |
fftw_plan Plan; | |
in = fftw_alloc_complex(FFTLEN); | |
out = fftw_alloc_complex(FFTLEN*2); | |
Plan = fftw_plan_dft_1d(FFTLEN, in, out, FFTW_FORWARD, FFTW_ESTIMATE); | |
for (i = 0; i < FFTLEN; i++) in[i][0] = in[i][1] = 0; | |
// Create 1-bitlen Hann window | |
for (i = 0; i < lenf; i++) Hann[i] = 0.5 * (1 - cos( 2 * M_PI * i / (1.0*lenf-1) ) ); | |
for (i = 0; i < FFTLEN; i++) Ibuffer[i] = Qbuffer[i] = 0; | |
acc = 0; | |
prevMB = 0; | |
lo_phase = 0; | |
prevlo = 0; | |
while ( ++n ) { | |
// Reference oscillator | |
lo = cos(w * n + lo_phase); | |
// Read data from DSP | |
if (!fread(a,1,1,stdin)) break; | |
if (!fread(b,1,1,stdin)) break; | |
// Move buffers | |
memcpy(tbuf,&Ibuffer[1],lenf-1); | |
memcpy(Ibuffer,tbuf,lenf-1); | |
memcpy(tbuf,&Qbuffer[1],lenf-1); | |
memcpy(Qbuffer,tbuf,lenf-1); | |
Ibuffer[lenf-1] = a[0]; | |
Qbuffer[lenf-1] = b[0]; | |
if (n % 2 == 0) { | |
// Apply Hann window | |
for (i = 0; i < lenf-1; i++) { | |
in[i][0] = Ibuffer[i] * Hann[i]; | |
in[i][1] = Qbuffer[i] * Hann[i]; | |
} | |
fftw_execute(Plan); | |
// Find the bin with most power | |
MaxBin = 0; | |
for (i = bin0-20; i < bin1+20; i++) { | |
Power[i] = power(out[i]); | |
if (MaxBin == 0 || Power[i] > Power[MaxBin]) MaxBin = i; | |
} | |
MaxBin -= (bin0 + bin1) / 2; | |
} | |
// PLL | |
if ( ((prevMB < 0 && MaxBin >= 0) || (prevMB >= 0 && MaxBin < 0)) && n > nextpll ) { | |
if (fmod(w * n + lo_phase, M_PI) > M_PI/2) | |
lo_phase -= .1; | |
else if (fmod(w * n + lo_phase, M_PI) < M_PI/2) | |
lo_phase += .1; | |
lo_phase = fmod(lo_phase, 2*M_PI); | |
nextpll = n + lenf / 2; | |
} | |
// Integrator | |
acc += MaxBin; | |
if ( (prevlo < 0 && lo >= 0) || (prevlo >= 0 && lo < 0)) { | |
bit = (acc > 0); | |
dbit = (prevbit != bit); | |
if (sync) { | |
printf("%d", dbit); | |
word = ((word << 1) + dbit) & 0xFF; | |
if (word == 0) { | |
sync = 0; | |
printf("\n"); | |
fflush(0); | |
} | |
} else { | |
word = ((word << 1) + dbit) & 0xFF; | |
sync = (word == 0xF9); | |
} | |
acc = 0; | |
prevbit = bit; | |
} | |
prevMB = MaxBin; | |
prevlo = lo; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment