kopia lustrzana https://github.com/projecthorus/radiosonde_auto_rx
Remove unneccessary demod files
rodzic
380cf3978a
commit
b5a5113509
|
@ -1,27 +0,0 @@
|
|||
|
||||
## Radiosonde decoders
|
||||
|
||||
alternative decoders using cross-correlation for better header-synchronization
|
||||
|
||||
#### Files
|
||||
|
||||
* `demod_dft.c`, `demod_dft.h`, <br />
|
||||
`rs41dm_dft.c`, `rs92dm_dft.c`, `dfm09dm_dft.c`, `m10dm_dft.c`, `lms6dm_dft.c`, <br />
|
||||
`RS/ecc/bch_ecc.c`
|
||||
|
||||
#### Compile
|
||||
(copy `bch_ecc.c`) <br />
|
||||
`gcc -c demod_dft.c` <br />
|
||||
`gcc rs41dm_dft.c demod_dft.o -lm -o rs41dm_dft` <br />
|
||||
`gcc dfm09dm_dft.c demod_dft.o -lm -o dfm09dm_dft` <br />
|
||||
`gcc m10dm_dft.c demod_dft.o -lm -o m10dm_dft` <br />
|
||||
`gcc lms6dm_dft.c demod_dft.o -lm -o lms6dm_dft` <br />
|
||||
`gcc rs92dm_dft.c demod_dft.o -lm -o rs92dm_dft` (needs `RS/rs92/nav_gps_vel.c`)
|
||||
|
||||
#### Usage/Examples
|
||||
`./rs41dm_dft --ecc2 --crc -vx --ptu <audio.wav>` <br />
|
||||
`./dfm09dm_dft --ecc -v --ptu <audio.wav>` (add `-i` for dfm06)<br />
|
||||
`./m10dm_dft --dc -vv --ptu -c <audio.wav>` <br />
|
||||
`./lms6dm_dft --vit --ecc -v <audio.wav>` <br />
|
||||
|
||||
|
434
demod/demod.c
434
demod/demod.c
|
@ -1,434 +0,0 @@
|
|||
|
||||
/*
|
||||
* 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;
|
||||
}
|
||||
|
|
@ -1,14 +0,0 @@
|
|||
|
||||
float read_wav_header(FILE*, float);
|
||||
int f32buf_sample(FILE*, int, int);
|
||||
int read_sbit(FILE*, int, int*, int, int, int, int);
|
||||
|
||||
int getmaxCorr(float*, unsigned int*, int);
|
||||
int headcmp(int, char*, int, unsigned int);
|
||||
float get_bufvar(int);
|
||||
|
||||
int init_buffers(char*, int, int);
|
||||
int free_buffers(void);
|
||||
|
||||
unsigned int get_sample(void);
|
||||
|
|
@ -1,695 +0,0 @@
|
|||
|
||||
/*
|
||||
* sync header: correlation/matched filter
|
||||
* compile:
|
||||
* gcc -c demod_dft.c
|
||||
*
|
||||
* author: zilog80
|
||||
*/
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <complex.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_dft.h"
|
||||
|
||||
|
||||
static unsigned int sample_in, sample_out, delay;
|
||||
static int buffered = 0;
|
||||
|
||||
static int L, M;
|
||||
|
||||
static float *match = NULL,
|
||||
*bufs = NULL;
|
||||
|
||||
static char *rawbits = NULL;
|
||||
|
||||
static int Nvar = 0; // < M
|
||||
static double xsum=0, qsum=0;
|
||||
static float *xs = NULL,
|
||||
*qs = NULL;
|
||||
|
||||
|
||||
static float dc_ofs = 0.0;
|
||||
static float dc = 0.0;
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
|
||||
static int LOG2N, N_DFT;
|
||||
|
||||
static float complex *ew;
|
||||
|
||||
static float complex *Fm, *X, *Z, *cx;
|
||||
static float *xn;
|
||||
|
||||
static void dft_raw(float complex *Z) {
|
||||
int s, l, l2, i, j, k;
|
||||
float complex w1, w2, T;
|
||||
|
||||
j = 1;
|
||||
for (i = 1; i < N_DFT; i++) {
|
||||
if (i < j) {
|
||||
T = Z[j-1];
|
||||
Z[j-1] = Z[i-1];
|
||||
Z[i-1] = T;
|
||||
}
|
||||
k = N_DFT/2;
|
||||
while (k < j) {
|
||||
j = j - k;
|
||||
k = k/2;
|
||||
}
|
||||
j = j + k;
|
||||
}
|
||||
|
||||
for (s = 0; s < LOG2N; s++) {
|
||||
l2 = 1 << s;
|
||||
l = l2 << 1;
|
||||
w1 = (float complex)1.0;
|
||||
w2 = ew[s]; // cexp(-I*M_PI/(float)l2)
|
||||
for (j = 1; j <= l2; j++) {
|
||||
for (i = j; i <= N_DFT; i += l) {
|
||||
k = i + l2;
|
||||
T = Z[k-1] * w1;
|
||||
Z[k-1] = Z[i-1] - T;
|
||||
Z[i-1] = Z[i-1] + T;
|
||||
}
|
||||
w1 = w1 * w2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void dft(float *x, float complex *Z) {
|
||||
int i;
|
||||
for (i = 0; i < N_DFT; i++) Z[i] = (float complex)x[i];
|
||||
dft_raw(Z);
|
||||
}
|
||||
|
||||
static void Nidft(float complex *Z, float complex *z) {
|
||||
int i;
|
||||
for (i = 0; i < N_DFT; i++) z[i] = conj(Z[i]);
|
||||
dft_raw(z);
|
||||
// idft():
|
||||
// for (i = 0; i < N_DFT; i++) z[i] = conj(z[i])/(float)N_DFT; // hier: z reell
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
int getCorrDFT(int K, unsigned int pos, float *maxv, unsigned int *maxvpos) {
|
||||
int i;
|
||||
int mp = -1;
|
||||
float mx = 0.0;
|
||||
float mx2 = 0.0;
|
||||
float re_cx = 0.0;
|
||||
float xnorm = 1;
|
||||
unsigned int mpos = 0;
|
||||
|
||||
dc = 0.0;
|
||||
|
||||
if (K + L > N_DFT) return -1;
|
||||
if (sample_out < L) return -2;
|
||||
|
||||
if (pos == 0) pos = sample_out;
|
||||
|
||||
|
||||
for (i = 0; i < K+L; i++) xn[i] = bufs[(pos+M -(K+L-1) + i) % M];
|
||||
while (i < N_DFT) xn[i++] = 0.0;
|
||||
|
||||
dft(xn, X);
|
||||
|
||||
dc = get_bufmu(pos-sample_out); //oder: dc = creal(X[0])/(K+L);
|
||||
|
||||
for (i = 0; i < N_DFT; i++) Z[i] = X[i]*Fm[i];
|
||||
|
||||
Nidft(Z, cx);
|
||||
|
||||
|
||||
// relativ Peak - Normierung erst zum Schluss;
|
||||
// dann jedoch nicht zwingend corr-Max wenn FM-Amplitude bzw. norm(x) nicht konstant
|
||||
// (z.B. rs41 Signal-Pausen). Moeglicherweise wird dann wahres corr-Max in dem
|
||||
// K-Fenster nicht erkannt, deshalb K nicht zu gross waehlen.
|
||||
//
|
||||
mx2 = 0.0; // t = L-1
|
||||
for (i = L-1; i < K+L; i++) { // i=t .. i=t+K < t+1+K
|
||||
re_cx = creal(cx[i]); // imag(cx)=0
|
||||
if (re_cx*re_cx > mx2) {
|
||||
mx = re_cx;
|
||||
mx2 = mx*mx;
|
||||
mp = i;
|
||||
}
|
||||
}
|
||||
if (mp == L-1 || mp == K+L-1) return -4; // Randwert
|
||||
// mp == t mp == K+t
|
||||
|
||||
mpos = pos - (K + L-1) + mp;
|
||||
xnorm = sqrt(qs[(mpos + 2*M) % M]); // Nvar = L
|
||||
mx /= xnorm*N_DFT;
|
||||
|
||||
*maxv = mx;
|
||||
*maxvpos = mpos;
|
||||
|
||||
if (pos == sample_out) buffered = sample_out-mpos;
|
||||
|
||||
return mp;
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
static int sample_rate = 0, bits_sample = 0, channels = 0;
|
||||
static float samples_per_bit = 0;
|
||||
static int wav_ch = 0; // 0: links bzw. mono; 1: rechts
|
||||
|
||||
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, int wav_channel) {
|
||||
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 (wav_channel >= 0 && wav_channel < channels) wav_ch = wav_channel;
|
||||
else wav_ch = 0;
|
||||
fprintf(stderr, "channel-In : %d\n", wav_ch+1);
|
||||
|
||||
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 == wav_ch) { // 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;
|
||||
}
|
||||
|
||||
float get_bufmu(int ofs) {
|
||||
float mu = xs[(sample_out+M + ofs) % M]/Nvar;
|
||||
return mu;
|
||||
}
|
||||
|
||||
int f32buf_sample(FILE *fp, int inv) {
|
||||
float s = 0.0;
|
||||
float xneu, xalt;
|
||||
|
||||
|
||||
if (f32read_sample(fp, &s) == EOF) return EOF;
|
||||
|
||||
if (inv) s = -s;
|
||||
bufs[sample_in % M] = s - dc_ofs;
|
||||
|
||||
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;
|
||||
|
||||
|
||||
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 inv, int option_dc) {
|
||||
int errs = 0;
|
||||
int pos;
|
||||
int step = 1;
|
||||
char sign = 0;
|
||||
|
||||
if (symlen != 1) step = 2;
|
||||
if (inv) sign=1;
|
||||
|
||||
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]^sign) != hdr[len-1]) errs += 1;
|
||||
len--;
|
||||
}
|
||||
|
||||
if (option_dc && errs < 3) {
|
||||
dc_ofs += dc;
|
||||
}
|
||||
|
||||
return errs;
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
int read_sbit(FILE *fp, int symlen, int *bit, int inv, int ofs, int reset) {
|
||||
// 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) == 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) == 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;
|
||||
}
|
||||
|
||||
int read_spkbit(FILE *fp, int symlen, int *bit, int inv, int ofs, int reset, int spike) {
|
||||
// symlen==2: manchester2 10->0,01->1: 2.bit
|
||||
|
||||
static double bitgrenze;
|
||||
static unsigned long scount;
|
||||
|
||||
float sample;
|
||||
float avg;
|
||||
float ths = 0.5, scale = 0.27;
|
||||
|
||||
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) == EOF) return EOF;
|
||||
|
||||
sample = bufs[(sample_out-buffered + ofs + M) % M];
|
||||
avg = 0.5*(bufs[(sample_out-buffered-1 + ofs + M) % M]
|
||||
+bufs[(sample_out-buffered+1 + ofs + M) % M]);
|
||||
if (spike && fabs(sample - avg) > ths) sample = avg + scale*(sample - avg); // spikes
|
||||
|
||||
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) == EOF) return EOF;
|
||||
|
||||
sample = bufs[(sample_out-buffered + ofs + M) % M];
|
||||
avg = 0.5*(bufs[(sample_out-buffered-1 + ofs + M) % M]
|
||||
+bufs[(sample_out-buffered+1 + ofs + M) % M]);
|
||||
if (spike && fabs(sample - avg) > ths) sample = avg + scale*(sample - avg); // spikes
|
||||
|
||||
sum += sample;
|
||||
|
||||
scount++;
|
||||
} while (scount < bitgrenze); // n < samples_per_bit
|
||||
|
||||
if (sum >= 0) *bit = 1;
|
||||
else *bit = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
int read_softbit(FILE *fp, int symlen, int *bit, float *sb, float level, int inv, int ofs, int reset) {
|
||||
// symlen==2: manchester2 10->0,01->1: 2.bit
|
||||
|
||||
static double bitgrenze;
|
||||
static unsigned long scount;
|
||||
|
||||
float sample;
|
||||
|
||||
double sum = 0.0;
|
||||
int n = 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) == EOF) return EOF;
|
||||
|
||||
sample = bufs[(sample_out-buffered + ofs + M) % M];
|
||||
if (scount > bitgrenze-samples_per_bit && scount < bitgrenze-2)
|
||||
{
|
||||
sum -= sample;
|
||||
n++;
|
||||
}
|
||||
scount++;
|
||||
} while (scount < bitgrenze); // n < samples_per_bit
|
||||
}
|
||||
|
||||
bitgrenze += samples_per_bit;
|
||||
do {
|
||||
if (buffered > 0) buffered -= 1;
|
||||
else if (f32buf_sample(fp, inv) == EOF) return EOF;
|
||||
|
||||
sample = bufs[(sample_out-buffered + ofs + M) % M];
|
||||
if (scount > bitgrenze-samples_per_bit && scount < bitgrenze-2)
|
||||
{
|
||||
sum += sample;
|
||||
n++;
|
||||
}
|
||||
scount++;
|
||||
} while (scount < bitgrenze); // n < samples_per_bit
|
||||
|
||||
if (sum >= 0) *bit = 1;
|
||||
else *bit = 0;
|
||||
|
||||
*sb = sum / n;
|
||||
|
||||
if (*sb > +2.5*level) *sb = +0.8*level;
|
||||
if (*sb > +level) *sb = +level;
|
||||
|
||||
if (*sb < -2.5*level) *sb = -0.8*level;
|
||||
if (*sb < -level) *sb = -level;
|
||||
|
||||
*sb /= level;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
float header_level(char hdr[], int hLen, unsigned int pos, int inv) {
|
||||
int n, bitn;
|
||||
int sgn = 0;
|
||||
double s = 0.0;
|
||||
double sum = 0.0;
|
||||
|
||||
n = 0;
|
||||
bitn = 0;
|
||||
while ( bitn < hLen && (n < L) ) {
|
||||
sgn = (hdr[bitn]&1)*2-1; // {'0','1'} -> {-1,1}
|
||||
s = bufs[(pos-L + n + M) % M];
|
||||
if (inv) s = -s;
|
||||
sum += s * sgn;
|
||||
n++;
|
||||
bitn = n / samples_per_bit;
|
||||
}
|
||||
sum /= n;
|
||||
|
||||
return sum;
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
|
||||
static double norm2_match() {
|
||||
int i;
|
||||
double x, y = 0.0;
|
||||
for (i = 0; i < L; 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;
|
||||
|
||||
int p2 = 1;
|
||||
int K;
|
||||
int n, k;
|
||||
float *m = NULL;
|
||||
|
||||
|
||||
L = hLen * samples_per_bit + 0.5;
|
||||
M = 3*L;
|
||||
// if (samples_per_bit < 6) M = 6*L;
|
||||
|
||||
sample_in = 0;
|
||||
|
||||
p2 = 1;
|
||||
while (p2 < M) p2 <<= 1;
|
||||
while (p2 < 0x2000) p2 <<= 1; // or 0x4000, if sample not too short
|
||||
M = p2;
|
||||
N_DFT = p2;
|
||||
LOG2N = log(N_DFT)/log(2)+0.1; // 32bit cpu ... intermediate floating-point precision
|
||||
//while ((1 << LOG2N) < N_DFT) LOG2N++; // better N_DFT = (1 << LOG2N) ...
|
||||
|
||||
delay = L/16;
|
||||
K = M-L - delay; // L+K < M
|
||||
|
||||
Nvar = L; //L/2; // = L/k
|
||||
|
||||
|
||||
bufs = (float *)calloc( M+1, sizeof(float)); if (bufs == NULL) return -100;
|
||||
match = (float *)calloc( L+1, sizeof(float)); if (match == 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( 2*hLen+1, sizeof(char)); if (rawbits == NULL) return -100;
|
||||
|
||||
for (i = 0; i < M; i++) bufs[i] = 0.0;
|
||||
|
||||
alpha = exp(0.8);
|
||||
sqalp = sqrt(alpha/M_PI);
|
||||
//a = sqalp;
|
||||
|
||||
for (i = 0; i < L; 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 < L; i++) {
|
||||
match[i] /= normMatch;
|
||||
}
|
||||
|
||||
|
||||
xn = calloc(N_DFT+1, sizeof(float)); if (xn == NULL) return -1;
|
||||
|
||||
ew = calloc(LOG2N+1, sizeof(complex float)); if (ew == NULL) return -1;
|
||||
Fm = calloc(N_DFT+1, sizeof(complex float)); if (Fm == NULL) return -1;
|
||||
X = calloc(N_DFT+1, sizeof(complex float)); if (X == NULL) return -1;
|
||||
Z = calloc(N_DFT+1, sizeof(complex float)); if (Z == NULL) return -1;
|
||||
cx = calloc(N_DFT+1, sizeof(complex float)); if (cx == NULL) return -1;
|
||||
|
||||
for (n = 0; n < LOG2N; n++) {
|
||||
k = 1 << n;
|
||||
ew[n] = cexp(-I*M_PI/(float)k);
|
||||
}
|
||||
|
||||
m = calloc(N_DFT+1, sizeof(float)); if (m == NULL) return -1;
|
||||
for (i = 0; i < L; i++) m[L-1 - i] = match[i]; // t = L-1
|
||||
while (i < N_DFT) m[i++] = 0.0;
|
||||
dft(m, Fm);
|
||||
|
||||
free(m); m = NULL;
|
||||
|
||||
return K;
|
||||
}
|
||||
|
||||
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 (rawbits) { free(rawbits); rawbits = NULL; }
|
||||
|
||||
if (xn) { free(xn); xn = NULL; }
|
||||
if (ew) { free(ew); ew = NULL; }
|
||||
if (Fm) { free(Fm); Fm = NULL; }
|
||||
if (X) { free(X); X = NULL; }
|
||||
if (Z) { free(Z); Z = NULL; }
|
||||
if (cx) { free(cx); cx = NULL; }
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
unsigned int get_sample() {
|
||||
return sample_out;
|
||||
}
|
||||
|
|
@ -1,18 +0,0 @@
|
|||
|
||||
float read_wav_header(FILE*, float, int);
|
||||
int f32buf_sample(FILE*, int);
|
||||
int read_sbit(FILE*, int, int*, int, int, int);
|
||||
int read_spkbit(FILE*, int, int*, int, int, int, int);
|
||||
int read_softbit(FILE *fp, int symlen, int *bit, float *sb, float level, int inv, int ofs, int reset);
|
||||
float header_level(char hdr[], int hLen, unsigned int pos, int inv);
|
||||
|
||||
int getCorrDFT(int, unsigned int, float *, unsigned int *);
|
||||
int headcmp(int, char*, int, unsigned int, int, int);
|
||||
float get_bufvar(int);
|
||||
float get_bufmu(int);
|
||||
|
||||
int init_buffers(char*, int, int);
|
||||
int free_buffers(void);
|
||||
|
||||
unsigned int get_sample(void);
|
||||
|
773
demod/demod_iq.c
773
demod/demod_iq.c
|
@ -1,773 +0,0 @@
|
|||
|
||||
/*
|
||||
* sync header: correlation/matched filter
|
||||
* compile:
|
||||
* gcc -c demod_iq.c
|
||||
*
|
||||
* author: zilog80
|
||||
*/
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "demod_iq.h"
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
|
||||
static void raw_dft(dft_t *dft, float complex *Z) {
|
||||
int s, l, l2, i, j, k;
|
||||
float complex w1, w2, T;
|
||||
|
||||
j = 1;
|
||||
for (i = 1; i < dft->N; i++) {
|
||||
if (i < j) {
|
||||
T = Z[j-1];
|
||||
Z[j-1] = Z[i-1];
|
||||
Z[i-1] = T;
|
||||
}
|
||||
k = dft->N/2;
|
||||
while (k < j) {
|
||||
j = j - k;
|
||||
k = k/2;
|
||||
}
|
||||
j = j + k;
|
||||
}
|
||||
|
||||
for (s = 0; s < dft->LOG2N; s++) {
|
||||
l2 = 1 << s;
|
||||
l = l2 << 1;
|
||||
w1 = (float complex)1.0;
|
||||
w2 = dft->ew[s]; // cexp(-I*M_PI/(float)l2)
|
||||
for (j = 1; j <= l2; j++) {
|
||||
for (i = j; i <= dft->N; i += l) {
|
||||
k = i + l2;
|
||||
T = Z[k-1] * w1;
|
||||
Z[k-1] = Z[i-1] - T;
|
||||
Z[i-1] = Z[i-1] + T;
|
||||
}
|
||||
w1 = w1 * w2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void cdft(dft_t *dft, float complex *z, float complex *Z) {
|
||||
int i;
|
||||
for (i = 0; i < dft->N; i++) Z[i] = z[i];
|
||||
raw_dft(dft, Z);
|
||||
}
|
||||
|
||||
static void rdft(dft_t *dft, float *x, float complex *Z) {
|
||||
int i;
|
||||
for (i = 0; i < dft->N; i++) Z[i] = (float complex)x[i];
|
||||
raw_dft(dft, Z);
|
||||
}
|
||||
|
||||
static void Nidft(dft_t *dft, float complex *Z, float complex *z) {
|
||||
int i;
|
||||
for (i = 0; i < dft->N; i++) z[i] = conj(Z[i]);
|
||||
raw_dft(dft, z);
|
||||
// idft():
|
||||
// for (i = 0; i < dft->N; i++) z[i] = conj(z[i])/(float)dft->N; // hier: z reell
|
||||
}
|
||||
|
||||
static float bin2freq0(dft_t *dft, int k) {
|
||||
float fq = dft->sr * k / /*(float)*/dft->N;
|
||||
if (fq >= dft->sr/2.0) fq -= dft->sr;
|
||||
return fq;
|
||||
}
|
||||
static float bin2freq(dft_t *dft, int k) {
|
||||
float fq = k / (float)dft->N;
|
||||
if ( fq >= 0.5) fq -= 1.0;
|
||||
return fq*dft->sr;
|
||||
}
|
||||
static float bin2fq(dft_t *dft, int k) {
|
||||
float fq = k / (float)dft->N;
|
||||
if ( fq >= 0.5) fq -= 1.0;
|
||||
return fq;
|
||||
}
|
||||
|
||||
static int max_bin(dft_t *dft, float complex *Z) {
|
||||
int k, kmax;
|
||||
double max;
|
||||
|
||||
max = 0; kmax = 0;
|
||||
for (k = 0; k < dft->N; k++) {
|
||||
if (cabs(Z[k]) > max) {
|
||||
max = cabs(Z[k]);
|
||||
kmax = k;
|
||||
}
|
||||
}
|
||||
|
||||
return kmax;
|
||||
}
|
||||
|
||||
static int dft_window(dft_t *dft, int w) {
|
||||
int n;
|
||||
|
||||
if (w < 0 || w > 3) return -1;
|
||||
|
||||
for (n = 0; n < dft->N2; n++) {
|
||||
switch (w)
|
||||
{
|
||||
case 0: // (boxcar)
|
||||
dft->win[n] = 1.0;
|
||||
break;
|
||||
case 1: // Hann
|
||||
dft->win[n] = 0.5 * ( 1.0 - cos(2*M_PI*n/(float)(dft->N2-1)) );
|
||||
break ;
|
||||
case 2: // Hamming
|
||||
dft->win[n] = 25/46.0 + (1.0 - 25/46.0)*cos(2*M_PI*n / (float)(dft->N2-1));
|
||||
break ;
|
||||
case 3: // Blackmann
|
||||
dft->win[n] = 7938/18608.0
|
||||
- 9240/18608.0*cos(2*M_PI*n / (float)(dft->N2-1))
|
||||
+ 1430/18608.0*cos(4*M_PI*n / (float)(dft->N2-1));
|
||||
break ;
|
||||
}
|
||||
}
|
||||
while (n < dft->N) dft->win[n++] = 0.0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
int getCorrDFT(dsp_t *dsp, ui32_t pos, float *maxv, ui32_t *maxvpos) {
|
||||
int i;
|
||||
int mp = -1;
|
||||
float mx = 0.0;
|
||||
float mx2 = 0.0;
|
||||
float re_cx = 0.0;
|
||||
float xnorm = 1;
|
||||
ui32_t mpos = 0;
|
||||
|
||||
|
||||
dsp->dc = 0.0;
|
||||
|
||||
if (dsp->K + dsp->L > dsp->DFT.N) return -1;
|
||||
if (dsp->sample_out < dsp->L) return -2;
|
||||
|
||||
if (pos == 0) pos = dsp->sample_out;
|
||||
|
||||
dsp->dc = get_bufmu(dsp, pos - dsp->sample_out); //oder unten: dft_dc = creal(X[0])/(K+L);
|
||||
// wenn richtige Stelle (Varianz pruefen, kein M10-carrier?), dann von bufs[] subtrahieren
|
||||
|
||||
|
||||
for (i = 0; i < dsp->K + dsp->L; i++) (dsp->DFT).xn[i] = dsp->bufs[(pos+dsp->M -(dsp->K + dsp->L-1) + i) % dsp->M];
|
||||
while (i < dsp->DFT.N) (dsp->DFT).xn[i++] = 0.0;
|
||||
|
||||
rdft(&dsp->DFT, dsp->DFT.xn, dsp->DFT.X);
|
||||
|
||||
// dft_dc = creal(dsp->DFT.X[0])/dsp->DFT.N;
|
||||
|
||||
for (i = 0; i < dsp->DFT.N; i++) dsp->DFT.Z[i] = dsp->DFT.X[i]*dsp->DFT.Fm[i];
|
||||
|
||||
Nidft(&dsp->DFT, dsp->DFT.Z, dsp->DFT.cx);
|
||||
|
||||
|
||||
// relativ Peak - Normierung erst zum Schluss;
|
||||
// dann jedoch nicht zwingend corr-Max wenn FM-Amplitude bzw. norm(x) nicht konstant
|
||||
// (z.B. rs41 Signal-Pausen). Moeglicherweise wird dann wahres corr-Max in dem
|
||||
// K-Fenster nicht erkannt, deshalb K nicht zu gross waehlen.
|
||||
//
|
||||
mx2 = 0.0; // t = L-1
|
||||
for (i = dsp->L-1; i < dsp->K + dsp->L; i++) { // i=t .. i=t+K < t+1+K
|
||||
re_cx = creal(dsp->DFT.cx[i]); // imag(cx)=0
|
||||
if (re_cx*re_cx > mx2) {
|
||||
mx = re_cx;
|
||||
mx2 = mx*mx;
|
||||
mp = i;
|
||||
}
|
||||
}
|
||||
if (mp == dsp->L-1 || mp == dsp->K + dsp->L-1) return -4; // Randwert
|
||||
// mp == t mp == K+t
|
||||
|
||||
mpos = pos - (dsp->K + dsp->L-1) + mp; // t = L-1
|
||||
|
||||
//xnorm = sqrt(dsp->qs[(mpos + 2*dsp->M) % dsp->M]); // Nvar = L
|
||||
xnorm = 0.0;
|
||||
for (i = 0; i < dsp->L; i++) xnorm += dsp->bufs[(mpos-i + dsp->M) % dsp->M]*dsp->bufs[(mpos-i + dsp->M) % dsp->M];
|
||||
xnorm = sqrt(xnorm);
|
||||
|
||||
mx /= xnorm*(dsp->DFT).N;
|
||||
|
||||
*maxv = mx;
|
||||
*maxvpos = mpos;
|
||||
|
||||
if (pos == dsp->sample_out) dsp->buffered = dsp->sample_out - mpos;
|
||||
|
||||
return mp;
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
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(pcm_t *pcm, FILE *fp) {
|
||||
char txt[4+1] = "\0\0\0\0";
|
||||
unsigned char dat[4];
|
||||
int byte, p=0;
|
||||
int sample_rate = 0, bits_sample = 0, channels = 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 (pcm->sel_ch < 0 || pcm->sel_ch >= channels) pcm->sel_ch = 0; // default channel: 0
|
||||
fprintf(stderr, "channel-In : %d\n", pcm->sel_ch+1);
|
||||
|
||||
if ((bits_sample != 8) && (bits_sample != 16)) return -1;
|
||||
|
||||
|
||||
pcm->sr = sample_rate;
|
||||
pcm->bps = bits_sample;
|
||||
pcm->nch = channels;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int f32read_sample(dsp_t *dsp, float *s) {
|
||||
int i;
|
||||
short b = 0;
|
||||
|
||||
for (i = 0; i < dsp->nch; i++) {
|
||||
|
||||
if (fread( &b, dsp->bps/8, 1, dsp->fp) != 1) return EOF;
|
||||
|
||||
if (i == dsp->ch) { // 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 (dsp->bps == 8) { b -= 128; }
|
||||
*s = b/128.0;
|
||||
if (dsp->bps == 16) { *s /= 256.0; }
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int f32read_csample(dsp_t *dsp, float complex *z) {
|
||||
short x = 0, y = 0;
|
||||
|
||||
if (fread( &x, dsp->bps/8, 1, dsp->fp) != 1) return EOF;
|
||||
if (fread( &y, dsp->bps/8, 1, dsp->fp) != 1) return EOF;
|
||||
|
||||
*z = x + I*y;
|
||||
|
||||
if (dsp->bps == 8) { *z -= 128 + I*128; }
|
||||
*z /= 128.0;
|
||||
if (dsp->bps == 16) { *z /= 256.0; }
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
float get_bufvar(dsp_t *dsp, int ofs) {
|
||||
float mu = dsp->xs[(dsp->sample_out+dsp->M + ofs) % dsp->M]/dsp->Nvar;
|
||||
float var = dsp->qs[(dsp->sample_out+dsp->M + ofs) % dsp->M]/dsp->Nvar - mu*mu;
|
||||
return var;
|
||||
}
|
||||
|
||||
float get_bufmu(dsp_t *dsp, int ofs) {
|
||||
float mu = dsp->xs[(dsp->sample_out+dsp->M + ofs) % dsp->M]/dsp->Nvar;
|
||||
return mu;
|
||||
}
|
||||
|
||||
static int get_SNR(dsp_t *dsp) {
|
||||
|
||||
if (dsp->opt_iq)
|
||||
// if(dsp->rs_typ == RS41)
|
||||
{
|
||||
if (dsp->sample_posnoise > 0) // rs41
|
||||
{
|
||||
if (dsp->sample_out >= dsp->sample_posframe && dsp->sample_out < dsp->sample_posframe+dsp->len_sq) {
|
||||
if (dsp->sample_out == dsp->sample_posframe) dsp->V_signal = 0.0;
|
||||
dsp->V_signal += cabs(dsp->rot_iqbuf[dsp->sample_out % dsp->N_IQBUF]);
|
||||
}
|
||||
if (dsp->sample_out == dsp->sample_posframe+dsp->len_sq) dsp->V_signal /= (double)dsp->len_sq;
|
||||
|
||||
if (dsp->sample_out >= dsp->sample_posnoise && dsp->sample_out < dsp->sample_posnoise+dsp->len_sq) {
|
||||
if (dsp->sample_out == dsp->sample_posnoise) dsp->V_noise = 0.0;
|
||||
dsp->V_noise += cabs(dsp->rot_iqbuf[dsp->sample_out % dsp->N_IQBUF]);
|
||||
}
|
||||
if (dsp->sample_out == dsp->sample_posnoise+dsp->len_sq) {
|
||||
dsp->V_noise /= (double)dsp->len_sq;
|
||||
if (dsp->V_signal > 0 && dsp->V_noise > 0) {
|
||||
// iq-samples/V [-1..1]
|
||||
// dBw = 2*dBv, P=c*U*U
|
||||
// dBw = 2*10*log10(V/V0)
|
||||
dsp->SNRdB = 20.0 * log10(dsp->V_signal/dsp->V_noise+1e-20);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else dsp->SNRdB = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int f32buf_sample(dsp_t *dsp, int inv) {
|
||||
float s = 0.0;
|
||||
float xneu, xalt;
|
||||
|
||||
float complex z, w, z0;
|
||||
//static float complex z0; //= 1.0;
|
||||
double gain = 1.0;
|
||||
|
||||
double t = dsp->sample_in / (double)dsp->sr;
|
||||
|
||||
if (dsp->opt_iq) {
|
||||
|
||||
if ( f32read_csample(dsp, &z) == EOF ) return EOF;
|
||||
dsp->raw_iqbuf[dsp->sample_in % dsp->N_IQBUF] = z;
|
||||
|
||||
z *= cexp(-t*2*M_PI*dsp->df*I);
|
||||
z0 = dsp->rot_iqbuf[(dsp->sample_in-1 + dsp->N_IQBUF) % dsp->N_IQBUF];
|
||||
w = z * conj(z0);
|
||||
s = gain * carg(w)/M_PI;
|
||||
//z0 = z;
|
||||
dsp->rot_iqbuf[dsp->sample_in % dsp->N_IQBUF] = z;
|
||||
|
||||
get_SNR(dsp);
|
||||
|
||||
|
||||
if (dsp->opt_iq >= 2)
|
||||
{
|
||||
double xbit = 0.0;
|
||||
//float complex xi = cexp(+I*M_PI*dsp->h/dsp->sps);
|
||||
double f1 = -dsp->h*dsp->sr/(2*dsp->sps);
|
||||
double f2 = -f1;
|
||||
|
||||
float complex X1 = 0;
|
||||
float complex X2 = 0;
|
||||
|
||||
int n = dsp->sps;
|
||||
while (n > 0) {
|
||||
n--;
|
||||
t = -n / (double)dsp->sr;
|
||||
z = dsp->rot_iqbuf[(dsp->sample_in - n + dsp->N_IQBUF) % dsp->N_IQBUF]; // +1
|
||||
X1 += z*cexp(-t*2*M_PI*f1*I);
|
||||
X2 += z*cexp(-t*2*M_PI*f2*I);
|
||||
}
|
||||
|
||||
xbit = cabs(X2) - cabs(X1);
|
||||
|
||||
s = xbit / dsp->sps;
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (f32read_sample(dsp, &s) == EOF) return EOF;
|
||||
}
|
||||
|
||||
if (inv) s = -s; // swap IQ?
|
||||
dsp->bufs[dsp->sample_in % dsp->M] = s - dsp->dc_ofs;
|
||||
|
||||
xneu = dsp->bufs[(dsp->sample_in ) % dsp->M];
|
||||
xalt = dsp->bufs[(dsp->sample_in+dsp->M - dsp->Nvar) % dsp->M];
|
||||
dsp->xsum += xneu - xalt; // + xneu - xalt
|
||||
dsp->qsum += (xneu - xalt)*(xneu + xalt); // + xneu*xneu - xalt*xalt
|
||||
dsp->xs[dsp->sample_in % dsp->M] = dsp->xsum;
|
||||
dsp->qs[dsp->sample_in % dsp->M] = dsp->qsum;
|
||||
|
||||
|
||||
dsp->sample_out = dsp->sample_in - dsp->delay;
|
||||
|
||||
dsp->sample_in += 1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int read_bufbit(dsp_t *dsp, int symlen, char *bits, ui32_t mvp, int pos) {
|
||||
// symlen==2: manchester2 0->10,1->01->1: 2.bit
|
||||
|
||||
float rbitgrenze = pos*symlen*dsp->sps;
|
||||
ui32_t rcount = rbitgrenze+0.99; // ceil
|
||||
|
||||
double sum = 0.0;
|
||||
|
||||
|
||||
rbitgrenze += dsp->sps;
|
||||
do {
|
||||
sum += dsp->bufs[(rcount + mvp + dsp->M) % dsp->M];
|
||||
rcount++;
|
||||
} while (rcount < rbitgrenze); // n < dsp->sps
|
||||
|
||||
if (symlen == 2) {
|
||||
rbitgrenze += dsp->sps;
|
||||
do {
|
||||
sum -= dsp->bufs[(rcount + mvp + dsp->M) % dsp->M];
|
||||
rcount++;
|
||||
} while (rcount < rbitgrenze); // n < dsp->sps
|
||||
}
|
||||
|
||||
|
||||
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(dsp_t *dsp, int symlen, ui32_t mvp, int inv, int option_dc) {
|
||||
int errs = 0;
|
||||
int pos;
|
||||
int step = 1;
|
||||
char sign = 0;
|
||||
int len = dsp->hdrlen/symlen;
|
||||
|
||||
if (symlen != 1) step = 2;
|
||||
if (inv) sign=1;
|
||||
|
||||
for (pos = 0; pos < len; pos++) { // L = dsp->hdrlen * dsp->sps + 0.5;
|
||||
//read_bufbit(dsp, symlen, dsp->rawbits+pos*step, mvp+1-(int)(len*dsp->sps), pos);
|
||||
read_bufbit(dsp, symlen, dsp->rawbits+pos*step, mvp+1-dsp->L, pos);
|
||||
}
|
||||
dsp->rawbits[pos] = '\0';
|
||||
|
||||
while (len > 0) {
|
||||
if ((dsp->rawbits[len-1]^sign) != dsp->hdr[len-1]) errs += 1;
|
||||
len--;
|
||||
}
|
||||
|
||||
if (option_dc && errs < 3) {
|
||||
dsp->dc_ofs += dsp->dc;
|
||||
}
|
||||
|
||||
return errs;
|
||||
}
|
||||
|
||||
int get_fqofs_rs41(dsp_t *dsp, ui32_t mvp, float *freq, float *snr) {
|
||||
int j;
|
||||
int buf_start;
|
||||
int presamples;
|
||||
|
||||
// if(dsp->rs_typ == RS41_PREAMBLE) ...
|
||||
if (dsp->opt_iq)
|
||||
{
|
||||
presamples = 256*dsp->sps;
|
||||
|
||||
if (presamples > dsp->DFT.N2) presamples = dsp->DFT.N2;
|
||||
|
||||
buf_start = mvp - dsp->hdrlen*dsp->sps - presamples;
|
||||
|
||||
while (buf_start < 0) buf_start += dsp->N_IQBUF;
|
||||
|
||||
for (j = 0; j < dsp->DFT.N2; j++) {
|
||||
dsp->DFT.Z[j] = dsp->DFT.win[j]*dsp->raw_iqbuf[(buf_start+j) % dsp->N_IQBUF];
|
||||
}
|
||||
while (j < dsp->DFT.N) dsp->DFT.Z[j++] = 0;
|
||||
|
||||
raw_dft(&dsp->DFT, dsp->DFT.Z);
|
||||
dsp->df = bin2freq(&dsp->DFT, max_bin(&dsp->DFT, dsp->DFT.Z));
|
||||
|
||||
// if |df|<eps, +-2400Hz dominant (rs41)
|
||||
if (fabs(dsp->df) > 1000.0) dsp->df = 0.0;
|
||||
|
||||
|
||||
dsp->sample_posframe = dsp->sample_in; // > sample_out //mvp - dsp->hdrlen*dsp->sps;
|
||||
dsp->sample_posnoise = mvp + dsp->sr*7/8.0; // rs41
|
||||
|
||||
|
||||
*freq = dsp->df;
|
||||
*snr = dsp->SNRdB;
|
||||
}
|
||||
else return -1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
int read_slbit(dsp_t *dsp, int symlen, int *bit, int inv, int ofs, int pos, float l, int spike) {
|
||||
// symlen==2: manchester2 10->0,01->1: 2.bit
|
||||
|
||||
float bitgrenze = pos*symlen*dsp->sps;
|
||||
ui32_t scount = ceil(bitgrenze);//+0.99; // dfm?
|
||||
|
||||
float sample;
|
||||
float avg;
|
||||
float ths = 0.5, scale = 0.27;
|
||||
|
||||
double sum = 0.0;
|
||||
double mid;
|
||||
//double l = 0.5 .. 1.0 .. sps/2;
|
||||
|
||||
if (pos == 0) scount = 0;
|
||||
|
||||
if (symlen == 2) {
|
||||
mid = bitgrenze + (dsp->sps-1)/2.0;
|
||||
bitgrenze += dsp->sps;
|
||||
do {
|
||||
if (dsp->buffered > 0) dsp->buffered -= 1;
|
||||
else if (f32buf_sample(dsp, inv) == EOF) return EOF;
|
||||
|
||||
sample = dsp->bufs[(dsp->sample_out-dsp->buffered + ofs + dsp->M) % dsp->M];
|
||||
avg = 0.5*(dsp->bufs[(dsp->sample_out-dsp->buffered-1 + ofs + dsp->M) % dsp->M]
|
||||
+dsp->bufs[(dsp->sample_out-dsp->buffered+1 + ofs + dsp->M) % dsp->M]);
|
||||
if (spike && fabs(sample - avg) > ths) sample = avg + scale*(sample - avg); // spikes
|
||||
|
||||
if ( l < 0 || (mid-l < scount && scount < mid+l)) sum -= sample;
|
||||
|
||||
scount++;
|
||||
} while (scount < bitgrenze); // n < dsp->sps
|
||||
}
|
||||
|
||||
mid = bitgrenze + (dsp->sps-1)/2.0;
|
||||
bitgrenze += dsp->sps;
|
||||
do {
|
||||
if (dsp->buffered > 0) dsp->buffered -= 1;
|
||||
else if (f32buf_sample(dsp, inv) == EOF) return EOF;
|
||||
|
||||
sample = dsp->bufs[(dsp->sample_out-dsp->buffered + ofs + dsp->M) % dsp->M];
|
||||
avg = 0.5*(dsp->bufs[(dsp->sample_out-dsp->buffered-1 + ofs + dsp->M) % dsp->M]
|
||||
+dsp->bufs[(dsp->sample_out-dsp->buffered+1 + ofs + dsp->M) % dsp->M]);
|
||||
if (spike && fabs(sample - avg) > ths) sample = avg + scale*(sample - avg); // spikes
|
||||
|
||||
if ( l < 0 || (mid-l < scount && scount < mid+l)) sum += sample;
|
||||
|
||||
scount++;
|
||||
} while (scount < bitgrenze); // n < dsp->sps
|
||||
|
||||
|
||||
if (sum >= 0) *bit = 1;
|
||||
else *bit = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
|
||||
#define SQRT2 1.4142135624 // sqrt(2)
|
||||
// sigma = sqrt(log(2)) / (2*PI*BT):
|
||||
//#define SIGMA 0.2650103635 // BT=0.5: 0.2650103635 , BT=0.3: 0.4416839392
|
||||
|
||||
// Gaussian FM-pulse
|
||||
static double Q(double x) {
|
||||
return 0.5 - 0.5*erf(x/SQRT2);
|
||||
}
|
||||
static double pulse(double t, double sigma) {
|
||||
return Q((t-0.5)/sigma) - Q((t+0.5)/sigma);
|
||||
}
|
||||
|
||||
|
||||
static double norm2_vect(float *vect, int n) {
|
||||
int i;
|
||||
double x, y = 0.0;
|
||||
for (i = 0; i < n; i++) {
|
||||
x = vect[i];
|
||||
y += x*x;
|
||||
}
|
||||
return y;
|
||||
}
|
||||
|
||||
int init_buffers(dsp_t *dsp) {
|
||||
|
||||
int i, pos;
|
||||
float b0, b1, b2, b, t;
|
||||
float normMatch;
|
||||
double sigma = sqrt(log(2)) / (2*M_PI*dsp->BT);
|
||||
|
||||
int p2 = 1;
|
||||
int K, L, M;
|
||||
int n, k;
|
||||
float *m = NULL;
|
||||
|
||||
|
||||
L = dsp->hdrlen * dsp->sps + 0.5;
|
||||
M = 3*L;
|
||||
//if (dsp->sps < 6) M = 6*L;
|
||||
|
||||
dsp->delay = L/16;
|
||||
dsp->sample_in = 0;
|
||||
|
||||
p2 = 1;
|
||||
while (p2 < M) p2 <<= 1;
|
||||
while (p2 < 0x2000) p2 <<= 1; // or 0x4000, if sample not too short
|
||||
M = p2;
|
||||
dsp->DFT.N = p2;
|
||||
dsp->DFT.LOG2N = log(dsp->DFT.N)/log(2)+0.1; // 32bit cpu ... intermediate floating-point precision
|
||||
//while ((1 << dsp->DFT.LOG2N) < dsp->DFT.N) dsp->DFT.LOG2N++; // better N = (1 << LOG2N) ...
|
||||
|
||||
K = M-L - dsp->delay; // L+K < M
|
||||
|
||||
dsp->DFT.sr = dsp->sr;
|
||||
|
||||
dsp->K = K;
|
||||
dsp->L = L;
|
||||
dsp->M = M;
|
||||
|
||||
dsp->Nvar = L; // wenn Nvar fuer xnorm, dann Nvar=rshd.L
|
||||
|
||||
|
||||
dsp->bufs = (float *)calloc( M+1, sizeof(float)); if (dsp->bufs == NULL) return -100;
|
||||
dsp->match = (float *)calloc( L+1, sizeof(float)); if (dsp->match == NULL) return -100;
|
||||
|
||||
dsp->xs = (float *)calloc( M+1, sizeof(float)); if (dsp->xs == NULL) return -100;
|
||||
dsp->qs = (float *)calloc( M+1, sizeof(float)); if (dsp->qs == NULL) return -100;
|
||||
|
||||
dsp->rawbits = (char *)calloc( 2*dsp->hdrlen+1, sizeof(char)); if (dsp->rawbits == NULL) return -100;
|
||||
|
||||
|
||||
for (i = 0; i < M; i++) dsp->bufs[i] = 0.0;
|
||||
|
||||
|
||||
for (i = 0; i < L; i++) {
|
||||
pos = i/dsp->sps;
|
||||
t = (i - pos*dsp->sps)/dsp->sps - 0.5;
|
||||
|
||||
b1 = ((dsp->hdr[pos] & 0x1) - 0.5)*2.0;
|
||||
b = b1*pulse(t, sigma);
|
||||
|
||||
if (pos > 0) {
|
||||
b0 = ((dsp->hdr[pos-1] & 0x1) - 0.5)*2.0;
|
||||
b += b0*pulse(t+1, sigma);
|
||||
}
|
||||
|
||||
if (pos < dsp->hdrlen-1) {
|
||||
b2 = ((dsp->hdr[pos+1] & 0x1) - 0.5)*2.0;
|
||||
b += b2*pulse(t-1, sigma);
|
||||
}
|
||||
|
||||
dsp->match[i] = b;
|
||||
}
|
||||
|
||||
normMatch = sqrt( norm2_vect(dsp->match, L) );
|
||||
for (i = 0; i < L; i++) {
|
||||
dsp->match[i] /= normMatch;
|
||||
}
|
||||
|
||||
|
||||
dsp->DFT.xn = calloc(dsp->DFT.N+1, sizeof(float)); if (dsp->DFT.xn == NULL) return -1;
|
||||
|
||||
dsp->DFT.Fm = calloc(dsp->DFT.N+1, sizeof(float complex)); if (dsp->DFT.Fm == NULL) return -1;
|
||||
dsp->DFT.X = calloc(dsp->DFT.N+1, sizeof(float complex)); if (dsp->DFT.X == NULL) return -1;
|
||||
dsp->DFT.Z = calloc(dsp->DFT.N+1, sizeof(float complex)); if (dsp->DFT.Z == NULL) return -1;
|
||||
dsp->DFT.cx = calloc(dsp->DFT.N+1, sizeof(float complex)); if (dsp->DFT.cx == NULL) return -1;
|
||||
|
||||
dsp->DFT.ew = calloc(dsp->DFT.LOG2N+1, sizeof(float complex)); if (dsp->DFT.ew == NULL) return -1;
|
||||
|
||||
// FFT window
|
||||
// a) N2 = N
|
||||
// b) N2 < N (interpolation)
|
||||
dsp->DFT.win = calloc(dsp->DFT.N+1, sizeof(float complex)); if (dsp->DFT.win == NULL) return -1; // float real
|
||||
dsp->DFT.N2 = dsp->DFT.N;
|
||||
//dsp->DFT.N2 = dsp->DFT.N/2 - 1; // N=2^log2N
|
||||
dft_window(&dsp->DFT, 1);
|
||||
|
||||
for (n = 0; n < dsp->DFT.LOG2N; n++) {
|
||||
k = 1 << n;
|
||||
dsp->DFT.ew[n] = cexp(-I*M_PI/(float)k);
|
||||
}
|
||||
|
||||
m = calloc(dsp->DFT.N+1, sizeof(float)); if (m == NULL) return -1;
|
||||
for (i = 0; i < L; i++) m[L-1 - i] = dsp->match[i]; // t = L-1
|
||||
while (i < dsp->DFT.N) m[i++] = 0.0;
|
||||
rdft(&dsp->DFT, m, dsp->DFT.Fm);
|
||||
|
||||
free(m); m = NULL;
|
||||
|
||||
|
||||
if (dsp->opt_iq)
|
||||
{
|
||||
if (dsp->nch < 2) return -1;
|
||||
|
||||
dsp->N_IQBUF = dsp->DFT.N;
|
||||
dsp->raw_iqbuf = calloc(dsp->N_IQBUF+1, sizeof(float complex)); if (dsp->raw_iqbuf == NULL) return -1;
|
||||
dsp->rot_iqbuf = calloc(dsp->N_IQBUF+1, sizeof(float complex)); if (dsp->rot_iqbuf == NULL) return -1;
|
||||
|
||||
dsp->len_sq = dsp->sps*8;
|
||||
}
|
||||
|
||||
|
||||
return K;
|
||||
}
|
||||
|
||||
int free_buffers(dsp_t *dsp) {
|
||||
|
||||
if (dsp->match) { free(dsp->match); dsp->match = NULL; }
|
||||
if (dsp->bufs) { free(dsp->bufs); dsp->bufs = NULL; }
|
||||
if (dsp->xs) { free(dsp->xs); dsp->xs = NULL; }
|
||||
if (dsp->qs) { free(dsp->qs); dsp->qs = NULL; }
|
||||
if (dsp->rawbits) { free(dsp->rawbits); dsp->rawbits = NULL; }
|
||||
|
||||
if (dsp->DFT.xn) { free(dsp->DFT.xn); dsp->DFT.xn = NULL; }
|
||||
if (dsp->DFT.ew) { free(dsp->DFT.ew); dsp->DFT.ew = NULL; }
|
||||
if (dsp->DFT.Fm) { free(dsp->DFT.Fm); dsp->DFT.Fm = NULL; }
|
||||
if (dsp->DFT.X) { free(dsp->DFT.X); dsp->DFT.X = NULL; }
|
||||
if (dsp->DFT.Z) { free(dsp->DFT.Z); dsp->DFT.Z = NULL; }
|
||||
if (dsp->DFT.cx) { free(dsp->DFT.cx); dsp->DFT.cx = NULL; }
|
||||
|
||||
if (dsp->DFT.win) { free(dsp->DFT.win); dsp->DFT.win = NULL; }
|
||||
|
||||
if (dsp->opt_iq)
|
||||
{
|
||||
if (dsp->raw_iqbuf) { free(dsp->raw_iqbuf); dsp->raw_iqbuf = NULL; }
|
||||
if (dsp->rot_iqbuf) { free(dsp->rot_iqbuf); dsp->rot_iqbuf = NULL; }
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
ui32_t get_sample(dsp_t *dsp) {
|
||||
return dsp->sample_out;
|
||||
}
|
||||
|
115
demod/demod_iq.h
115
demod/demod_iq.h
|
@ -1,115 +0,0 @@
|
|||
|
||||
|
||||
#include <math.h>
|
||||
#include <complex.h>
|
||||
|
||||
typedef unsigned char ui8_t;
|
||||
typedef unsigned short ui16_t;
|
||||
typedef unsigned int ui32_t;
|
||||
typedef char i8_t;
|
||||
typedef short i16_t;
|
||||
typedef int i32_t;
|
||||
|
||||
|
||||
typedef struct {
|
||||
int sr; // sample_rate
|
||||
int LOG2N;
|
||||
int N;
|
||||
int N2;
|
||||
float *xn;
|
||||
float complex *ew;
|
||||
float complex *Fm;
|
||||
float complex *X;
|
||||
float complex *Z;
|
||||
float complex *cx;
|
||||
float complex *win; // float real
|
||||
} dft_t;
|
||||
|
||||
|
||||
typedef struct {
|
||||
FILE *fp;
|
||||
//
|
||||
int sr; // sample_rate
|
||||
int bps; // bits/sample
|
||||
int nch; // channels
|
||||
int ch; // select channel
|
||||
//
|
||||
int symlen;
|
||||
float sps; // samples per symbol
|
||||
float _spb; // samples per bit
|
||||
float br; // baud rate
|
||||
//
|
||||
ui32_t sample_in;
|
||||
ui32_t sample_out;
|
||||
ui32_t delay;
|
||||
int buffered;
|
||||
int L;
|
||||
int M;
|
||||
int K;
|
||||
float *match;
|
||||
float *bufs;
|
||||
float dc_ofs;
|
||||
float dc;
|
||||
//
|
||||
int N_norm;
|
||||
int Nvar;
|
||||
float xsum;
|
||||
float qsum;
|
||||
float *xs;
|
||||
float *qs;
|
||||
|
||||
// IQ-data
|
||||
int opt_iq;
|
||||
int N_IQBUF;
|
||||
float complex *raw_iqbuf;
|
||||
float complex *rot_iqbuf;
|
||||
|
||||
//
|
||||
char *rawbits;
|
||||
char *hdr;
|
||||
int hdrlen;
|
||||
|
||||
//
|
||||
float BT; // bw/time (ISI)
|
||||
float h; // modulation index
|
||||
|
||||
// DFT
|
||||
dft_t DFT;
|
||||
|
||||
double df;
|
||||
int len_sq;
|
||||
|
||||
ui32_t sample_posframe;
|
||||
ui32_t sample_posnoise;
|
||||
|
||||
double V_noise;
|
||||
double V_signal;
|
||||
double SNRdB;
|
||||
|
||||
} dsp_t;
|
||||
|
||||
|
||||
typedef struct {
|
||||
int sr; // sample_rate
|
||||
int bps; // bits_sample bits/sample
|
||||
int nch; // channels
|
||||
int sel_ch; // select wav channel
|
||||
} pcm_t;
|
||||
|
||||
|
||||
|
||||
float read_wav_header(pcm_t *, FILE *);
|
||||
int f32buf_sample(dsp_t *, int);
|
||||
int read_slbit(dsp_t *, int, int*, int, int, int, float, int);
|
||||
|
||||
int getCorrDFT(dsp_t *, ui32_t, float *, ui32_t *);
|
||||
int headcmp(dsp_t *, int, ui32_t, int, int);
|
||||
int get_fqofs_rs41(dsp_t *, ui32_t, float *, float *);
|
||||
float get_bufvar(dsp_t *, int);
|
||||
float get_bufmu(dsp_t *, int);
|
||||
|
||||
int init_buffers(dsp_t *);
|
||||
int free_buffers(dsp_t *);
|
||||
|
||||
ui32_t get_sample(dsp_t *);
|
||||
|
744
demod/dfm09dm.c
744
demod/dfm09dm.c
|
@ -1,744 +0,0 @@
|
|||
|
||||
/*
|
||||
* dfm09 (dfm06)
|
||||
* sync header: correlation/matched filter
|
||||
* files: dfm09dm.c demod.h demod.c
|
||||
* compile:
|
||||
* gcc -c demod.c
|
||||
* gcc dfm09dm.c demod.o -lm -o dfm09dm
|
||||
*
|
||||
* author: zilog80
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#ifdef CYGWIN
|
||||
#include <fcntl.h> // cygwin: _setmode()
|
||||
#include <io.h>
|
||||
#endif
|
||||
|
||||
|
||||
typedef unsigned char ui8_t;
|
||||
typedef unsigned int ui32_t;
|
||||
|
||||
//#include "demod.c"
|
||||
#include "demod.h"
|
||||
|
||||
|
||||
typedef struct {
|
||||
int frnr;
|
||||
int sonde_typ;
|
||||
ui32_t SN6;
|
||||
ui32_t SN9;
|
||||
int week; int gpssec;
|
||||
int jahr; int monat; int tag;
|
||||
int std; int min; float sek;
|
||||
double lat; double lon; double alt;
|
||||
double dir; double horiV; double vertV;
|
||||
float meas24[5];
|
||||
float status[2];
|
||||
} gpx_t;
|
||||
|
||||
gpx_t gpx;
|
||||
|
||||
char dat_str[9][13+1];
|
||||
|
||||
|
||||
int option_verbose = 0, // ausfuehrliche Anzeige
|
||||
option_raw = 0, // rohe Frames
|
||||
option_inv = 0, // invertiert Signal
|
||||
option_ecc = 0,
|
||||
option_ptu = 0,
|
||||
option_ths = 0,
|
||||
wavloaded = 0;
|
||||
|
||||
int start = 0;
|
||||
|
||||
|
||||
//#define HEADLEN 32
|
||||
// DFM09: Manchester2: 01->1,10->0
|
||||
char rawheader[] = "10011010100110010101101001010101"; //->"0100010111001111"; // 0x45CF (big endian)
|
||||
|
||||
#define BITFRAME_LEN 280
|
||||
char frame_bits[BITFRAME_LEN+4] = "0100010111001111";
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
#define BAUD_RATE 2500
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
|
||||
#define B 8 // codeword: 8 bit
|
||||
#define S 4 // davon 4 bit data
|
||||
|
||||
#define HEAD 0 // 16 bit
|
||||
#define CONF (16+0) // 56 bit
|
||||
#define DAT1 (16+56) // 104 bit
|
||||
#define DAT2 (16+160) // 104 bit
|
||||
// frame: 280 bit
|
||||
|
||||
ui8_t H[4][8] = // Parity-Check
|
||||
{{ 0, 1, 1, 1, 1, 0, 0, 0},
|
||||
{ 1, 0, 1, 1, 0, 1, 0, 0},
|
||||
{ 1, 1, 0, 1, 0, 0, 1, 0},
|
||||
{ 1, 1, 1, 0, 0, 0, 0, 1}};
|
||||
ui8_t He[8] = { 0x7, 0xB, 0xD, 0xE, 0x8, 0x4, 0x2, 0x1}; // Spalten von H:
|
||||
// 1-bit-error-Syndrome
|
||||
ui8_t hamming_conf[ 7*B]; // 7*8=56
|
||||
ui8_t hamming_dat1[13*B]; // 13*8=104
|
||||
ui8_t hamming_dat2[13*B];
|
||||
|
||||
ui8_t block_conf[ 7*S]; // 7*4=28
|
||||
ui8_t block_dat1[13*S]; // 13*4=52
|
||||
ui8_t block_dat2[13*S];
|
||||
|
||||
ui32_t bits2val(ui8_t *bits, int len) { // big endian
|
||||
int j;
|
||||
ui32_t val;
|
||||
if ((len < 0) || (len > 32)) return -1; // = 0xFFFF
|
||||
val = 0;
|
||||
for (j = 0; j < len; j++) {
|
||||
val |= (bits[j] << (len-1-j));
|
||||
}
|
||||
return val;
|
||||
}
|
||||
|
||||
void deinterleave(char *str, int L, ui8_t *block) {
|
||||
int i, j;
|
||||
for (j = 0; j < B; j++) { // L = 7, 13
|
||||
for (i = 0; i < L; i++) {
|
||||
if (str[L*j+i] >= 0x30 && str[L*j+i] <= 0x31) {
|
||||
block[B*i+j] = str[L*j+i] - 0x30; // ASCII -> bit
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int check(ui8_t code[8]) {
|
||||
int i, j; // Bei Demodulierung durch Nulldurchgaenge, wenn durch Fehler ausser Takt,
|
||||
ui32_t synval = 0; // verschieben sich die bits. Fuer Hamming-Decode waere es besser,
|
||||
ui8_t syndrom[4]; // sync zu Beginn mit Header und dann Takt beibehalten fuer decision.
|
||||
int ret=0;
|
||||
|
||||
for (i = 0; i < 4; i++) { // S = 4
|
||||
syndrom[i] = 0;
|
||||
for (j = 0; j < 8; j++) { // B = 8
|
||||
syndrom[i] ^= H[i][j] & code[j];
|
||||
}
|
||||
}
|
||||
synval = bits2val(syndrom, 4);
|
||||
if (synval) {
|
||||
ret = -1;
|
||||
for (j = 0; j < 8; j++) { // 1-bit-error
|
||||
if (synval == He[j]) { // reicht auf databits zu pruefen, d.h.
|
||||
ret = j+1; // (systematischer Code) He[0..3]
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
else ret = 0;
|
||||
if (ret > 0) code[ret-1] ^= 0x1;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int hamming(ui8_t *ham, int L, ui8_t *sym) {
|
||||
int i, j;
|
||||
int ret = 0; // L = 7, 13
|
||||
for (i = 0; i < L; i++) { // L * 2 nibble (data+parity)
|
||||
if (option_ecc) ret |= check(ham+B*i);
|
||||
for (j = 0; j < S; j++) { // systematic: bits 0..S-1 data
|
||||
sym[S*i+j] = ham[B*i+j];
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
char nib2chr(ui8_t nib) {
|
||||
char c = '_';
|
||||
if (nib < 0x10) {
|
||||
if (nib < 0xA) c = 0x30 + nib;
|
||||
else c = 0x41 + nib-0xA;
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
int dat_out(ui8_t *dat_bits) {
|
||||
int i, ret = 0;
|
||||
static int fr_id;
|
||||
// int jahr = 0, monat = 0, tag = 0, std = 0, min = 0;
|
||||
int frnr = 0;
|
||||
int msek = 0;
|
||||
int lat = 0, lon = 0, alt = 0;
|
||||
int nib;
|
||||
int dvv; // signed/unsigned 16bit
|
||||
|
||||
fr_id = bits2val(dat_bits+48, 4);
|
||||
|
||||
|
||||
if (fr_id >= 0 && fr_id <= 8) {
|
||||
for (i = 0; i < 13; i++) {
|
||||
nib = bits2val(dat_bits+4*i, 4);
|
||||
dat_str[fr_id][i] = nib2chr(nib);
|
||||
}
|
||||
dat_str[fr_id][13] = '\0';
|
||||
}
|
||||
|
||||
if (fr_id == 0) {
|
||||
start = 1;
|
||||
frnr = bits2val(dat_bits+24, 8);
|
||||
gpx.frnr = frnr;
|
||||
}
|
||||
|
||||
if (fr_id == 1) {
|
||||
// 00..31: ? GPS-Sats in Sicht?
|
||||
msek = bits2val(dat_bits+32, 16);
|
||||
gpx.sek = msek/1000.0;
|
||||
}
|
||||
|
||||
if (fr_id == 2) {
|
||||
lat = bits2val(dat_bits, 32);
|
||||
gpx.lat = lat/1e7;
|
||||
dvv = (short)bits2val(dat_bits+32, 16); // (short)? zusammen mit dir sollte unsigned sein
|
||||
gpx.horiV = dvv/1e2;
|
||||
}
|
||||
|
||||
if (fr_id == 3) {
|
||||
lon = bits2val(dat_bits, 32);
|
||||
gpx.lon = lon/1e7;
|
||||
dvv = bits2val(dat_bits+32, 16) & 0xFFFF; // unsigned
|
||||
gpx.dir = dvv/1e2;
|
||||
}
|
||||
|
||||
if (fr_id == 4) {
|
||||
alt = bits2val(dat_bits, 32);
|
||||
gpx.alt = alt/1e2;
|
||||
dvv = (short)bits2val(dat_bits+32, 16); // signed
|
||||
gpx.vertV = dvv/1e2;
|
||||
}
|
||||
|
||||
if (fr_id == 5) {
|
||||
}
|
||||
|
||||
if (fr_id == 6) {
|
||||
}
|
||||
|
||||
if (fr_id == 7) {
|
||||
}
|
||||
|
||||
if (fr_id == 8) {
|
||||
gpx.jahr = bits2val(dat_bits, 12);
|
||||
gpx.monat = bits2val(dat_bits+12, 4);
|
||||
gpx.tag = bits2val(dat_bits+16, 5);
|
||||
gpx.std = bits2val(dat_bits+21, 5);
|
||||
gpx.min = bits2val(dat_bits+26, 6);
|
||||
}
|
||||
|
||||
ret = fr_id;
|
||||
return ret;
|
||||
}
|
||||
|
||||
// DFM-06 (NXP8)
|
||||
float fl20(int d) { // float20
|
||||
int val, p;
|
||||
float f;
|
||||
p = (d>>16) & 0xF;
|
||||
val = d & 0xFFFF;
|
||||
f = val/(float)(1<<p);
|
||||
return f;
|
||||
}
|
||||
/*
|
||||
float flo20(int d) {
|
||||
int m, e;
|
||||
float f1, f;
|
||||
m = d & 0xFFFF;
|
||||
e = (d >> 16) & 0xF;
|
||||
f = m / pow(2,e);
|
||||
return f;
|
||||
}
|
||||
*/
|
||||
|
||||
// DFM-09 (STM32)
|
||||
float fl24(int d) { // float24
|
||||
int val, p;
|
||||
float f;
|
||||
p = (d>>20) & 0xF;
|
||||
val = d & 0xFFFFF;
|
||||
f = val/(float)(1<<p);
|
||||
return f;
|
||||
}
|
||||
|
||||
// temperature approximation
|
||||
float get_Temp(float *meas) { // meas[0..4]
|
||||
// NTC-Thermistor EPCOS B57540G0502
|
||||
// R/T No 8402, R25=Ro=5k
|
||||
// B0/100=3450
|
||||
// 1/T = 1/To + 1/B log(r) , r=R/Ro
|
||||
// GRAW calibration data -80C..+40C on EEPROM ?
|
||||
// meas0 = g*(R + Rs)
|
||||
// meas3 = g*Rs , Rs: dfm6:10k, dfm9:20k
|
||||
// meas4 = g*Rf , Rf=220k
|
||||
float B0 = 3260.0; // B/Kelvin, fit -55C..+40C
|
||||
float T0 = 25 + 273.15; // t0=25C
|
||||
float R0 = 5.0e3; // R0=R25=5k
|
||||
float Rf = 220e3; // Rf = 220k
|
||||
float g = meas[4]/Rf;
|
||||
float R = (meas[0]-meas[3]) / g; // meas[0,3,4] > 0 ?
|
||||
float T = 0; // T/Kelvin
|
||||
if (meas[0]*meas[3]*meas[4] == 0) R = 0;
|
||||
if (R > 0) T = 1/(1/T0 + 1/B0 * log(R/R0));
|
||||
return T - 273.15; // Celsius
|
||||
// DFM-06: meas20 * 16 = meas24
|
||||
// -> (meas24[0]-meas24[3])/meas24[4]=(meas20[0]-meas20[3])/meas20[4]
|
||||
}
|
||||
float get_Temp2(float *meas) { // meas[0..4]
|
||||
// NTC-Thermistor EPCOS B57540G0502
|
||||
// R/T No 8402, R25=Ro=5k
|
||||
// B0/100=3450
|
||||
// 1/T = 1/To + 1/B log(r) , r=R/Ro
|
||||
// GRAW calibration data -80C..+40C on EEPROM ?
|
||||
// meas0 = g*(R+Rs)+ofs
|
||||
// meas3 = g*Rs+ofs , Rs: dfm6:10k, dfm9:20k
|
||||
// meas4 = g*Rf+ofs , Rf=220k
|
||||
float f = meas[0],
|
||||
f1 = meas[3],
|
||||
f2 = meas[4];
|
||||
float B0 = 3260.0; // B/Kelvin, fit -55C..+40C
|
||||
float T0 = 25 + 273.15; // t0=25C
|
||||
float R0 = 5.0e3; // R0=R25=5k
|
||||
float Rf2 = 220e3; // Rf2 = Rf = 220k
|
||||
float g_o = f2/Rf2; // approx gain
|
||||
float Rs_o = f1/g_o; // = Rf2 * f1/f2;
|
||||
float Rf1 = Rs_o; // Rf1 = Rs: dfm6:10k, dfm9:20k
|
||||
float g = g_o; // gain
|
||||
float Rb = 0.0; // offset
|
||||
float R = 0; // thermistor
|
||||
float T = 0; // T/Kelvin
|
||||
|
||||
if ( 8e3 < Rs_o && Rs_o < 12e3) Rf1 = 10e3; // dfm6
|
||||
else if (18e3 < Rs_o && Rs_o < 22e3) Rf1 = 20e3; // dfm9
|
||||
g = (f2 - f1) / (Rf2 - Rf1);
|
||||
Rb = (f1*Rf2-f2*Rf1)/(f2-f1); // ofs/g
|
||||
|
||||
R = (f-f1)/g; // meas[0,3,4] > 0 ?
|
||||
if (R > 0) T = 1/(1/T0 + 1/B0 * log(R/R0));
|
||||
|
||||
if (option_ptu && option_verbose == 2) {
|
||||
printf(" (Rso: %.1f , Rb: %.1f)", Rs_o/1e3, Rb/1e3);
|
||||
}
|
||||
|
||||
return T - 273.15;
|
||||
// DFM-06: meas20 * 16 = meas24
|
||||
}
|
||||
float get_Temp4(float *meas) { // meas[0..4]
|
||||
// NTC-Thermistor EPCOS B57540G0502
|
||||
// [ T/C , R/R25 , alpha ] :
|
||||
// [ -55.0 , 51.991 , 6.4 ]
|
||||
// [ -50.0 , 37.989 , 6.2 ]
|
||||
// [ -45.0 , 28.07 , 5.9 ]
|
||||
// [ -40.0 , 20.96 , 5.7 ]
|
||||
// [ -35.0 , 15.809 , 5.5 ]
|
||||
// [ -30.0 , 12.037 , 5.4 ]
|
||||
// [ -25.0 , 9.2484 , 5.2 ]
|
||||
// [ -20.0 , 7.1668 , 5.0 ]
|
||||
// [ -15.0 , 5.5993 , 4.9 ]
|
||||
// [ -10.0 , 4.4087 , 4.7 ]
|
||||
// [ -5.0 , 3.4971 , 4.6 ]
|
||||
// [ 0.0 , 2.7936 , 4.4 ]
|
||||
// [ 5.0 , 2.2468 , 4.3 ]
|
||||
// [ 10.0 , 1.8187 , 4.2 ]
|
||||
// [ 15.0 , 1.4813 , 4.0 ]
|
||||
// [ 20.0 , 1.2136 , 3.9 ]
|
||||
// [ 25.0 , 1.0000 , 3.8 ]
|
||||
// [ 30.0 , 0.82845 , 3.7 ]
|
||||
// [ 35.0 , 0.68991 , 3.6 ]
|
||||
// [ 40.0 , 0.57742 , 3.5 ]
|
||||
// -> Steinhart–Hart coefficients (polyfit):
|
||||
float p0 = 1.09698417e-03,
|
||||
p1 = 2.39564629e-04,
|
||||
p2 = 2.48821437e-06,
|
||||
p3 = 5.84354921e-08;
|
||||
// T/K = 1/( p0 + p1*ln(R) + p2*ln(R)^2 + p3*ln(R)^3 )
|
||||
float Rf = 220e3; // Rf = 220k
|
||||
float g = meas[4]/Rf;
|
||||
float R = (meas[0]-meas[3]) / g; // meas[0,3,4] > 0 ?
|
||||
float T = 0; // T/Kelvin
|
||||
if (R > 0) T = 1/( p0 + p1*log(R) + p2*log(R)*log(R) + p3*log(R)*log(R)*log(R) );
|
||||
return T - 273.15; // Celsius
|
||||
// DFM-06: meas20 * 16 = meas24
|
||||
// -> (meas24[0]-meas24[3])/meas24[4]=(meas20[0]-meas20[3])/meas20[4]
|
||||
}
|
||||
|
||||
|
||||
#define SNbit 0x0100
|
||||
int conf_out(ui8_t *conf_bits) {
|
||||
int conf_id;
|
||||
int ret = 0;
|
||||
int val, hl;
|
||||
static int chAbit, chA[2];
|
||||
ui32_t SN6, SN9;
|
||||
|
||||
conf_id = bits2val(conf_bits, 4);
|
||||
|
||||
//if (conf_id > 6) gpx.SN6 = 0; //// gpx.sonde_typ & 0xF = 9; // SNbit?
|
||||
|
||||
if ((gpx.sonde_typ & 0xFF) < 9 && conf_id == 6) {
|
||||
SN6 = bits2val(conf_bits+4, 4*6); // DFM-06: Kanal 6
|
||||
if ( SN6 == gpx.SN6 ) { // nur Nibble-Werte 0..9
|
||||
gpx.sonde_typ = SNbit | 6;
|
||||
ret = 6;
|
||||
}
|
||||
else {
|
||||
gpx.sonde_typ = 0;
|
||||
}
|
||||
gpx.SN6 = SN6;
|
||||
}
|
||||
if (conf_id == 0xA) { // 0xACxxxxy
|
||||
val = bits2val(conf_bits+8, 4*5);
|
||||
hl = (val & 1) == 0;
|
||||
chA[hl] = (val >> 4) & 0xFFFF;
|
||||
chAbit |= 1 << hl;
|
||||
if (chAbit == 3) { // DFM-09: Kanal A
|
||||
SN9 = (chA[1] << 16) | chA[0];
|
||||
if ( SN9 == gpx.SN9 ) {
|
||||
gpx.sonde_typ = SNbit | 9;
|
||||
ret = 9;
|
||||
}
|
||||
else {
|
||||
gpx.sonde_typ = 0;
|
||||
}
|
||||
gpx.SN9 = SN9;
|
||||
chAbit = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (conf_id >= 0 && conf_id <= 4) {
|
||||
val = bits2val(conf_bits+4, 4*6);
|
||||
gpx.meas24[conf_id] = fl24(val);
|
||||
// DFM-09 (STM32): 24bit 0exxxxx
|
||||
// DFM-06 (NXP8): 20bit 0exxxx0
|
||||
// fl20(bits2val(conf_bits+4, 4*5))
|
||||
// = fl20(exxxx)
|
||||
// = fl24(exxxx0)/2^4
|
||||
// meas20 * 16 = meas24
|
||||
}
|
||||
|
||||
// STM32-status: Bat, MCU-Temp
|
||||
if ((gpx.sonde_typ & 0xFF) == 9) { // DFM-09 (STM32)
|
||||
if (conf_id == 0x5) { // voltage
|
||||
val = bits2val(conf_bits+8, 4*4);
|
||||
gpx.status[0] = val/1000.0;
|
||||
}
|
||||
if (conf_id == 0x6) { // T-intern (STM32)
|
||||
val = bits2val(conf_bits+8, 4*4);
|
||||
gpx.status[1] = val/100.0;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void print_gpx() {
|
||||
int i, j;
|
||||
|
||||
if (start) {
|
||||
|
||||
if (option_raw == 2) {
|
||||
for (i = 0; i < 9; i++) {
|
||||
printf(" %s", dat_str[i]);
|
||||
}
|
||||
for (i = 0; i < 9; i++) {
|
||||
for (j = 0; j < 13; j++) dat_str[i][j] = ' ';
|
||||
}
|
||||
}
|
||||
else {
|
||||
//if (option_auto && option_verbose) printf("[%c] ", option_inv?'-':'+');
|
||||
printf("[%3d] ", gpx.frnr);
|
||||
printf("%4d-%02d-%02d ", gpx.jahr, gpx.monat, gpx.tag);
|
||||
printf("%02d:%02d:%04.1f ", gpx.std, gpx.min, gpx.sek);
|
||||
printf(" ");
|
||||
printf("lat: %.6f ", gpx.lat);
|
||||
printf("lon: %.6f ", gpx.lon);
|
||||
printf("alt: %.1f ", gpx.alt);
|
||||
printf(" vH: %5.2f ", gpx.horiV);
|
||||
printf(" D: %5.1f ", gpx.dir);
|
||||
printf(" vV: %5.2f ", gpx.vertV);
|
||||
if (option_ptu) {
|
||||
float t = get_Temp(gpx.meas24);
|
||||
if (t > -270.0) printf(" T=%.1fC ", t);
|
||||
if (option_verbose == 2) {
|
||||
float t2 = get_Temp2(gpx.meas24);
|
||||
float t4 = get_Temp4(gpx.meas24);
|
||||
if (t2 > -270.0) printf(" T2=%.1fC ", t2);
|
||||
if (t4 > -270.0) printf(" T4=%.1fC ", t4);
|
||||
printf(" f0: %.2f ", gpx.meas24[0]);
|
||||
printf(" f3: %.2f ", gpx.meas24[3]);
|
||||
printf(" f4: %.2f ", gpx.meas24[4]);
|
||||
}
|
||||
}
|
||||
if (option_verbose == 2 && (gpx.sonde_typ & 0xFF) == 9) {
|
||||
printf(" U: %.2fV ", gpx.status[0]);
|
||||
printf(" Ti: %.1fK ", gpx.status[1]);
|
||||
}
|
||||
if (option_verbose && (gpx.sonde_typ & SNbit))
|
||||
{
|
||||
if ((gpx.sonde_typ & 0xFF) == 6) {
|
||||
printf(" (ID%1d:%06X) ", gpx.sonde_typ & 0xF, gpx.SN6);
|
||||
}
|
||||
if ((gpx.sonde_typ & 0xFF) == 9) {
|
||||
printf(" (ID%1d:%06u) ", gpx.sonde_typ & 0xF, gpx.SN9);
|
||||
}
|
||||
gpx.sonde_typ ^= SNbit;
|
||||
}
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
int print_frame() {
|
||||
int i;
|
||||
int nib = 0;
|
||||
int frid = -1;
|
||||
int ret0, ret1, ret2;
|
||||
int ret = 0;
|
||||
|
||||
|
||||
deinterleave(frame_bits+CONF, 7, hamming_conf);
|
||||
deinterleave(frame_bits+DAT1, 13, hamming_dat1);
|
||||
deinterleave(frame_bits+DAT2, 13, hamming_dat2);
|
||||
|
||||
ret0 = hamming(hamming_conf, 7, block_conf);
|
||||
ret1 = hamming(hamming_dat1, 13, block_dat1);
|
||||
ret2 = hamming(hamming_dat2, 13, block_dat2);
|
||||
ret = ret0 | ret1 | ret2;
|
||||
|
||||
if (option_raw == 1) {
|
||||
|
||||
for (i = 0; i < 7; i++) {
|
||||
nib = bits2val(block_conf+S*i, S);
|
||||
printf("%01X", nib & 0xFF);
|
||||
}
|
||||
if (option_ecc) {
|
||||
if (ret0 == 0) printf(" [OK] ");
|
||||
else if (ret0 > 0) printf(" [KO] ");
|
||||
else printf(" [NO] ");
|
||||
}
|
||||
printf(" ");
|
||||
for (i = 0; i < 13; i++) {
|
||||
nib = bits2val(block_dat1+S*i, S);
|
||||
printf("%01X", nib & 0xFF);
|
||||
}
|
||||
if (option_ecc) {
|
||||
if (ret1 == 0) printf(" [OK] ");
|
||||
else if (ret1 > 0) printf(" [KO] ");
|
||||
else printf(" [NO] ");
|
||||
}
|
||||
printf(" ");
|
||||
for (i = 0; i < 13; i++) {
|
||||
nib = bits2val(block_dat2+S*i, S);
|
||||
printf("%01X", nib & 0xFF);
|
||||
}
|
||||
if (option_ecc) {
|
||||
if (ret2 == 0) printf(" [OK] ");
|
||||
else if (ret2 > 0) printf(" [KO] ");
|
||||
else printf(" [NO] ");
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
}
|
||||
else if (option_ecc) {
|
||||
|
||||
if (ret0 == 0 || ret0 > 0) {
|
||||
conf_out(block_conf);
|
||||
}
|
||||
if (ret1 == 0 || ret1 > 0) {
|
||||
frid = dat_out(block_dat1);
|
||||
if (frid == 8) print_gpx();
|
||||
}
|
||||
if (ret2 == 0 || ret2 > 0) {
|
||||
frid = dat_out(block_dat2);
|
||||
if (frid == 8) print_gpx();
|
||||
}
|
||||
|
||||
}
|
||||
else {
|
||||
|
||||
conf_out(block_conf);
|
||||
frid = dat_out(block_dat1);
|
||||
if (frid == 8) print_gpx();
|
||||
frid = dat_out(block_dat2);
|
||||
if (frid == 8) print_gpx();
|
||||
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
|
||||
FILE *fp = NULL;
|
||||
char *fpname = NULL;
|
||||
float spb = 0.0;
|
||||
int header_found = 0;
|
||||
int ret = 0;
|
||||
|
||||
int bit;
|
||||
int bitpos = 0;
|
||||
int bitQ;
|
||||
int pos;
|
||||
int herrs, herr1;
|
||||
int headerlen = 0;
|
||||
int frm = 0, nfrms = 8; // nfrms=1,2,4,8
|
||||
|
||||
float mv;
|
||||
unsigned int mv_pos, mv0_pos;
|
||||
|
||||
float thres = 0.6;
|
||||
|
||||
int bitofs = 0, dif = 0;
|
||||
int symlen = 2;
|
||||
|
||||
|
||||
#ifdef CYGWIN
|
||||
_setmode(fileno(stdin), _O_BINARY); // _setmode(_fileno(stdin), _O_BINARY);
|
||||
#endif
|
||||
setbuf(stdout, NULL);
|
||||
|
||||
fpname = argv[0];
|
||||
++argv;
|
||||
while ((*argv) && (!wavloaded)) {
|
||||
if ( (strcmp(*argv, "-h") == 0) || (strcmp(*argv, "--help") == 0) ) {
|
||||
fprintf(stderr, "%s [options] audio.wav\n", fpname);
|
||||
fprintf(stderr, " options:\n");
|
||||
fprintf(stderr, " -v, -vv\n");
|
||||
fprintf(stderr, " -r, --raw\n");
|
||||
fprintf(stderr, " -i, --invert\n");
|
||||
fprintf(stderr, " --ecc (Hamming ECC)\n");
|
||||
fprintf(stderr, " --ths <x> (peak threshold; default=%.1f)\n", thres);
|
||||
return 0;
|
||||
}
|
||||
else if ( (strcmp(*argv, "-v") == 0) || (strcmp(*argv, "--verbose") == 0) ) {
|
||||
option_verbose = 1;
|
||||
}
|
||||
else if ( (strcmp(*argv, "-vv") == 0) ) { option_verbose = 2; }
|
||||
else if ( (strcmp(*argv, "-r") == 0) || (strcmp(*argv, "--raw") == 0) ) {
|
||||
option_raw = 1;
|
||||
}
|
||||
else if ( (strcmp(*argv, "-R") == 0) || (strcmp(*argv, "--RAW") == 0) ) {
|
||||
option_raw = 2;
|
||||
}
|
||||
else if ( (strcmp(*argv, "-i") == 0) || (strcmp(*argv, "--invert") == 0) ) {
|
||||
option_inv = 0x1;
|
||||
}
|
||||
else if ( (strcmp(*argv, "--ecc") == 0) ) { option_ecc = 1; }
|
||||
else if ( (strcmp(*argv, "--ptu") == 0) ) { option_ptu = 1; }
|
||||
else if ( (strcmp(*argv, "--ths") == 0) ) {
|
||||
++argv;
|
||||
if (*argv) {
|
||||
thres = atof(*argv);
|
||||
}
|
||||
else return -1;
|
||||
}
|
||||
else {
|
||||
fp = fopen(*argv, "rb");
|
||||
if (fp == NULL) {
|
||||
fprintf(stderr, "%s konnte nicht geoeffnet werden\n", *argv);
|
||||
return -1;
|
||||
}
|
||||
wavloaded = 1;
|
||||
}
|
||||
++argv;
|
||||
}
|
||||
if (!wavloaded) fp = stdin;
|
||||
|
||||
|
||||
spb = read_wav_header(fp, (float)BAUD_RATE);
|
||||
if ( spb < 0 ) {
|
||||
fclose(fp);
|
||||
fprintf(stderr, "error: wav header\n");
|
||||
return -1;
|
||||
}
|
||||
if ( spb < 8 ) {
|
||||
fprintf(stderr, "note: sample rate low\n");
|
||||
}
|
||||
|
||||
|
||||
symlen = 2;
|
||||
headerlen = strlen(rawheader);
|
||||
bitofs = 2; // +1 .. +2
|
||||
if (init_buffers(rawheader, headerlen, 0) < 0) { // shape=0 (alt. shape=1)
|
||||
fprintf(stderr, "error: init buffers\n");
|
||||
return -1;
|
||||
};
|
||||
|
||||
|
||||
mv = -1; mv_pos = 0;
|
||||
|
||||
while ( f32buf_sample(fp, option_inv, 1) != EOF ) {
|
||||
|
||||
mv0_pos = mv_pos;
|
||||
dif = getmaxCorr(&mv, &mv_pos, headerlen+headerlen/2);
|
||||
|
||||
if (mv > thres) {
|
||||
if (mv_pos > mv0_pos) {
|
||||
|
||||
header_found = 0;
|
||||
herrs = headcmp(symlen, rawheader, headerlen, mv_pos); // symlen=2
|
||||
herr1 = 0;
|
||||
if (herrs <= 3 && herrs > 0) {
|
||||
herr1 = headcmp(symlen, rawheader, headerlen, mv_pos+1);
|
||||
if (herr1 < herrs) {
|
||||
herrs = herr1;
|
||||
herr1 = 1;
|
||||
}
|
||||
}
|
||||
if (herrs <= 1) header_found = 1; // herrs <= 1 bitfehler in header
|
||||
|
||||
if (header_found) {
|
||||
|
||||
bitpos = 0;
|
||||
pos = headerlen;
|
||||
pos /= 2;
|
||||
|
||||
frm = 0;
|
||||
while ( frm < nfrms ) { // nfrms=1,2,4,8
|
||||
while ( pos < BITFRAME_LEN ) {
|
||||
header_found = !(frm==nfrms-1 && pos>=BITFRAME_LEN-10);
|
||||
bitQ = read_sbit(fp, symlen, &bit, option_inv, bitofs, bitpos==0, !header_found); // symlen=2, return: zeroX/bit
|
||||
if (bitQ == EOF) { frm = nfrms; break; }
|
||||
frame_bits[pos] = 0x30 + bit;
|
||||
pos++;
|
||||
bitpos += 1;
|
||||
}
|
||||
frame_bits[pos] = '\0';
|
||||
ret = print_frame();
|
||||
if (pos < BITFRAME_LEN) break;
|
||||
pos = 0;
|
||||
frm += 1;
|
||||
//if (ret < 0) frms += 1;
|
||||
}
|
||||
|
||||
header_found = 0;
|
||||
pos = headerlen;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
free_buffers();
|
||||
|
||||
fclose(fp);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
1038
demod/dfm09dm_dft.c
1038
demod/dfm09dm_dft.c
Plik diff jest za duży
Load Diff
1078
demod/lms6dm_dft.c
1078
demod/lms6dm_dft.c
Plik diff jest za duży
Load Diff
1097
demod/lmsXdm_dft.c
1097
demod/lmsXdm_dft.c
Plik diff jest za duży
Load Diff
|
@ -1,979 +0,0 @@
|
|||
|
||||
/*
|
||||
* m10
|
||||
* sync header: correlation/matched filter
|
||||
* files: m10dm_dft.c demod_dft.h demod_dft.c
|
||||
* compile:
|
||||
* gcc -c demod_dft.c
|
||||
* gcc m10dm_dft.c demod_dft.o -lm -o m10dm_dft
|
||||
*
|
||||
* author: zilog80
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#ifdef CYGWIN
|
||||
#include <fcntl.h> // cygwin: _setmode()
|
||||
#include <io.h>
|
||||
#endif
|
||||
|
||||
|
||||
typedef unsigned char ui8_t;
|
||||
typedef unsigned short ui16_t;
|
||||
typedef unsigned int ui32_t;
|
||||
|
||||
//#include "demod_dft.c"
|
||||
#include "demod_dft.h"
|
||||
|
||||
|
||||
typedef struct {
|
||||
int week; int gpssec;
|
||||
int jahr; int monat; int tag;
|
||||
int wday;
|
||||
int std; int min; int sek;
|
||||
double lat; double lon; double alt;
|
||||
double vH; double vD; double vV;
|
||||
double vx; double vy; double vD2;
|
||||
char SN[12];
|
||||
} datum_t;
|
||||
|
||||
datum_t datum;
|
||||
|
||||
int option_verbose = 0, // ausfuehrliche Anzeige
|
||||
option_raw = 0, // rohe Frames
|
||||
option_inv = 0, // invertiert Signal
|
||||
option_res = 0, // genauere Bitmessung
|
||||
option_color = 0,
|
||||
option_ptu = 0,
|
||||
option_dc = 0,
|
||||
wavloaded = 0;
|
||||
int wav_channel = 0; // audio channel: left
|
||||
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
/*
|
||||
* Convert GPS Week and Seconds to Modified Julian Day.
|
||||
* - Adapted from sci.astro FAQ.
|
||||
* - Ignores UTC leap seconds.
|
||||
*/
|
||||
void Gps2Date(long GpsWeek, long GpsSeconds, int *Year, int *Month, int *Day) {
|
||||
|
||||
long GpsDays, Mjd;
|
||||
long J, C, Y, M;
|
||||
|
||||
GpsDays = GpsWeek * 7 + (GpsSeconds / 86400);
|
||||
Mjd = 44244 + GpsDays;
|
||||
|
||||
J = Mjd + 2468570;
|
||||
C = 4 * J / 146097;
|
||||
J = J - (146097 * C + 3) / 4;
|
||||
Y = 4000 * (J + 1) / 1461001;
|
||||
J = J - 1461 * Y / 4 + 31;
|
||||
M = 80 * J / 2447;
|
||||
*Day = J - 2447 * M / 80;
|
||||
J = M / 11;
|
||||
*Month = M + 2 - (12 * J);
|
||||
*Year = 100 * (C - 49) + Y + J;
|
||||
}
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
9600 baud -> 9616 baud ?
|
||||
*/
|
||||
#define BAUD_RATE 9616
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
Header = Sync-Header + Sonde-Header:
|
||||
1100110011001100 1010011001001100 1101010011010011 0100110101010101 0011010011001100
|
||||
uudduudduudduudd ududduuddudduudd uudududduududduu dudduudududududu dduududduudduudd (oder:)
|
||||
dduudduudduudduu duduudduuduudduu ddududuudduduudd uduuddududududud uudduduudduudduu (komplement)
|
||||
0 0 0 0 0 0 0 0 1 1 - - - 0 0 0 0 1 1 0 0 1 0 0 1 0 0 1 1 1 1 1 0 0 1 0 0 0 0 0
|
||||
*/
|
||||
|
||||
#define BITS 8
|
||||
#define HEADLEN 32 // HEADLEN+HEADOFS=32 <= strlen(header)
|
||||
#define HEADOFS 0
|
||||
// Sync-Header (raw) // Sonde-Header (bits)
|
||||
//char head[] = "11001100110011001010011001001100"; //"0110010010011111"; // M10: 64 9F , M2K2: 64 8F
|
||||
//"0111011010011111"; // M10: 76 9F , w/ aux-data
|
||||
//"0110010001001001"; // M10-dop: 64 49 09
|
||||
//"0110010010101111"; // M10+: 64 AF w/ gtop-GPS
|
||||
char rawheader[] = "10011001100110010100110010011001";
|
||||
|
||||
#define FRAME_LEN (100+1) // 0x64+1
|
||||
#define BITFRAME_LEN (FRAME_LEN*BITS)
|
||||
|
||||
#define AUX_LEN 20
|
||||
#define BITAUX_LEN (AUX_LEN*BITS)
|
||||
|
||||
ui8_t frame_bytes[FRAME_LEN+AUX_LEN+4];
|
||||
|
||||
char frame_bits[BITFRAME_LEN+BITAUX_LEN+8];
|
||||
|
||||
int auxlen = 0; // 0 .. 0x76-0x64
|
||||
|
||||
|
||||
int bits2bytes(char *bitstr, ui8_t *bytes) {
|
||||
int i, bit, d, byteval;
|
||||
int bitpos, bytepos;
|
||||
|
||||
bitpos = 0;
|
||||
bytepos = 0;
|
||||
|
||||
while (bytepos < FRAME_LEN+AUX_LEN) {
|
||||
|
||||
byteval = 0;
|
||||
d = 1;
|
||||
for (i = 0; i < BITS; i++) {
|
||||
//bit=*(bitstr+bitpos+i); /* little endian */
|
||||
bit=*(bitstr+bitpos+7-i); /* big endian */
|
||||
// bit == 'x' ?
|
||||
if (bit == '1') byteval += d;
|
||||
else /*if ((bit == '0') || (bit == 'x'))*/ byteval += 0;
|
||||
d <<= 1;
|
||||
}
|
||||
bitpos += BITS;
|
||||
bytes[bytepos++] = byteval & 0xFF;
|
||||
|
||||
}
|
||||
|
||||
//while (bytepos < FRAME_LEN+AUX_LEN) bytes[bytepos++] = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
#define stdFLEN 0x64 // pos[0]=0x64
|
||||
#define pos_GPSTOW 0x0A // 4 byte
|
||||
#define pos_GPSlat 0x0E // 4 byte
|
||||
#define pos_GPSlon 0x12 // 4 byte
|
||||
#define pos_GPSalt 0x16 // 4 byte
|
||||
#define pos_GPSweek 0x20 // 2 byte
|
||||
//Velocity East-North-Up (ENU)
|
||||
#define pos_GPSvE 0x04 // 2 byte
|
||||
#define pos_GPSvN 0x06 // 2 byte
|
||||
#define pos_GPSvU 0x08 // 2 byte
|
||||
#define pos_SN 0x5D // 2+3 byte
|
||||
#define pos_Check (stdFLEN-1) // 2 byte
|
||||
|
||||
|
||||
#define ANSI_COLOR_RED "\x1b[31m"
|
||||
#define ANSI_COLOR_GREEN "\x1b[32m"
|
||||
#define ANSI_COLOR_YELLOW "\x1b[33m"
|
||||
#define ANSI_COLOR_BLUE "\x1b[34m"
|
||||
#define ANSI_COLOR_MAGENTA "\x1b[35m"
|
||||
#define ANSI_COLOR_CYAN "\x1b[36m"
|
||||
#define ANSI_COLOR_RESET "\x1b[0m"
|
||||
|
||||
#define XTERM_COLOR_BROWN "\x1b[38;5;94m" // 38;5;{0..255}m
|
||||
|
||||
#define col_GPSweek "\x1b[38;5;20m" // 2 byte
|
||||
#define col_GPSTOW "\x1b[38;5;27m" // 4 byte
|
||||
#define col_GPSdate "\x1b[38;5;94m" //111
|
||||
#define col_GPSlat "\x1b[38;5;34m" // 4 byte
|
||||
#define col_GPSlon "\x1b[38;5;70m" // 4 byte
|
||||
#define col_GPSalt "\x1b[38;5;82m" // 4 byte
|
||||
#define col_GPSvel "\x1b[38;5;36m" // 6 byte
|
||||
#define col_SN "\x1b[38;5;58m" // 3 byte
|
||||
#define col_Check "\x1b[38;5;11m" // 2 byte
|
||||
#define col_TXT "\x1b[38;5;244m"
|
||||
#define col_FRTXT "\x1b[38;5;244m"
|
||||
#define col_CSok "\x1b[38;5;2m"
|
||||
#define col_CSno "\x1b[38;5;1m"
|
||||
|
||||
/*
|
||||
$ for code in {0..255}
|
||||
> do echo -e "\e[38;5;${code}m"'\\e[38;5;'"$code"m"\e[0m"
|
||||
> done
|
||||
*/
|
||||
|
||||
int get_GPSweek() {
|
||||
int i;
|
||||
unsigned byte;
|
||||
ui8_t gpsweek_bytes[2];
|
||||
int gpsweek;
|
||||
|
||||
for (i = 0; i < 2; i++) {
|
||||
byte = frame_bytes[pos_GPSweek + i];
|
||||
gpsweek_bytes[i] = byte;
|
||||
}
|
||||
|
||||
gpsweek = (gpsweek_bytes[0] << 8) + gpsweek_bytes[1];
|
||||
datum.week = gpsweek;
|
||||
|
||||
if (gpsweek < 0 || gpsweek > 3000) return -1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
char weekday[7][3] = { "So", "Mo", "Di", "Mi", "Do", "Fr", "Sa"};
|
||||
|
||||
int get_GPStime() {
|
||||
int i;
|
||||
unsigned byte;
|
||||
ui8_t gpstime_bytes[4];
|
||||
int gpstime, day; // int ms;
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
byte = frame_bytes[pos_GPSTOW + i];
|
||||
gpstime_bytes[i] = byte;
|
||||
}
|
||||
|
||||
gpstime = 0;
|
||||
for (i = 0; i < 4; i++) {
|
||||
gpstime |= gpstime_bytes[i] << (8*(3-i));
|
||||
}
|
||||
|
||||
//ms = gpstime % 1000;
|
||||
gpstime /= 1000;
|
||||
datum.gpssec = gpstime;
|
||||
|
||||
day = gpstime / (24 * 3600);
|
||||
gpstime %= (24*3600);
|
||||
|
||||
if ((day < 0) || (day > 6)) return -1;
|
||||
datum.wday = day;
|
||||
datum.std = gpstime/3600;
|
||||
datum.min = (gpstime%3600)/60;
|
||||
datum.sek = gpstime%60;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
double B60B60 = 0xB60B60; // 2^32/360 = 0xB60B60.xxx
|
||||
|
||||
int get_GPSlat() {
|
||||
int i;
|
||||
unsigned byte;
|
||||
ui8_t gpslat_bytes[4];
|
||||
int gpslat;
|
||||
double lat;
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
byte = frame_bytes[pos_GPSlat + i];
|
||||
gpslat_bytes[i] = byte;
|
||||
}
|
||||
|
||||
gpslat = 0;
|
||||
for (i = 0; i < 4; i++) {
|
||||
gpslat |= gpslat_bytes[i] << (8*(3-i));
|
||||
}
|
||||
lat = gpslat / B60B60;
|
||||
datum.lat = lat;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int get_GPSlon() {
|
||||
int i;
|
||||
unsigned byte;
|
||||
ui8_t gpslon_bytes[4];
|
||||
int gpslon;
|
||||
double lon;
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
byte = frame_bytes[pos_GPSlon + i];
|
||||
gpslon_bytes[i] = byte;
|
||||
}
|
||||
|
||||
gpslon = 0;
|
||||
for (i = 0; i < 4; i++) {
|
||||
gpslon |= gpslon_bytes[i] << (8*(3-i));
|
||||
}
|
||||
lon = gpslon / B60B60;
|
||||
datum.lon = lon;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int get_GPSalt() {
|
||||
int i;
|
||||
unsigned byte;
|
||||
ui8_t gpsalt_bytes[4];
|
||||
int gpsalt;
|
||||
double alt;
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
byte = frame_bytes[pos_GPSalt + i];
|
||||
gpsalt_bytes[i] = byte;
|
||||
}
|
||||
|
||||
gpsalt = 0;
|
||||
for (i = 0; i < 4; i++) {
|
||||
gpsalt |= gpsalt_bytes[i] << (8*(3-i));
|
||||
}
|
||||
alt = gpsalt / 1000.0;
|
||||
datum.alt = alt;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int get_GPSvel() {
|
||||
int i;
|
||||
unsigned byte;
|
||||
ui8_t gpsVel_bytes[2];
|
||||
short vel16;
|
||||
double vx, vy, dir, alpha;
|
||||
const double ms2kn100 = 2e2; // m/s -> knots: 1 m/s = 3.6/1.852 kn = 1.94 kn
|
||||
|
||||
for (i = 0; i < 2; i++) {
|
||||
byte = frame_bytes[pos_GPSvE + i];
|
||||
gpsVel_bytes[i] = byte;
|
||||
}
|
||||
vel16 = gpsVel_bytes[0] << 8 | gpsVel_bytes[1];
|
||||
vx = vel16 / ms2kn100; // ost
|
||||
|
||||
for (i = 0; i < 2; i++) {
|
||||
byte = frame_bytes[pos_GPSvN + i];
|
||||
gpsVel_bytes[i] = byte;
|
||||
}
|
||||
vel16 = gpsVel_bytes[0] << 8 | gpsVel_bytes[1];
|
||||
vy= vel16 / ms2kn100; // nord
|
||||
|
||||
datum.vx = vx;
|
||||
datum.vy = vy;
|
||||
datum.vH = sqrt(vx*vx+vy*vy);
|
||||
///*
|
||||
alpha = atan2(vy, vx)*180/M_PI; // ComplexPlane (von x-Achse nach links) - GeoMeteo (von y-Achse nach rechts)
|
||||
dir = 90-alpha; // z=x+iy= -> i*conj(z)=y+ix=re(i(pi/2-t)), Achsen und Drehsinn vertauscht
|
||||
if (dir < 0) dir += 360; // atan2(y,x)=atan(y/x)=pi/2-atan(x/y) , atan(1/t) = pi/2 - atan(t)
|
||||
datum.vD2 = dir;
|
||||
//*/
|
||||
dir = atan2(vx, vy) * 180 / M_PI;
|
||||
if (dir < 0) dir += 360;
|
||||
datum.vD = dir;
|
||||
|
||||
for (i = 0; i < 2; i++) {
|
||||
byte = frame_bytes[pos_GPSvU + i];
|
||||
gpsVel_bytes[i] = byte;
|
||||
}
|
||||
vel16 = gpsVel_bytes[0] << 8 | gpsVel_bytes[1];
|
||||
datum.vV = vel16 / ms2kn100;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int get_SN() {
|
||||
int i;
|
||||
unsigned byte;
|
||||
ui8_t sn_bytes[5];
|
||||
|
||||
for (i = 0; i < 11; i++) datum.SN[i] = ' '; datum.SN[11] = '\0';
|
||||
|
||||
for (i = 0; i < 5; i++) {
|
||||
byte = frame_bytes[pos_SN + i];
|
||||
sn_bytes[i] = byte;
|
||||
}
|
||||
|
||||
byte = sn_bytes[2];
|
||||
sprintf(datum.SN, "%1X%02u", (byte>>4)&0xF, byte&0xF);
|
||||
byte = sn_bytes[3] | (sn_bytes[4]<<8);
|
||||
sprintf(datum.SN+3, " %1X %1u%04u", sn_bytes[0]&0xF, (byte>>13)&0x7, byte&0x1FFF);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
/*
|
||||
g : F^n -> F^16 // checksum, linear
|
||||
g(m||b) = f(g(m),b)
|
||||
|
||||
// update checksum
|
||||
f : F^16 x F^8 -> F^16 linear
|
||||
|
||||
010100001000000101000000
|
||||
001010000100000010100000
|
||||
000101000010000001010000
|
||||
000010100001000000101000
|
||||
000001010000100000010100
|
||||
100000100000010000001010
|
||||
000000011010100000000100
|
||||
100000000101010000000010
|
||||
000000001000000000000000
|
||||
000000000100000000000000
|
||||
000000000010000000000000
|
||||
000000000001000000000000
|
||||
000000000000100000000000
|
||||
000000000000010000000000
|
||||
000000000000001000000000
|
||||
000000000000000100000000
|
||||
*/
|
||||
|
||||
int update_checkM10(int c, ui8_t b) {
|
||||
int c0, c1, t, t6, t7, s;
|
||||
|
||||
c1 = c & 0xFF;
|
||||
|
||||
// B
|
||||
b = (b >> 1) | ((b & 1) << 7);
|
||||
b ^= (b >> 2) & 0xFF;
|
||||
|
||||
// A1
|
||||
t6 = ( c & 1) ^ ((c>>2) & 1) ^ ((c>>4) & 1);
|
||||
t7 = ((c>>1) & 1) ^ ((c>>3) & 1) ^ ((c>>5) & 1);
|
||||
t = (c & 0x3F) | (t6 << 6) | (t7 << 7);
|
||||
|
||||
// A2
|
||||
s = (c >> 7) & 0xFF;
|
||||
s ^= (s >> 2) & 0xFF;
|
||||
|
||||
|
||||
c0 = b ^ t ^ s;
|
||||
|
||||
return ((c1<<8) | c0) & 0xFFFF;
|
||||
}
|
||||
|
||||
int checkM10(ui8_t *msg, int len) {
|
||||
int i, cs;
|
||||
|
||||
cs = 0;
|
||||
for (i = 0; i < len; i++) {
|
||||
cs = update_checkM10(cs, msg[i]);
|
||||
}
|
||||
|
||||
return cs & 0xFFFF;
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
// Temperatur Sensor
|
||||
// NTC-Thermistor Shibaura PB5-41E
|
||||
//
|
||||
float get_Temp(int csOK) {
|
||||
// NTC-Thermistor Shibaura PB5-41E
|
||||
// T00 = 273.15 + 0.0 , R00 = 15e3
|
||||
// T25 = 273.15 + 25.0 , R25 = 5.369e3
|
||||
// B00 = 3450.0 Kelvin // 0C..100C, poor fit low temps
|
||||
// [ T/C , R/1e3 ] ( [P__-43]/2.0 ):
|
||||
// [ -50.0 , 204.0 ]
|
||||
// [ -45.0 , 150.7 ]
|
||||
// [ -40.0 , 112.6 ]
|
||||
// [ -35.0 , 84.90 ]
|
||||
// [ -30.0 , 64.65 ]
|
||||
// [ -25.0 , 49.66 ]
|
||||
// [ -20.0 , 38.48 ]
|
||||
// [ -15.0 , 30.06 ]
|
||||
// [ -10.0 , 23.67 ]
|
||||
// [ -5.0 , 18.78 ]
|
||||
// [ 0.0 , 15.00 ]
|
||||
// [ 5.0 , 12.06 ]
|
||||
// [ 10.0 , 9.765 ]
|
||||
// [ 15.0 , 7.955 ]
|
||||
// [ 20.0 , 6.515 ]
|
||||
// [ 25.0 , 5.370 ]
|
||||
// [ 30.0 , 4.448 ]
|
||||
// [ 35.0 , 3.704 ]
|
||||
// [ 40.0 , 3.100 ]
|
||||
// -> Steinhart–Hart coefficients (polyfit):
|
||||
float p0 = 1.07303516e-03,
|
||||
p1 = 2.41296733e-04,
|
||||
p2 = 2.26744154e-06,
|
||||
p3 = 6.52855181e-08;
|
||||
// T/K = 1/( p0 + p1*ln(R) + p2*ln(R)^2 + p3*ln(R)^3 )
|
||||
|
||||
// range/scale 0, 1, 2: // M10-pcb
|
||||
float Rs[3] = { 12.1e3 , 36.5e3 , 475.0e3 }; // bias/series
|
||||
float Rp[3] = { 1e20 , 330.0e3 , 3000.0e3 }; // parallel, Rp[0]=inf
|
||||
|
||||
ui8_t scT; // {0,1,2}, range/scale voltage divider
|
||||
ui16_t ADC_RT; // ADC12 P6.7(A7) , adr_0377h,adr_0376h
|
||||
ui16_t Tcal[2]; // adr_1000h[scT*4]
|
||||
|
||||
float adc_max = 4095.0; // ADC12
|
||||
float x, R;
|
||||
float T = 0; // T/Kelvin
|
||||
|
||||
scT = frame_bytes[0x3E]; // adr_0455h
|
||||
ADC_RT = (frame_bytes[0x40] << 8) | frame_bytes[0x3F];
|
||||
ADC_RT -= 0xA000;
|
||||
Tcal[0] = (frame_bytes[0x42] << 8) | frame_bytes[0x41];
|
||||
Tcal[1] = (frame_bytes[0x44] << 8) | frame_bytes[0x43];
|
||||
|
||||
x = (adc_max-ADC_RT)/ADC_RT; // (Vcc-Vout)/Vout
|
||||
if (scT < 3) R = Rs[scT] /( x - Rs[scT]/Rp[scT] );
|
||||
else R = -1;
|
||||
|
||||
if (R > 0) T = 1/( p0 + p1*log(R) + p2*log(R)*log(R) + p3*log(R)*log(R)*log(R) );
|
||||
|
||||
if (option_verbose >= 3 && csOK) { // on-chip temperature
|
||||
ui16_t ADC_Ti_raw = (frame_bytes[0x49] << 8) | frame_bytes[0x48]; // int.temp.diode, ref: 4095->1.5V
|
||||
float vti, ti;
|
||||
// INCH1A (temp.diode), slau144
|
||||
vti = ADC_Ti_raw/4095.0 * 1.5; // V_REF+ = 1.5V, no calibration
|
||||
ti = (vti-0.986)/0.00355; // 0.986/0.00355=277.75, 1.5/4095/0.00355=0.1032
|
||||
fprintf(stdout, " (Ti:%.1fC)", ti);
|
||||
// SegmentA-Calibration:
|
||||
//ui16_t T30 = adr_10e2h; // CAL_ADC_15T30
|
||||
//ui16_t T85 = adr_10e4h; // CAL_ADC_15T85
|
||||
//float tic = (ADC_Ti_raw-T30)*(85.0-30.0)/(T85-T30) + 30.0;
|
||||
//fprintf(stdout, " (Tic:%.1fC)", tic);
|
||||
}
|
||||
|
||||
return T - 273.15; // Celsius
|
||||
}
|
||||
/*
|
||||
frame[0x32]: adr_1074h
|
||||
frame[0x33]: adr_1075h
|
||||
frame[0x34]: adr_1076h
|
||||
|
||||
frame[0x35..0x37]: TBCCR1 ; relHumCap-freq
|
||||
|
||||
frame[0x38]: adr_1078h
|
||||
frame[0x39]: adr_1079h
|
||||
frame[0x3A]: adr_1077h
|
||||
frame[0x3B]: adr_100Ch
|
||||
frame[0x3C..3D]: 0
|
||||
|
||||
|
||||
frame[0x3E]: scale_index ; scale/range-index
|
||||
frame[0x3F..40] = ADC12_A7 | 0xA000, V_R+=AVcc ; Thermistor
|
||||
|
||||
frame[0x41]: adr_1000h[scale_index*4]
|
||||
frame[0x42]: adr_1000h[scale_index*4+1]
|
||||
frame[0x43]: adr_1000h[scale_index*4+2]
|
||||
frame[0x44]: adr_1000h[scale_index*4+3]
|
||||
|
||||
frame[0x45..46]: ADC12_A5/4, V_R+=2.5V
|
||||
frame[0x47]: ADC12_A2/16 , V_R+=2.5V
|
||||
frame[0x48..49]: ADC12_iT, V_R+=1.5V (int.Temp.diode)
|
||||
frame[0x4C..4D]: ADC12_A6, V_R+=2.5V
|
||||
frame[0x4E..4F]: ADC12_A3, V_R+=AVcc
|
||||
frame[0x50..54]: 0;
|
||||
frame[0x55..56]: ADC12_A1, V_R+=AVcc
|
||||
frame[0x57..58]: ADC12_A0, V_R+=AVcc
|
||||
frame[0x59..5A]: ADC12_A4, V_R+=AVcc // ntc2: R(25C)=2.2k, Rs=22.1e3 (relHumCap-Temp)
|
||||
|
||||
frame[0x5B]:
|
||||
frame[0x5C]: adr_108Eh
|
||||
|
||||
|
||||
frame[0x5D]: adr_1082h (SN)
|
||||
frame[0x5E]: adr_1083h (SN)
|
||||
frame[0x5F]: adr_1084h (SN)
|
||||
frame[0x60]: adr_1080h (SN)
|
||||
frame[0x61]: adr_1081h (SN)
|
||||
*/
|
||||
float get_Tntc2(int csOK) {
|
||||
// SMD ntc
|
||||
float Rs = 22.1e3; // P5.6=Vcc
|
||||
// float R25 = 2.2e3;
|
||||
// float b = 3650.0; // B/Kelvin
|
||||
// float T25 = 25.0 + 273.15; // T0=25C, R0=R25=5k
|
||||
// -> Steinhart–Hart coefficients (polyfit):
|
||||
float p0 = 4.42606809e-03,
|
||||
p1 = -6.58184309e-04,
|
||||
p2 = 8.95735557e-05,
|
||||
p3 = -2.84347503e-06;
|
||||
float T = 0.0; // T/Kelvin
|
||||
ui16_t ADC_ntc2; // ADC12 P6.4(A4)
|
||||
float x, R;
|
||||
if (csOK)
|
||||
{
|
||||
ADC_ntc2 = (frame_bytes[0x5A] << 8) | frame_bytes[0x59];
|
||||
x = (4095.0 - ADC_ntc2)/ADC_ntc2; // (Vcc-Vout)/Vout
|
||||
R = Rs / x;
|
||||
//if (R > 0) T = 1/(1/T25 + 1/b * log(R/R25));
|
||||
if (R > 0) T = 1/( p0 + p1*log(R) + p2*log(R)*log(R) + p3*log(R)*log(R)*log(R) );
|
||||
}
|
||||
return T - 273.15;
|
||||
}
|
||||
|
||||
// Humidity Sensor
|
||||
// U.P.S.I.
|
||||
//
|
||||
#define FREQ_CAPCLK (8e6/2) // 8 MHz XT2 crystal, InputDivider IDx=01 (/2)
|
||||
#define LN2 0.693147181
|
||||
#define ADR_108A 1000.0 // 0x3E8=1000
|
||||
|
||||
float get_count_RH() { // capture 1000 rising edges
|
||||
ui32_t TBCCR1_1000 = frame_bytes[0x35] | (frame_bytes[0x36]<<8) | (frame_bytes[0x37]<<16);
|
||||
return TBCCR1_1000 / ADR_108A;
|
||||
}
|
||||
float get_TLC555freq() {
|
||||
return FREQ_CAPCLK / get_count_RH();
|
||||
}
|
||||
/*
|
||||
double get_C_RH() { // TLC555 astable: R_A=3.65k, R_B=338k
|
||||
double R_B = 338e3;
|
||||
double R_A = 3.65e3;
|
||||
double C_RH = 1/get_TLC555freq() / (LN2 * (R_A + 2*R_B));
|
||||
return C_RH;
|
||||
}
|
||||
double get_RH(int csOK) {
|
||||
// U.P.S.I.
|
||||
// C_RH/C_55 = 0.8955 + 0.002*RH , T=20C
|
||||
// C_RH = C_RH(RH,T) , RH = RH(C_RH,T)
|
||||
// C_RH/C_55 approx.eq. count_RH/count_ref
|
||||
// c55=270pF? diff=C_55-c55, T=20C
|
||||
ui32_t c = frame_bytes[0x32] | (frame_bytes[0x33]<<8) | (frame_bytes[0x34]<<16); // CalRef 55%RH , T=20C ?
|
||||
double count_ref = c / ADR_108A; // CalRef 55%RH , T=20C ?
|
||||
double C_RH = get_C_RH();
|
||||
double T = get_Tntc2(csOK);
|
||||
return 0;
|
||||
}
|
||||
*/
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
int print_pos(int csOK) {
|
||||
int err;
|
||||
|
||||
err = 0;
|
||||
err |= get_GPSweek();
|
||||
err |= get_GPStime();
|
||||
err |= get_GPSlat();
|
||||
err |= get_GPSlon();
|
||||
err |= get_GPSalt();
|
||||
|
||||
if (!err) {
|
||||
|
||||
Gps2Date(datum.week, datum.gpssec, &datum.jahr, &datum.monat, &datum.tag);
|
||||
|
||||
if (option_color) {
|
||||
fprintf(stdout, col_TXT);
|
||||
fprintf(stdout, " (W "col_GPSweek"%d"col_TXT") ", datum.week);
|
||||
fprintf(stdout, col_GPSTOW"%s"col_TXT" ", weekday[datum.wday]);
|
||||
fprintf(stdout, col_GPSdate"%04d-%02d-%02d"col_TXT" ("col_GPSTOW"%02d:%02d:%02d"col_TXT") ",
|
||||
datum.jahr, datum.monat, datum.tag, datum.std, datum.min, datum.sek);
|
||||
fprintf(stdout, " lat: "col_GPSlat"%.6f"col_TXT" ", datum.lat);
|
||||
fprintf(stdout, " lon: "col_GPSlon"%.6f"col_TXT" ", datum.lon);
|
||||
fprintf(stdout, " alt: "col_GPSalt"%.2f"col_TXT" ", datum.alt);
|
||||
if (option_verbose) {
|
||||
err |= get_GPSvel();
|
||||
if (!err) {
|
||||
//if (option_verbose == 2) fprintf(stdout, " "col_GPSvel"(%.1f , %.1f : %.1f)"col_TXT" ", datum.vx, datum.vy, datum.vD2);
|
||||
fprintf(stdout, " vH: "col_GPSvel"%.1f"col_TXT" D: "col_GPSvel"%.1f"col_TXT"° vV: "col_GPSvel"%.1f"col_TXT" ", datum.vH, datum.vD, datum.vV);
|
||||
}
|
||||
if (option_verbose >= 2) {
|
||||
get_SN();
|
||||
fprintf(stdout, " SN: "col_SN"%s"col_TXT, datum.SN);
|
||||
}
|
||||
if (option_verbose >= 2) {
|
||||
fprintf(stdout, " # ");
|
||||
if (csOK) fprintf(stdout, " "col_CSok"[OK]"col_TXT);
|
||||
else fprintf(stdout, " "col_CSno"[NO]"col_TXT);
|
||||
}
|
||||
}
|
||||
if (option_ptu) {
|
||||
float t = get_Temp(csOK);
|
||||
if (t > -270.0) fprintf(stdout, " T=%.1fC ", t);
|
||||
if (option_verbose >= 3) {
|
||||
float t2 = get_Tntc2(csOK);
|
||||
float fq555 = get_TLC555freq();
|
||||
if (t2 > -270.0) fprintf(stdout, " (T2:%.1fC) (%.3fkHz) ", t2, fq555/1e3);
|
||||
}
|
||||
}
|
||||
fprintf(stdout, ANSI_COLOR_RESET"");
|
||||
}
|
||||
else {
|
||||
fprintf(stdout, " (W %d) ", datum.week);
|
||||
fprintf(stdout, "%s ", weekday[datum.wday]);
|
||||
fprintf(stdout, "%04d-%02d-%02d (%02d:%02d:%02d) ",
|
||||
datum.jahr, datum.monat, datum.tag, datum.std, datum.min, datum.sek);
|
||||
fprintf(stdout, " lat: %.6f ", datum.lat);
|
||||
fprintf(stdout, " lon: %.6f ", datum.lon);
|
||||
fprintf(stdout, " alt: %.2f ", datum.alt);
|
||||
if (option_verbose) {
|
||||
err |= get_GPSvel();
|
||||
if (!err) {
|
||||
//if (option_verbose == 2) fprintf(stdout, " (%.1f , %.1f : %.1f°) ", datum.vx, datum.vy, datum.vD2);
|
||||
fprintf(stdout, " vH: %.1f D: %.1f° vV: %.1f ", datum.vH, datum.vD, datum.vV);
|
||||
}
|
||||
if (option_verbose >= 2) {
|
||||
get_SN();
|
||||
fprintf(stdout, " SN: %s", datum.SN);
|
||||
}
|
||||
if (option_verbose >= 2) {
|
||||
fprintf(stdout, " # ");
|
||||
if (csOK) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]");
|
||||
}
|
||||
}
|
||||
if (option_ptu) {
|
||||
float t = get_Temp(csOK);
|
||||
if (t > -270.0) fprintf(stdout, " T=%.1fC ", t);
|
||||
if (option_verbose >= 3) {
|
||||
float t2 = get_Tntc2(csOK);
|
||||
float fq555 = get_TLC555freq();
|
||||
if (t2 > -270.0) fprintf(stdout, " (T2:%.1fC) (%.3fkHz) ", t2, fq555/1e3);
|
||||
}
|
||||
}
|
||||
}
|
||||
fprintf(stdout, "\n");
|
||||
|
||||
}
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
int print_frame(int pos) {
|
||||
int i;
|
||||
ui8_t byte;
|
||||
int cs1, cs2;
|
||||
int flen = stdFLEN; // stdFLEN=0x64, auxFLEN=0x76
|
||||
|
||||
bits2bytes(frame_bits, frame_bytes);
|
||||
flen = frame_bytes[0];
|
||||
if (flen == stdFLEN) auxlen = 0;
|
||||
else {
|
||||
auxlen = flen - stdFLEN;
|
||||
if (auxlen < 0 || auxlen > AUX_LEN) auxlen = 0;
|
||||
}
|
||||
|
||||
cs1 = (frame_bytes[pos_Check+auxlen] << 8) | frame_bytes[pos_Check+auxlen+1];
|
||||
cs2 = checkM10(frame_bytes, pos_Check+auxlen);
|
||||
|
||||
if (option_raw) {
|
||||
|
||||
if (option_color && frame_bytes[1] != 0x49) {
|
||||
fprintf(stdout, col_FRTXT);
|
||||
for (i = 0; i < FRAME_LEN+auxlen; i++) {
|
||||
byte = frame_bytes[i];
|
||||
if ((i >= pos_GPSTOW) && (i < pos_GPSTOW+4)) fprintf(stdout, col_GPSTOW);
|
||||
if ((i >= pos_GPSlat) && (i < pos_GPSlat+4)) fprintf(stdout, col_GPSlat);
|
||||
if ((i >= pos_GPSlon) && (i < pos_GPSlon+4)) fprintf(stdout, col_GPSlon);
|
||||
if ((i >= pos_GPSalt) && (i < pos_GPSalt+4)) fprintf(stdout, col_GPSalt);
|
||||
if ((i >= pos_GPSweek) && (i < pos_GPSweek+2)) fprintf(stdout, col_GPSweek);
|
||||
if ((i >= pos_GPSvE) && (i < pos_GPSvE+6)) fprintf(stdout, col_GPSvel);
|
||||
if ((i >= pos_SN) && (i < pos_SN+5)) fprintf(stdout, col_SN);
|
||||
if ((i >= pos_Check+auxlen) && (i < pos_Check+auxlen+2)) fprintf(stdout, col_Check);
|
||||
fprintf(stdout, "%02x", byte);
|
||||
fprintf(stdout, col_FRTXT);
|
||||
}
|
||||
if (option_verbose) {
|
||||
fprintf(stdout, " # "col_Check"%04x"col_FRTXT, cs2);
|
||||
if (cs1 == cs2) fprintf(stdout, " "col_CSok"[OK]"col_TXT);
|
||||
else fprintf(stdout, " "col_CSno"[NO]"col_TXT);
|
||||
}
|
||||
fprintf(stdout, ANSI_COLOR_RESET"\n");
|
||||
}
|
||||
else {
|
||||
for (i = 0; i < FRAME_LEN+auxlen; i++) {
|
||||
byte = frame_bytes[i];
|
||||
fprintf(stdout, "%02x", byte);
|
||||
}
|
||||
if (option_verbose) {
|
||||
fprintf(stdout, " # %04x", cs2);
|
||||
if (cs1 == cs2) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]");
|
||||
}
|
||||
fprintf(stdout, "\n");
|
||||
}
|
||||
|
||||
}
|
||||
else if (frame_bytes[1] == 0x49) {
|
||||
if (option_verbose == 3) {
|
||||
for (i = 0; i < FRAME_LEN+auxlen; i++) {
|
||||
byte = frame_bytes[i];
|
||||
fprintf(stdout, "%02x", byte);
|
||||
}
|
||||
fprintf(stdout, "\n");
|
||||
}
|
||||
}
|
||||
else print_pos(cs1 == cs2);
|
||||
|
||||
return (frame_bytes[0]<<8)|frame_bytes[1];
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
|
||||
int spike = 0;
|
||||
|
||||
FILE *fp = NULL;
|
||||
char *fpname = NULL;
|
||||
float spb = 0.0;
|
||||
int header_found = 0;
|
||||
|
||||
int bit, bit0;
|
||||
int bitpos = 0;
|
||||
int bitQ;
|
||||
int pos;
|
||||
int herrs, herr1;
|
||||
int headerlen = 0;
|
||||
|
||||
int k, K;
|
||||
float mv;
|
||||
unsigned int mv_pos, mv0_pos;
|
||||
int mp = 0;
|
||||
|
||||
float thres = 0.76;
|
||||
|
||||
int symlen = 2;
|
||||
int bitofs = 0; // 0 .. +2
|
||||
int shift = 0;
|
||||
|
||||
|
||||
#ifdef CYGWIN
|
||||
_setmode(fileno(stdin), _O_BINARY); // _setmode(_fileno(stdin), _O_BINARY);
|
||||
#endif
|
||||
setbuf(stdout, NULL);
|
||||
|
||||
fpname = argv[0];
|
||||
++argv;
|
||||
while ((*argv) && (!wavloaded)) {
|
||||
if ( (strcmp(*argv, "-h") == 0) || (strcmp(*argv, "--help") == 0) ) {
|
||||
fprintf(stderr, "%s [options] audio.wav\n", fpname);
|
||||
fprintf(stderr, " options:\n");
|
||||
//fprintf(stderr, " -v, --verbose\n");
|
||||
fprintf(stderr, " -r, --raw\n");
|
||||
fprintf(stderr, " -c, --color\n");
|
||||
return 0;
|
||||
}
|
||||
else if ( (strcmp(*argv, "-v") == 0) || (strcmp(*argv, "--verbose") == 0) ) {
|
||||
option_verbose = 1;
|
||||
}
|
||||
else if ( (strcmp(*argv, "-vv" ) == 0) ) option_verbose = 2;
|
||||
else if ( (strcmp(*argv, "-vvv") == 0) ) option_verbose = 3;
|
||||
else if ( (strcmp(*argv, "-r") == 0) || (strcmp(*argv, "--raw") == 0) ) {
|
||||
option_raw = 1;
|
||||
}
|
||||
else if ( (strcmp(*argv, "-i") == 0) || (strcmp(*argv, "--invert") == 0) ) {
|
||||
option_inv = 1; // nicht noetig
|
||||
}
|
||||
else if ( (strcmp(*argv, "-c") == 0) || (strcmp(*argv, "--color") == 0) ) {
|
||||
option_color = 1;
|
||||
}
|
||||
else if (strcmp(*argv, "--res") == 0) { option_res = 1; }
|
||||
else if ( (strcmp(*argv, "--ptu") == 0) ) {
|
||||
option_ptu = 1;
|
||||
}
|
||||
else if ( (strcmp(*argv, "--dc") == 0) ) {
|
||||
option_dc = 1;
|
||||
}
|
||||
else if ( (strcmp(*argv, "--spike") == 0) ) {
|
||||
spike = 1;
|
||||
}
|
||||
else if ( (strcmp(*argv, "--ch2") == 0) ) { wav_channel = 1; } // right channel (default: 0=left)
|
||||
else if ( (strcmp(*argv, "--ths") == 0) ) {
|
||||
++argv;
|
||||
if (*argv) {
|
||||
thres = atof(*argv);
|
||||
}
|
||||
else return -1;
|
||||
}
|
||||
else if ( (strcmp(*argv, "-d") == 0) ) {
|
||||
++argv;
|
||||
if (*argv) {
|
||||
shift = atoi(*argv);
|
||||
if (shift > 4) shift = 4;
|
||||
if (shift < -4) shift = -4;
|
||||
}
|
||||
else return -1;
|
||||
}
|
||||
else {
|
||||
fp = fopen(*argv, "rb");
|
||||
if (fp == NULL) {
|
||||
fprintf(stderr, "%s konnte nicht geoeffnet werden\n", *argv);
|
||||
return -1;
|
||||
}
|
||||
wavloaded = 1;
|
||||
}
|
||||
++argv;
|
||||
}
|
||||
if (!wavloaded) fp = stdin;
|
||||
|
||||
|
||||
spb = read_wav_header(fp, (float)BAUD_RATE, wav_channel);
|
||||
if ( spb < 0 ) {
|
||||
fclose(fp);
|
||||
fprintf(stderr, "error: wav header\n");
|
||||
return -1;
|
||||
}
|
||||
if ( spb < 8 ) {
|
||||
fprintf(stderr, "note: sample rate low\n");
|
||||
}
|
||||
|
||||
|
||||
symlen = 2;
|
||||
bitofs += shift;
|
||||
|
||||
headerlen = strlen(rawheader);
|
||||
K = init_buffers(rawheader, headerlen, 1); // shape=0 (alt. shape=1)
|
||||
if ( K < 0 ) {
|
||||
fprintf(stderr, "error: init buffers\n");
|
||||
return -1;
|
||||
};
|
||||
|
||||
|
||||
k = 0;
|
||||
mv = 0;
|
||||
mv_pos = 0;
|
||||
|
||||
while ( f32buf_sample(fp, option_inv) != EOF ) {
|
||||
|
||||
k += 1;
|
||||
if (k >= K-4) {
|
||||
mv0_pos = mv_pos;
|
||||
mp = getCorrDFT(K, 0, &mv, &mv_pos);
|
||||
k = 0;
|
||||
}
|
||||
else {
|
||||
mv = 0.0;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (mp > 0 && (mv > thres || mv < -thres)) {
|
||||
if (mv_pos > mv0_pos) {
|
||||
|
||||
header_found = 0;
|
||||
herrs = headcmp(1, rawheader, headerlen, mv_pos, mv<0, option_dc); // header nicht manchester!
|
||||
herr1 = 0;
|
||||
if (herrs <= 3 && herrs > 0) {
|
||||
herr1 = headcmp(1, rawheader, headerlen, mv_pos+1, mv<0, 0); // nur 1x dc
|
||||
//int herr2 = headcmp(1, rawheader, headerlen, mv_pos-1, mv<0, 0);
|
||||
if (herr1 < herrs) {
|
||||
herrs = herr1;
|
||||
herr1 = 1;
|
||||
}
|
||||
}
|
||||
if (herrs <= 1) header_found = 1; // herrs <= 1 bitfehler in header
|
||||
|
||||
if (header_found) {
|
||||
|
||||
bitpos = 0;
|
||||
pos = 0;
|
||||
pos /= 2;
|
||||
bit0 = '0'; // oder: mv[j] > 0
|
||||
|
||||
while ( pos < BITFRAME_LEN+BITAUX_LEN ) {
|
||||
bitQ = read_spkbit(fp, symlen, &bit, option_inv, bitofs, bitpos==0, spike); // symlen=2
|
||||
if (bitQ == EOF) { break; }
|
||||
frame_bits[pos] = 0x31 ^ (bit0 ^ bit);
|
||||
pos++;
|
||||
bit0 = bit;
|
||||
bitpos += 1;
|
||||
}
|
||||
frame_bits[pos] = '\0';
|
||||
print_frame(pos);
|
||||
if (pos < BITFRAME_LEN) break;
|
||||
|
||||
header_found = 0;
|
||||
|
||||
// bis Ende der Sekunde vorspulen; allerdings Doppel-Frame alle 10 sek
|
||||
if (option_verbose < 3) { // && (regulare frame) // print_frame-return?
|
||||
while ( bitpos < 5*BITFRAME_LEN ) {
|
||||
bitQ = read_spkbit(fp, symlen, &bit, option_inv, bitofs, bitpos==0, spike); // symlen=2
|
||||
if ( bitQ == EOF) break;
|
||||
bitpos++;
|
||||
}
|
||||
}
|
||||
|
||||
pos = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
free_buffers();
|
||||
|
||||
fclose(fp);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -257,11 +257,13 @@ float read_wav_header(pcm_t *pcm, FILE *fp) {
|
|||
int sample_rate = 0, bits_sample = 0, channels = 0;
|
||||
|
||||
if (fread(txt, 1, 4, fp) < 4) return -1;
|
||||
if (strncmp(txt, "RIFF", 4)) return -1;
|
||||
if (strncmp(txt, "RIFF", 4) && strncmp(txt, "RF64", 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;
|
||||
if (strncmp(txt, "WAVE", 4)) return -1;
|
||||
|
||||
// pos_fmt = 12L
|
||||
for ( ; ; ) {
|
||||
if ( (byte=fgetc(fp)) == EOF ) return -1;
|
||||
|
|
1245
demod/rs41dm.c
1245
demod/rs41dm.c
Plik diff jest za duży
Load Diff
1489
demod/rs41dm_dft.c
1489
demod/rs41dm_dft.c
Plik diff jest za duży
Load Diff
1430
demod/rs41dm_iq.c
1430
demod/rs41dm_iq.c
Plik diff jest za duży
Load Diff
1456
demod/rs92dm.c
1456
demod/rs92dm.c
Plik diff jest za duży
Load Diff
1497
demod/rs92dm_dft.c
1497
demod/rs92dm_dft.c
Plik diff jest za duży
Load Diff
Ładowanie…
Reference in New Issue