radiosonde_auto_rx/demod/demod.c

435 wiersze
11 KiB
C

/*
* sync header: correlation/matched filter
* compile:
* gcc -c demod.c
*
* author: zilog80
*/
/* ------------------------------------------------------------------------------------ */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
typedef unsigned char ui8_t;
typedef unsigned short ui16_t;
typedef unsigned int ui32_t;
typedef short i16_t;
typedef int i32_t;
//#include "demod.h"
static unsigned int sample_in, sample_out, delay;
static int buffered = 0;
static int N, M;
static float *match = NULL,
*bufs = NULL,
*corrbuf = NULL;
static char *rawbits = NULL;
static int Nvar = 0; // < M
static double xsum=0, qsum=0;
static float *xs = NULL,
*qs = NULL;
/* ------------------------------------------------------------------------------------ */
static int sample_rate = 0, bits_sample = 0, channels = 0;
static float samples_per_bit = 0;
static int findstr(char *buff, char *str, int pos) {
int i;
for (i = 0; i < 4; i++) {
if (buff[(pos+i)%4] != str[i]) break;
}
return i;
}
float read_wav_header(FILE *fp, float baudrate) {
char txt[4+1] = "\0\0\0\0";
unsigned char dat[4];
int byte, p=0;
if (fread(txt, 1, 4, fp) < 4) return -1;
if (strncmp(txt, "RIFF", 4)) return -1;
if (fread(txt, 1, 4, fp) < 4) return -1;
// pos_WAVE = 8L
if (fread(txt, 1, 4, fp) < 4) return -1;
if (strncmp(txt, "WAVE", 4)) return -1;
// pos_fmt = 12L
for ( ; ; ) {
if ( (byte=fgetc(fp)) == EOF ) return -1;
txt[p % 4] = byte;
p++; if (p==4) p=0;
if (findstr(txt, "fmt ", p) == 4) break;
}
if (fread(dat, 1, 4, fp) < 4) return -1;
if (fread(dat, 1, 2, fp) < 2) return -1;
if (fread(dat, 1, 2, fp) < 2) return -1;
channels = dat[0] + (dat[1] << 8);
if (fread(dat, 1, 4, fp) < 4) return -1;
memcpy(&sample_rate, dat, 4); //sample_rate = dat[0]|(dat[1]<<8)|(dat[2]<<16)|(dat[3]<<24);
if (fread(dat, 1, 4, fp) < 4) return -1;
if (fread(dat, 1, 2, fp) < 2) return -1;
//byte = dat[0] + (dat[1] << 8);
if (fread(dat, 1, 2, fp) < 2) return -1;
bits_sample = dat[0] + (dat[1] << 8);
// pos_dat = 36L + info
for ( ; ; ) {
if ( (byte=fgetc(fp)) == EOF ) return -1;
txt[p % 4] = byte;
p++; if (p==4) p=0;
if (findstr(txt, "data", p) == 4) break;
}
if (fread(dat, 1, 4, fp) < 4) return -1;
fprintf(stderr, "sample_rate: %d\n", sample_rate);
fprintf(stderr, "bits : %d\n", bits_sample);
fprintf(stderr, "channels : %d\n", channels);
if ((bits_sample != 8) && (bits_sample != 16)) return -1;
samples_per_bit = sample_rate/baudrate;
fprintf(stderr, "samples/bit: %.2f\n", samples_per_bit);
return samples_per_bit;
}
static int f32read_sample(FILE *fp, float *s) {
int i;
short b = 0;
for (i = 0; i < channels; i++) {
if (fread( &b, bits_sample/8, 1, fp) != 1) return EOF;
if (i == 0) { // i = 0: links bzw. mono
//if (bits_sample == 8) sint = b-128; // 8bit: 00..FF, centerpoint 0x80=128
//if (bits_sample == 16) sint = (short)b;
if (bits_sample == 8) { b -= 128; }
*s = b/128.0;
if (bits_sample == 16) { *s /= 256.0; }
}
}
return 0;
}
float get_bufvar(int ofs) {
float mu = xs[(sample_out+M + ofs) % M]/Nvar;
float var = qs[(sample_out+M + ofs) % M]/Nvar - mu*mu;
return var;
}
int getmaxCorr(float *maxv, unsigned int *maxvpos, int len) {
// In: current Max: maxv at maxvpos
// Out: Max
// Maximum im Intervall [sample_out-slen, sample_out-1]
// Randwerte zaehlen nicht als Extremwerte;
// nur neu berechnen, wenn neue Werte groesser als altes Max
int slen, pos;
unsigned int mpos=0;
float m, s0, s, s1;
int posIn = 0; // -1..0..1; // rs41:0
float S_neu = corrbuf[(sample_out+M+posIn) % M];
float S_vor = corrbuf[(sample_out+M+posIn-1) % M];
if (sample_in < delay) return 0;
slen = len*samples_per_bit;
if (slen > M) slen = M;
if ( (sample_out - *maxvpos >= slen-4) ||
(sample_out - *maxvpos < slen && *maxv <= S_vor && S_vor >= S_neu) )
{
m = -1.0;
for (pos = 1; pos < slen+posIn; pos++) {
s0 = corrbuf[(sample_out + 2*M - slen + pos-1) % M];
s = corrbuf[(sample_out + 2*M - slen + pos ) % M];
s1 = corrbuf[(sample_out + 2*M - slen + pos+1) % M];
if (s > m && s>=s0 && s>=s1) {
m = s;
mpos = sample_out - slen + pos;
}
}
*maxv = m;
*maxvpos = mpos;
}
buffered = sample_out-mpos;
return -buffered;
}
int f32buf_sample(FILE *fp, int inv, int cm) {
static unsigned int sample_in0;
int i;
float s = 0.0;
float xneu, xalt,
corr = 0.0,
norm2 = 0.0;
if (f32read_sample(fp, &s) == EOF) return EOF;
if (inv) s = -s;
bufs[sample_in % M] = s;
xneu = bufs[(sample_in ) % M];
xalt = bufs[(sample_in+M - Nvar) % M];
xsum += xneu - xalt; // + xneu - xalt
qsum += (xneu - xalt)*(xneu + xalt); // + xneu*xneu - xalt*xalt
xs[sample_in % M] = xsum;
qs[sample_in % M] = qsum;
if (cm) {
if (sample_in > sample_in0+1 || sample_in <= sample_in0) {
for (i = 0; i < M; i++) corrbuf[i] = 0.0; // -1.0
}
for (i = 0; i < N; i++) {
corr += match[i]*bufs[(sample_in+M -(N-1) + i) % M];
}
norm2 = qsum; //=qs[(sample_in) % M]; // N=Nvar ; N>Nvar, approx: norm2 *= N/(float)Nvar
//norm2 = qs[(sample_in+M - Nvar) % M] + qs[(sample_in) % M]; // N=2*Nvar
//for (i = 0; i < N; i+=Nvar) norm2 += qs[(sample_in+M - i) % M]; // N=k*Nvar
corrbuf[sample_in % M] = corr/sqrt(norm2);
sample_in0 = sample_in;
}
sample_out = sample_in - delay;
sample_in += 1;
return 0;
}
static int read_bufbit(int symlen, char *bits, unsigned int mvp, int reset) {
// symlen==2: manchester2 0->10,1->01->1: 2.bit
static unsigned int rcount;
static float rbitgrenze;
double sum = 0.0;
if (reset) {
rcount = 0;
rbitgrenze = 0;
}
rbitgrenze += samples_per_bit;
do {
sum += bufs[(rcount + mvp + M) % M];
rcount++;
} while (rcount < rbitgrenze); // n < samples_per_bit
if (symlen == 2) {
rbitgrenze += samples_per_bit;
do {
sum -= bufs[(rcount + mvp + M) % M];
rcount++;
} while (rcount < rbitgrenze); // n < samples_per_bit
}
if (symlen != 2) {
if (sum >= 0) *bits = '1';
else *bits = '0';
}
else {
if (sum >= 0) strncpy(bits, "10", 2);
else strncpy(bits, "01", 2);
}
return 0;
}
int headcmp(int symlen, char *hdr, int len, unsigned int mvp) {
int errs = 0;
int pos;
int step = 1;
if (symlen != 1) step = 2;
for (pos = 0; pos < len; pos += step) {
read_bufbit(symlen, rawbits+pos, mvp+1-(int)(len*samples_per_bit), pos==0);
}
rawbits[pos] = '\0';
while (len > 0) {
if (rawbits[len-1] != hdr[len-1]) errs += 1;
len--;
}
return errs;
}
/* -------------------------------------------------------------------------- */
int read_sbit(FILE *fp, int symlen, int *bit, int inv, int ofs, int reset, int cm) {
// symlen==2: manchester2 10->0,01->1: 2.bit
static double bitgrenze;
static unsigned long scount;
float sample;
double sum = 0.0;
if (reset) {
scount = 0;
bitgrenze = 0;
}
if (symlen == 2) {
bitgrenze += samples_per_bit;
do {
if (buffered > 0) buffered -= 1;
else if (f32buf_sample(fp, inv, cm) == EOF) return EOF;
sample = bufs[(sample_out-buffered + ofs + M) % M];
sum -= sample;
scount++;
} while (scount < bitgrenze); // n < samples_per_bit
}
bitgrenze += samples_per_bit;
do {
if (buffered > 0) buffered -= 1;
else if (f32buf_sample(fp, inv, cm) == EOF) return EOF;
sample = bufs[(sample_out-buffered + ofs + M) % M];
sum += sample;
scount++;
} while (scount < bitgrenze); // n < samples_per_bit
if (sum >= 0) *bit = 1;
else *bit = 0;
return 0;
}
/* -------------------------------------------------------------------------- */
static double norm2_match() {
int i;
double x, y = 0.0;
for (i = 0; i < N; i++) {
x = match[i];
y += x*x;
}
return y;
}
int init_buffers(char hdr[], int hLen, int shape) {
//hLen = strlen(header) = HEADLEN;
int i, pos;
float b, x;
float normMatch;
float alpha, sqalp, a = 1.0;
N = hLen * samples_per_bit;
M = 2*N; // >= N
Nvar = N; //N/2; // = N/k
bufs = (float *)calloc( M+1, sizeof(float)); if (bufs == NULL) return -100;
match = (float *)calloc( N+1, sizeof(float)); if (match == NULL) return -100;
corrbuf = (float *)calloc( M+1, sizeof(float)); if (corrbuf == NULL) return -100;
xs = (float *)calloc( M+1, sizeof(float)); if (xs == NULL) return -100;
qs = (float *)calloc( M+1, sizeof(float)); if (qs == NULL) return -100;
rawbits = (char *)calloc( N+1, sizeof(char)); if (rawbits == NULL) return -100;
for (i = 0; i < M; i++) bufs[i] = 0.0;
for (i = 0; i < M; i++) corrbuf[i] = 0.0;
alpha = exp(0.8);
sqalp = sqrt(alpha/M_PI);
//a = sqalp;
for (i = 0; i < N; i++) {
pos = i/samples_per_bit;
x = (i - pos*samples_per_bit)*2.0/samples_per_bit - 1;
a = sqalp;
if ( ( pos < hLen-1 && hdr[pos]!=hdr[pos+1] && x > 0.0 )
|| ( pos > 0 && hdr[pos-1]!=hdr[pos] && x < 0.0 ) ) // x=0: a=sqalp
{
switch (shape) {
case 1: if ( fabs(x) > 0.6 ) a *= (1 - fabs(x))/0.6;
break;
case 2: a = sqalp * exp(-alpha*x*x);
break;
case 3: a = 1 - fabs( x );
break;
default: a = sqalp;
if (i-pos*samples_per_bit < 2 ||
i-pos*samples_per_bit > samples_per_bit-2) a = 0.8*sqalp;
}
}
b = ((hdr[pos] & 0x1) - 0.5)*2.0; // {-1,+1}
b *= a;
match[i] = b;
}
normMatch = sqrt(norm2_match());
for (i = 0; i < N; i++) {
match[i] /= normMatch;
}
delay = N/4;
sample_in = 0;
return 0;
}
int free_buffers() {
if (match) { free(match); match = NULL; }
if (bufs) { free(bufs); bufs = NULL; }
if (xs) { free(xs); xs = NULL; }
if (qs) { free(qs); qs = NULL; }
if (corrbuf) { free(corrbuf); corrbuf = NULL; }
if (rawbits) { free(rawbits); rawbits = NULL; }
return 0;
}
/* ------------------------------------------------------------------------------------ */
unsigned int get_sample() {
return sample_out;
}