kopia lustrzana https://github.com/projecthorus/radiosonde_auto_rx
Switch to new 'mod' demodulators for RS41 and RS92
Mark Jessop
rodzic
bba0aa394c
commit
ebd678d3fe
|
|
@ -236,7 +236,7 @@ class SondeDecoder(object):
|
|||
if self.save_decode_audio:
|
||||
decode_cmd += " tee decode_%s.wav |" % str(self.device_idx)
|
||||
|
||||
decode_cmd += "./rs41ecc --crc --ecc --ptu --json 2>/dev/null"
|
||||
decode_cmd += "./rs41mod --ptu --json 2>/dev/null"
|
||||
|
||||
elif self.sonde_type == "RS92":
|
||||
# Decoding a RS92 requires either an ephemeris or an almanac file.
|
||||
|
|
@ -281,7 +281,7 @@ class SondeDecoder(object):
|
|||
if self.save_decode_audio:
|
||||
decode_cmd += " tee decode_%s.wav |" % str(self.device_idx)
|
||||
|
||||
decode_cmd += "./rs92ecc -vx -v --crc --ecc --vel --json %s 2>/dev/null" % _rs92_gps_data
|
||||
decode_cmd += "./rs92mod -vx -v --crc --ecc --vel --json %s 2>/dev/null" % _rs92_gps_data
|
||||
|
||||
elif self.sonde_type == "DFM":
|
||||
# DFM06/DFM09 Sondes.
|
||||
|
|
|
|||
|
|
@ -28,7 +28,7 @@ except ImportError:
|
|||
|
||||
|
||||
# List of binaries we check for on startup
|
||||
REQUIRED_RS_UTILS = ['dft_detect', 'rs41ecc', 'rs92ecc', 'dfm09ecc', 'm10', 'imet1rs_dft']
|
||||
REQUIRED_RS_UTILS = ['dft_detect', 'dfm09ecc', 'm10', 'imet1rs_dft', 'rs41mod', 'rs92mod']
|
||||
|
||||
def check_rs_utils():
|
||||
""" Check the required RS decoder binaries exist
|
||||
|
|
|
|||
|
|
@ -15,9 +15,22 @@ gcc -c demod_dft.c
|
|||
gcc rs92dm_dft.c demod_dft.o -lm -o rs92ecc -I../ecc/ -I../rs92
|
||||
gcc rs41dm_dft.c demod_dft.o -lm -o rs41ecc -I../ecc/ -I../rs41 -w
|
||||
gcc dfm09dm_dft.c demod_dft.o -lm -o dfm09ecc -I../ecc/ -I../dfm
|
||||
|
||||
# New demodulators
|
||||
cd ../demod/mod/
|
||||
gcc -c demod_mod.c -w
|
||||
gcc -c bch_ecc_mod.c -w
|
||||
gcc rs41mod.c demod_mod.o bch_ecc_mod.o -lm -o rs41mod -w
|
||||
# Holding off on DFM decoder until the DFM17/15 ID issue is resolved.
|
||||
#gcc dfm09mod.c demod_mod.o -lm -o dfm09mod -w
|
||||
gcc rs92mod.c demod_mod.o bch_ecc_mod.o -lm -o rs92mod -w
|
||||
#gcc lms6mod.c demod_mod.o bch_ecc_mod.o -lm -o lms6mod -w
|
||||
#gcc m10mod.c demod_mod.o -lm -o m10mod -w
|
||||
|
||||
|
||||
# Build M10 decoder
|
||||
echo "Building M10 Demodulator."
|
||||
cd ../m10/
|
||||
cd ../../m10/
|
||||
g++ M10.cpp M10Decoder.cpp M10GeneralParser.cpp M10GtopParser.cpp M10TrimbleParser.cpp AudioFile.cpp -lm -o m10 -std=c++11
|
||||
|
||||
echo "Building iMet Demodulator."
|
||||
|
|
@ -34,4 +47,9 @@ cp ../demod/dfm09ecc .
|
|||
cp ../m10/m10 .
|
||||
cp ../imet/imet1rs_dft .
|
||||
|
||||
cp ../demod/mod/rs41mod .
|
||||
#cp ../demod/mod/dfm09mod .
|
||||
cp ../demod/mod/rs92mod .
|
||||
#cp ../demod/mod/lms6mod .
|
||||
|
||||
echo "Done!"
|
||||
|
|
|
|||
|
|
@ -194,7 +194,7 @@ processing_type['rs41_rtlfm'] = {
|
|||
# Currently using a timeout to kill rtl_fm as it doesnt notice the end of the incoming samples.
|
||||
'demod': _demod_command,
|
||||
# Decode using rs41ecc
|
||||
'decode': "../rs41ecc --ptu --crc --ecc 2>/dev/null",
|
||||
'decode': "../rs41mod --ptu --crc --ecc2 2>/dev/null",
|
||||
# Count the number of telemetry lines.
|
||||
"post_process" : " | grep 00000 | wc -l",
|
||||
'files' : "./generated/rs41*.bin"
|
||||
|
|
@ -224,7 +224,7 @@ _demod_command += " sox -t raw -r %d -e s -b 16 -c 1 - -r 48000 -b 8 -t wav - hi
|
|||
processing_type['rs92_rtlfm'] = {
|
||||
'demod': _demod_command,
|
||||
# Decode using rs92ecc
|
||||
'decode': "../rs92ecc -vx -v --crc --ecc --vel 2>/dev/null",
|
||||
'decode': "../rs92mod -vx -v --crc --ecc --vel 2>/dev/null",
|
||||
#'decode': "../rs92ecc -vx -v --crc --ecc -r --vel 2>/dev/null", # For measuring No-ECC performance
|
||||
# Count the number of telemetry lines.
|
||||
"post_process" : " | grep M2513116 | wc -l",
|
||||
|
|
@ -256,7 +256,7 @@ _demod_command += " sox -t raw -r %d -e s -b 16 -c 1 - -r 48000 -b 8 -t wav - hi
|
|||
processing_type['rs92ngp_rtlfm'] = {
|
||||
'demod': _demod_command,
|
||||
# Decode using rs92ecc
|
||||
'decode': "../rs92ecc -vx -v --crc --ecc --vel 2>/dev/null",
|
||||
'decode': "../rs92mod -vx -v --crc --ecc --vel 2>/dev/null",
|
||||
#'decode': "../rs92ecc -vx -v --crc --ecc -r --vel 2>/dev/null", # For measuring No-ECC performance
|
||||
# Count the number of telemetry lines.
|
||||
"post_process" : "| grep P3213708 | wc -l",
|
||||
|
|
@ -285,7 +285,7 @@ _demod_command += " sox -t raw -r %d -e s -b 16 -c 1 - -r 48000 -b 8 -t wav - hi
|
|||
|
||||
processing_type['dfm_rtlfm'] = {
|
||||
'demod': _demod_command,
|
||||
'decode': "../dfm09ecc -vv --json --dist --auto 2>/dev/null", # ECC
|
||||
'decode': "../dfm09mod -vv --json --dist --auto 2>/dev/null", # ECC
|
||||
#'decode': "../dfm09ecc -vv --ecc -r --auto 2>/dev/null", # No-ECC
|
||||
# Count the number of telemetry lines.
|
||||
"post_process" : " | grep frame | wc -l", # ECC
|
||||
|
|
|
|||
Plik diff jest za duży
Load Diff
|
|
@ -0,0 +1,97 @@
|
|||
|
||||
/*
|
||||
* BCH / Reed-Solomon
|
||||
* encoder()
|
||||
* decoder() (Euklid. Alg.)
|
||||
*
|
||||
*
|
||||
* author: zilog80
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#ifdef INCLUDESTATIC
|
||||
#define INCSTAT static
|
||||
#else
|
||||
typedef unsigned char ui8_t;
|
||||
typedef unsigned int ui32_t;
|
||||
#define INCSTAT
|
||||
#endif
|
||||
|
||||
|
||||
#define MAX_DEG 254 // max N-1
|
||||
|
||||
|
||||
typedef struct {
|
||||
ui32_t f;
|
||||
ui32_t ord;
|
||||
ui8_t alpha;
|
||||
ui8_t exp_a[256];
|
||||
ui8_t log_a[256];
|
||||
} GF_t;
|
||||
|
||||
typedef struct {
|
||||
ui8_t N;
|
||||
ui8_t t;
|
||||
ui8_t R; // RS: R=2t, BCH: R<=mt
|
||||
ui8_t K; // K=N-R
|
||||
ui8_t b;
|
||||
ui8_t p; ui8_t ip; // p*ip = 1 mod N
|
||||
ui8_t g[MAX_DEG+1]; // ohne g[] eventuell als init_return
|
||||
GF_t GF;
|
||||
} RS_t;
|
||||
|
||||
|
||||
static GF_t GF256RS = { f : 0x11D, // RS-GF(2^8): X^8 + X^4 + X^3 + X^2 + 1 : 0x11D
|
||||
ord : 256, // 2^8
|
||||
alpha: 0x02, // generator: alpha = X
|
||||
exp_a: {0},
|
||||
log_a: {0} };
|
||||
|
||||
static GF_t GF256RSccsds = { f : 0x187, // RS-GF(2^8): X^8 + X^7 + X^2 + X + 1 : 0x187
|
||||
ord : 256, // 2^8
|
||||
alpha: 0x02, // generator: alpha = X
|
||||
exp_a: {0},
|
||||
log_a: {0} };
|
||||
|
||||
static GF_t GF64BCH = { f : 0x43, // BCH-GF(2^6): X^6 + X + 1 : 0x43
|
||||
ord : 64, // 2^6
|
||||
alpha: 0x02, // generator: alpha = X
|
||||
exp_a: {0},
|
||||
log_a: {0} };
|
||||
|
||||
static GF_t GF16RS = { f : 0x13, // RS-GF(2^4): X^4 + X + 1 : 0x13
|
||||
ord : 16, // 2^4
|
||||
alpha: 0x02, // generator: alpha = X
|
||||
exp_a: {0},
|
||||
log_a: {0} };
|
||||
|
||||
static GF_t GF256AES = { f : 0x11B, // AES-GF(2^8): X^8 + X^4 + X^3 + X + 1 : 0x11B
|
||||
ord : 256, // 2^8
|
||||
alpha: 0x03, // generator: alpha = X+1
|
||||
exp_a: {0},
|
||||
log_a: {0} };
|
||||
|
||||
|
||||
static RS_t RS256 = { 255, 12, 24, 231, 0, 1, 1, {0}, {0} };
|
||||
static RS_t RS256ccsds = { 255, 16, 32, 223, 112, 11, 116, {0}, {0} };
|
||||
static RS_t BCH64 = { 63, 2, 12, 51, 1, 1, 1, {0}, {0} };
|
||||
|
||||
// static RS_t RS16_0 = { 15, 3, 6, 9, 0, 1, 1, {0}, {0} };
|
||||
static RS_t RS16ccsds = { 15, 2, 4, 11, 6, 1, 1, {0}, {0} };
|
||||
|
||||
|
||||
#ifndef INCLUDESTATIC
|
||||
|
||||
int rs_init_RS255(RS_t *RS);
|
||||
int rs_init_RS255ccsds(RS_t *RS);
|
||||
int rs_init_RS15ccsds(RS_t *RS);
|
||||
int rs_init_BCH64(RS_t *RS);
|
||||
|
||||
int rs_encode(RS_t *RS, ui8_t cw[]);
|
||||
int rs_decode(RS_t *RS, ui8_t cw[], ui8_t *err_pos, ui8_t *err_val);
|
||||
int rs_decode_ErrEra(RS_t *RS, ui8_t cw[], int nera, ui8_t era_pos[], ui8_t *err_pos, ui8_t *err_val);
|
||||
int rs_decode_bch_gf2t2(RS_t *RS, ui8_t cw[], ui8_t *err_pos, ui8_t *err_val);
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,827 @@
|
|||
|
||||
/*
|
||||
* sync header: correlation/matched filter
|
||||
* compile:
|
||||
* gcc -c demod_mod.c
|
||||
*
|
||||
* author: zilog80
|
||||
*/
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "demod_mod.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;
|
||||
}
|
||||
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
static int getCorrDFT(dsp_t *dsp) {
|
||||
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;
|
||||
|
||||
ui32_t pos = dsp->sample_out;
|
||||
|
||||
dsp->mv = 0.0;
|
||||
dsp->dc = 0.0;
|
||||
|
||||
if (dsp->K + dsp->L > dsp->DFT.N) return -1;
|
||||
if (dsp->sample_out < dsp->L) return -2;
|
||||
|
||||
|
||||
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;
|
||||
|
||||
dsp->mv = mx;
|
||||
dsp->mv_pos = 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 = 0.8;
|
||||
int n;
|
||||
|
||||
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;
|
||||
|
||||
/* //if (rs_type==rs41) get_SNR(dsp);
|
||||
// rs41, constant amplitude, avg/filter
|
||||
s = 0.0;
|
||||
for (n = 0; n < dsp->sps; n++) s += cabs(dsp->rot_iqbuf[(dsp->sample_in - n + dsp->N_IQBUF) % dsp->N_IQBUF]);
|
||||
s /= (float)n;
|
||||
*/
|
||||
|
||||
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;
|
||||
|
||||
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
|
||||
|
||||
double rbitgrenze = pos*symlen*dsp->sps;
|
||||
ui32_t rcount = ceil(rbitgrenze);//+0.99; // dfm?
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
static int headcmp(dsp_t *dsp, int opt_dc) {
|
||||
int errs = 0;
|
||||
int pos;
|
||||
int step = 1;
|
||||
char sign = 0;
|
||||
int len = dsp->hdrlen/dsp->symhd;
|
||||
int inv = dsp->mv < 0;
|
||||
|
||||
if (dsp->symhd != 1) step = 2;
|
||||
if (inv) sign=1;
|
||||
|
||||
for (pos = 0; pos < len; pos++) { // L = dsp->hdrlen * dsp->sps + 0.5;
|
||||
//read_bufbit(dsp, dsp->symhd, dsp->rawbits+pos*step, mvp+1-(int)(len*dsp->sps), pos);
|
||||
read_bufbit(dsp, dsp->symhd, dsp->rawbits+pos*step, dsp->mv_pos+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 (opt_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 *bit, int inv, int ofs, int pos, float l, int spike) {
|
||||
// symlen==2: manchester2 10->0,01->1: 2.bit
|
||||
|
||||
float sample;
|
||||
float avg;
|
||||
float ths = 0.5, scale = 0.27;
|
||||
|
||||
double sum = 0.0;
|
||||
double mid;
|
||||
//double l = 1.0;
|
||||
|
||||
double bg = pos*dsp->symlen*dsp->sps;
|
||||
|
||||
if (pos == 0) {
|
||||
bg = 0;
|
||||
dsp->sc = 0;
|
||||
}
|
||||
|
||||
|
||||
if (dsp->symlen == 2) {
|
||||
mid = bg + (dsp->sps-1)/2.0;
|
||||
bg += 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];
|
||||
if (spike && fabs(sample - avg) > ths) {
|
||||
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]);
|
||||
sample = avg + scale*(sample - avg); // spikes
|
||||
}
|
||||
|
||||
if ( l < 0 || (mid-l < dsp->sc && dsp->sc < mid+l)) sum -= sample;
|
||||
|
||||
dsp->sc++;
|
||||
} while (dsp->sc < bg); // n < dsp->sps
|
||||
}
|
||||
|
||||
mid = bg + (dsp->sps-1)/2.0;
|
||||
bg += 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];
|
||||
if (spike && fabs(sample - avg) > ths) {
|
||||
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]);
|
||||
sample = avg + scale*(sample - avg); // spikes
|
||||
}
|
||||
|
||||
if ( l < 0 || (mid-l < dsp->sc && dsp->sc < mid+l)) sum += sample;
|
||||
|
||||
dsp->sc++;
|
||||
} while (dsp->sc < bg); // 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;
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
|
||||
int find_header(dsp_t *dsp, float thres, int hdmax, int bitofs, int opt_dc) {
|
||||
ui32_t k = 0;
|
||||
ui32_t mvpos0 = 0;
|
||||
int mp;
|
||||
int header_found = 0;
|
||||
int herrs;
|
||||
|
||||
while ( f32buf_sample(dsp, 0) != EOF ) {
|
||||
|
||||
k += 1;
|
||||
if (k >= dsp->K-4) {
|
||||
mvpos0 = dsp->mv_pos;
|
||||
mp = getCorrDFT(dsp); // correlation score -> dsp->mv
|
||||
//if (option_auto == 0 && dsp->mv < 0) mv = 0;
|
||||
k = 0;
|
||||
}
|
||||
else {
|
||||
dsp->mv = 0.0;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (dsp->mv > thres || dsp->mv < -thres) {
|
||||
|
||||
if (dsp->mv_pos > mvpos0) {
|
||||
|
||||
header_found = 0;
|
||||
herrs = headcmp(dsp, opt_dc);
|
||||
if (herrs <= hdmax) header_found = 1; // max bitfehler in header
|
||||
|
||||
if (header_found) return 1;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return EOF;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,119 @@
|
|||
|
||||
|
||||
#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;
|
||||
int symhd;
|
||||
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;
|
||||
ui32_t sc;
|
||||
int buffered;
|
||||
int L;
|
||||
int M;
|
||||
int K;
|
||||
float *match;
|
||||
float *bufs;
|
||||
float dc_ofs;
|
||||
float dc;
|
||||
float mv;
|
||||
ui32_t mv_pos;
|
||||
//
|
||||
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, float, 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 *);
|
||||
|
||||
int find_header(dsp_t *, float, int, int, int);
|
||||
|
||||
Plik diff jest za duży
Load Diff
|
|
@ -0,0 +1,984 @@
|
|||
|
||||
/*
|
||||
* LMS6
|
||||
* (403 MHz)
|
||||
*
|
||||
* sync header: correlation/matched filter
|
||||
* files: lms6mod.c demod_mod.c demod_mod.h bch_ecc_mod.c bch_ecc_mod.h
|
||||
* compile, either (a) or (b):
|
||||
* (a)
|
||||
* gcc -c demod_mod.c
|
||||
* gcc -DINCLUDESTATIC lms6mod.c demod_mod.o -lm -o lms6mod
|
||||
* (b)
|
||||
* gcc -c demod_mod.c
|
||||
* gcc -c bch_ecc_mod.c
|
||||
* gcc lms6mod.c demod_mod.o bch_ecc_mod.o -lm -o lms6mod
|
||||
*
|
||||
* usage:
|
||||
* ./lms6mod --vit --ecc <audio.wav>
|
||||
* ( --vit recommended)
|
||||
* 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_mod.h"
|
||||
|
||||
//#define INCLUDESTATIC 1
|
||||
#ifdef INCLUDESTATIC
|
||||
#include "bch_ecc_mod.c"
|
||||
#else
|
||||
#include "bch_ecc_mod.h"
|
||||
#endif
|
||||
|
||||
|
||||
typedef struct {
|
||||
i8_t vbs; // verbose output
|
||||
i8_t raw; // raw frames
|
||||
i8_t crc; // CRC check output
|
||||
i8_t ecc; // Reed-Solomon ECC
|
||||
i8_t sat; // GPS sat data
|
||||
i8_t ptu; // PTU: temperature
|
||||
i8_t inv;
|
||||
i8_t vit;
|
||||
i8_t jsn; // JSON output (auto_rx)
|
||||
} option_t;
|
||||
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
#define BAUD_RATE 4800
|
||||
|
||||
#define BITS 8
|
||||
#define HEADOFS 0 //16
|
||||
#define HEADLEN ((4*16)-HEADOFS)
|
||||
|
||||
#define SYNC_LEN 5
|
||||
#define FRM_LEN (223)
|
||||
#define PAR_LEN (32)
|
||||
#define FRMBUF_LEN (3*FRM_LEN)
|
||||
#define BLOCKSTART (SYNC_LEN*BITS*2)
|
||||
#define BLOCK_LEN (FRM_LEN+PAR_LEN+SYNC_LEN) // 255+5 = 260
|
||||
#define RAWBITBLOCK_LEN ((BLOCK_LEN+1)*BITS*2) // (+1 tail)
|
||||
|
||||
#define FRAME_LEN (300) // 4800baud, 16bits/byte
|
||||
#define BITFRAME_LEN (FRAME_LEN*BITS)
|
||||
#define RAWBITFRAME_LEN (BITFRAME_LEN*2)
|
||||
#define OVERLAP 64
|
||||
#define OFS 4
|
||||
|
||||
|
||||
static char rawheader[] = "0101011000001000""0001110010010111""0001101010100111""0011110100111110"; // (c0,inv(c1))
|
||||
// (00) 58 f3 3f b8
|
||||
// char header[] = "0000001101011101""0100100111000010""0100111111110010""0110100001101011"; // (c0,c1)
|
||||
static ui8_t rs_sync[] = { 0x00, 0x58, 0xf3, 0x3f, 0xb8};
|
||||
// 0x58f33fb8 little-endian <-> 0x1ACFFC1D big-endian bytes
|
||||
|
||||
// (00) 58 f3 3f b8
|
||||
static char blk_syncbits[] = "0000000000000000""0000001101011101""0100100111000010""0100111111110010""0110100001101011";
|
||||
|
||||
static ui8_t frm_sync[] = { 0x24, 0x54, 0x00, 0x00};
|
||||
|
||||
|
||||
#define L 7 // d_f=10
|
||||
static char polyA[] = "1001111"; // 0x4f: x^6+x^3+x^2+x+1
|
||||
static char polyB[] = "1101101"; // 0x6d: x^6+x^5+x^3+x^2+1
|
||||
/*
|
||||
// d_f=6
|
||||
qA[] = "1110011"; // 0x73: x^6+x^5+x^4+x+1
|
||||
qB[] = "0011110"; // 0x1e: x^4+x^3+x^2+x
|
||||
pA[] = "10010101"; // 0x95: x^7+x^4+x^2+1 = (x+1)(x^6+x^5+x^4+x+1) = (x+1)qA
|
||||
pB[] = "00100010"; // 0x22: x^5+x = (x+1)(x^4+x^3+x^2+x)=x(x+1)^3 = (x+1)qB
|
||||
polyA = qA + x*qB
|
||||
polyB = qA + qB
|
||||
*/
|
||||
|
||||
#define N (1 << L)
|
||||
#define M (1 << (L-1))
|
||||
|
||||
typedef struct {
|
||||
ui8_t bIn;
|
||||
ui8_t codeIn;
|
||||
ui8_t prevState; // 0..M=64
|
||||
int w; // > 255 : if (w>250): w=250 ?
|
||||
//float sw;
|
||||
} states_t;
|
||||
|
||||
typedef struct {
|
||||
char rawbits[RAWBITFRAME_LEN+OVERLAP*BITS*2 +8];
|
||||
states_t state[RAWBITFRAME_LEN+OVERLAP +8][M];
|
||||
states_t d[N];
|
||||
} VIT_t;
|
||||
|
||||
typedef struct {
|
||||
int frnr;
|
||||
int sn;
|
||||
int week; int gpstow;
|
||||
int jahr; int monat; int tag;
|
||||
int wday;
|
||||
int std; int min; float sek;
|
||||
double lat; double lon; double alt;
|
||||
double vH; double vD; double vV;
|
||||
double vE; double vN; double vU;
|
||||
char blk_rawbits[RAWBITBLOCK_LEN+SYNC_LEN*BITS*2 +8];
|
||||
ui8_t frame[FRM_LEN]; // = { 0x24, 0x54, 0x00, 0x00}; // dataheader
|
||||
int frm_pos; // ecc_blk <-> frm_blk
|
||||
int sf;
|
||||
option_t option;
|
||||
RS_t RS;
|
||||
VIT_t *vit;
|
||||
} gpx_t;
|
||||
|
||||
|
||||
// ------------------------------------------------------------------------
|
||||
|
||||
static ui8_t vit_code[N];
|
||||
static vitCodes_init = 0;
|
||||
|
||||
static int vit_initCodes(gpx_t *gpx) {
|
||||
int cA, cB;
|
||||
int i, bits;
|
||||
|
||||
VIT_t *pv = calloc(1, sizeof(VIT_t));
|
||||
if (pv == NULL) return -1;
|
||||
gpx->vit = pv;
|
||||
|
||||
if ( vitCodes_init == 0 ) {
|
||||
for (bits = 0; bits < N; bits++) {
|
||||
cA = 0;
|
||||
cB = 0;
|
||||
for (i = 0; i < L; i++) {
|
||||
cA ^= (polyA[L-1-i]&1) & ((bits >> i)&1);
|
||||
cB ^= (polyB[L-1-i]&1) & ((bits >> i)&1);
|
||||
}
|
||||
vit_code[bits] = (cA<<1) | cB;
|
||||
}
|
||||
vitCodes_init = 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int vit_dist(int c, char *rc) {
|
||||
return (((c>>1)^rc[0])&1) + ((c^rc[1])&1);
|
||||
}
|
||||
|
||||
static int vit_start(VIT_t *vit, char *rc) {
|
||||
int t, m, j, c, d;
|
||||
|
||||
t = L-1;
|
||||
m = M;
|
||||
while ( t > 0 ) { // t=0..L-2: nextState<M
|
||||
for (j = 0; j < m; j++) {
|
||||
vit->state[t][j].prevState = j/2;
|
||||
}
|
||||
t--;
|
||||
m /= 2;
|
||||
}
|
||||
|
||||
m = 2;
|
||||
for (t = 1; t < L; t++) {
|
||||
for (j = 0; j < m; j++) {
|
||||
c = vit_code[j];
|
||||
vit->state[t][j].bIn = j % 2;
|
||||
vit->state[t][j].codeIn = c;
|
||||
d = vit_dist( c, rc+2*(t-1) );
|
||||
vit->state[t][j].w = vit->state[t-1][vit->state[t][j].prevState].w + d;
|
||||
}
|
||||
m *= 2;
|
||||
}
|
||||
|
||||
return t;
|
||||
}
|
||||
|
||||
static int vit_next(VIT_t *vit, int t, char *rc) {
|
||||
int b, nstate;
|
||||
int j, index;
|
||||
|
||||
for (j = 0; j < M; j++) {
|
||||
for (b = 0; b < 2; b++) {
|
||||
nstate = j*2 + b;
|
||||
vit->d[nstate].bIn = b;
|
||||
vit->d[nstate].codeIn = vit_code[nstate];
|
||||
vit->d[nstate].prevState = j;
|
||||
vit->d[nstate].w = vit->state[t][j].w + vit_dist( vit->d[nstate].codeIn, rc );
|
||||
}
|
||||
}
|
||||
|
||||
for (j = 0; j < M; j++) {
|
||||
|
||||
if ( vit->d[j].w <= vit->d[j+M].w ) index = j; else index = j+M;
|
||||
|
||||
vit->state[t+1][j] = vit->d[index];
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int vit_path(VIT_t *vit, int j, int t) {
|
||||
int c;
|
||||
|
||||
vit->rawbits[2*t] = '\0';
|
||||
while (t > 0) {
|
||||
c = vit->state[t][j].codeIn;
|
||||
vit->rawbits[2*t -2] = 0x30 + ((c>>1) & 1);
|
||||
vit->rawbits[2*t -1] = 0x30 + (c & 1);
|
||||
j = vit->state[t][j].prevState;
|
||||
t--;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int viterbi(VIT_t *vit, char *rc) {
|
||||
int t, tmax;
|
||||
int j, j_min, w_min;
|
||||
|
||||
vit_start(vit, rc);
|
||||
|
||||
tmax = strlen(rc)/2;
|
||||
|
||||
for (t = L-1; t < tmax; t++)
|
||||
{
|
||||
vit_next(vit, t, rc+2*t);
|
||||
}
|
||||
|
||||
w_min = -1;
|
||||
for (j = 0; j < M; j++) {
|
||||
if (w_min < 0) {
|
||||
w_min = vit->state[tmax][j].w;
|
||||
j_min = j;
|
||||
}
|
||||
if (vit->state[tmax][j].w < w_min) {
|
||||
w_min = vit->state[tmax][j].w;
|
||||
j_min = j;
|
||||
}
|
||||
}
|
||||
vit_path(vit, j_min, tmax);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------------
|
||||
|
||||
static int deconv(char* rawbits, char *bits) {
|
||||
|
||||
int j, n, bitA, bitB;
|
||||
char *p;
|
||||
int len;
|
||||
int errors = 0;
|
||||
int m = L-1;
|
||||
|
||||
len = strlen(rawbits);
|
||||
for (j = 0; j < m; j++) bits[j] = '0';
|
||||
n = 0;
|
||||
while ( 2*(m+n) < len ) {
|
||||
p = rawbits+2*(m+n);
|
||||
bitA = bitB = 0;
|
||||
for (j = 0; j < m; j++) {
|
||||
bitA ^= (bits[n+j]&1) & (polyA[j]&1);
|
||||
bitB ^= (bits[n+j]&1) & (polyB[j]&1);
|
||||
}
|
||||
if ( (bitA^(p[0]&1))==(polyA[m]&1) && (bitB^(p[1]&1))==(polyB[m]&1) ) bits[n+m] = '1';
|
||||
else if ( (bitA^(p[0]&1))==0 && (bitB^(p[1]&1))==0 ) bits[n+m] = '0';
|
||||
else {
|
||||
if ( (bitA^(p[0]&1))!=(polyA[m]&1) && (bitB^(p[1]&1))==(polyB[m]&1) ) bits[n+m] = 0x39;
|
||||
else bits[n+m] = 0x38;
|
||||
errors = n;
|
||||
break;
|
||||
}
|
||||
n += 1;
|
||||
}
|
||||
bits[n+m] = '\0';
|
||||
|
||||
return errors;
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------------
|
||||
|
||||
static int crc16_0(ui8_t frame[], int len) {
|
||||
int crc16poly = 0x1021;
|
||||
int rem = 0x0, i, j;
|
||||
int byte;
|
||||
|
||||
for (i = 0; i < len; i++) {
|
||||
byte = frame[i];
|
||||
rem = rem ^ (byte << 8);
|
||||
for (j = 0; j < 8; j++) {
|
||||
if (rem & 0x8000) {
|
||||
rem = (rem << 1) ^ crc16poly;
|
||||
}
|
||||
else {
|
||||
rem = (rem << 1);
|
||||
}
|
||||
rem &= 0xFFFF;
|
||||
}
|
||||
}
|
||||
return rem;
|
||||
}
|
||||
|
||||
static int check_CRC(ui8_t frame[]) {
|
||||
ui32_t crclen = 0,
|
||||
crcdat = 0;
|
||||
|
||||
crclen = 221;
|
||||
crcdat = (frame[crclen]<<8) | frame[crclen+1];
|
||||
if ( crcdat != crc16_0(frame, crclen) ) {
|
||||
return 1; // CRC NO
|
||||
}
|
||||
else return 0; // CRC OK
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------------
|
||||
|
||||
static int bits2bytes(char *bitstr, ui8_t *bytes) {
|
||||
int i, bit, d, byteval;
|
||||
int len = strlen(bitstr)/8;
|
||||
int bitpos, bytepos;
|
||||
|
||||
bitpos = 0;
|
||||
bytepos = 0;
|
||||
|
||||
while (bytepos < len) {
|
||||
|
||||
byteval = 0;
|
||||
d = 1;
|
||||
for (i = 0; i < BITS; i++) {
|
||||
bit=*(bitstr+bitpos+i); /* little endian */
|
||||
//bit=*(bitstr+bitpos+7-i); /* big endian */
|
||||
if ((bit == '1') || (bit == '9')) byteval += d;
|
||||
else /*if ((bit == '0') || (bit == '8'))*/ byteval += 0;
|
||||
d <<= 1;
|
||||
}
|
||||
bitpos += BITS;
|
||||
bytes[bytepos++] = byteval & 0xFF;
|
||||
}
|
||||
|
||||
//while (bytepos < FRAME_LEN+OVERLAP) bytes[bytepos++] = 0;
|
||||
|
||||
return bytepos;
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
|
||||
#define pos_SondeSN (OFS+0x00) // ?4 byte 00 7A....
|
||||
#define pos_FrameNb (OFS+0x04) // 2 byte
|
||||
//GPS Position
|
||||
#define pos_GPSTOW (OFS+0x06) // 4 byte
|
||||
#define pos_GPSlat (OFS+0x0E) // 4 byte
|
||||
#define pos_GPSlon (OFS+0x12) // 4 byte
|
||||
#define pos_GPSalt (OFS+0x16) // 4 byte
|
||||
//GPS Velocity East-North-Up (ENU)
|
||||
#define pos_GPSvO (OFS+0x1A) // 3 byte
|
||||
#define pos_GPSvN (OFS+0x1D) // 3 byte
|
||||
#define pos_GPSvV (OFS+0x20) // 3 byte
|
||||
|
||||
|
||||
static int get_SondeSN(gpx_t *gpx) {
|
||||
unsigned byte;
|
||||
|
||||
byte = (gpx->frame[pos_SondeSN]<<24) | (gpx->frame[pos_SondeSN+1]<<16)
|
||||
| (gpx->frame[pos_SondeSN+2]<<8) | gpx->frame[pos_SondeSN+3];
|
||||
gpx->sn = byte & 0xFFFFFF;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int get_FrameNb(gpx_t *gpx) {
|
||||
int i;
|
||||
unsigned byte;
|
||||
ui8_t frnr_bytes[2];
|
||||
int frnr;
|
||||
|
||||
for (i = 0; i < 2; i++) {
|
||||
byte = gpx->frame[pos_FrameNb + i];
|
||||
frnr_bytes[i] = byte;
|
||||
}
|
||||
|
||||
frnr = (frnr_bytes[0] << 8) + frnr_bytes[1] ;
|
||||
gpx->frnr = frnr;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
//char weekday[7][3] = { "So", "Mo", "Di", "Mi", "Do", "Fr", "Sa"};
|
||||
static char weekday[7][4] = { "Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat"};
|
||||
|
||||
static int get_GPStime(gpx_t *gpx) {
|
||||
int i;
|
||||
unsigned byte;
|
||||
ui8_t gpstime_bytes[4];
|
||||
int gpstime = 0, // 32bit
|
||||
day;
|
||||
float ms;
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
byte = gpx->frame[pos_GPSTOW + i];
|
||||
gpstime_bytes[i] = byte;
|
||||
}
|
||||
gpstime = 0;
|
||||
for (i = 0; i < 4; i++) {
|
||||
gpstime |= gpstime_bytes[i] << (8*(3-i));
|
||||
}
|
||||
|
||||
gpx->gpstow = gpstime;
|
||||
|
||||
ms = gpstime % 1000;
|
||||
gpstime /= 1000;
|
||||
|
||||
day = gpstime / (24 * 3600);
|
||||
gpstime %= (24*3600);
|
||||
|
||||
if ((day < 0) || (day > 6)) return -1;
|
||||
|
||||
gpx->wday = day;
|
||||
gpx->std = gpstime / 3600;
|
||||
gpx->min = (gpstime % 3600) / 60;
|
||||
gpx->sek = gpstime % 60 + ms/1000.0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static double B60B60 = (1<<30)/90.0; // 2^32/360 = 2^30/90 = 0xB60B60.711x
|
||||
|
||||
static int get_GPSlat(gpx_t *gpx) {
|
||||
int i;
|
||||
unsigned byte;
|
||||
ui8_t gpslat_bytes[4];
|
||||
int gpslat;
|
||||
double lat;
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
byte = gpx->frame[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;
|
||||
gpx->lat = lat;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int get_GPSlon(gpx_t *gpx) {
|
||||
int i;
|
||||
unsigned byte;
|
||||
ui8_t gpslon_bytes[4];
|
||||
int gpslon;
|
||||
double lon;
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
byte = gpx->frame[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;
|
||||
gpx->lon = lon;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int get_GPSalt(gpx_t *gpx) {
|
||||
int i;
|
||||
unsigned byte;
|
||||
ui8_t gpsheight_bytes[4];
|
||||
int gpsheight;
|
||||
double height;
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
byte = gpx->frame[pos_GPSalt + i];
|
||||
gpsheight_bytes[i] = byte;
|
||||
}
|
||||
|
||||
gpsheight = 0;
|
||||
for (i = 0; i < 4; i++) {
|
||||
gpsheight |= gpsheight_bytes[i] << (8*(3-i));
|
||||
}
|
||||
height = gpsheight / 1000.0;
|
||||
gpx->alt = height;
|
||||
|
||||
if (height < -200 || height > 60000) return -1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int get_GPSvel24(gpx_t *gpx) {
|
||||
int i;
|
||||
unsigned byte;
|
||||
ui8_t gpsVel_bytes[3];
|
||||
int vel24;
|
||||
double vx, vy, vz, dir; //, alpha;
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
byte = gpx->frame[pos_GPSvO + i];
|
||||
gpsVel_bytes[i] = byte;
|
||||
}
|
||||
vel24 = gpsVel_bytes[0] << 16 | gpsVel_bytes[1] << 8 | gpsVel_bytes[2];
|
||||
if (vel24 > (0x7FFFFF)) vel24 -= 0x1000000;
|
||||
vx = vel24 / 1e3; // ost
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
byte = gpx->frame[pos_GPSvN + i];
|
||||
gpsVel_bytes[i] = byte;
|
||||
}
|
||||
vel24 = gpsVel_bytes[0] << 16 | gpsVel_bytes[1] << 8 | gpsVel_bytes[2];
|
||||
if (vel24 > (0x7FFFFF)) vel24 -= 0x1000000;
|
||||
vy= vel24 / 1e3; // nord
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
byte = gpx->frame[pos_GPSvV + i];
|
||||
gpsVel_bytes[i] = byte;
|
||||
}
|
||||
vel24 = gpsVel_bytes[0] << 16 | gpsVel_bytes[1] << 8 | gpsVel_bytes[2];
|
||||
if (vel24 > (0x7FFFFF)) vel24 -= 0x1000000;
|
||||
vz = vel24 / 1e3; // hoch
|
||||
|
||||
gpx->vE = vx;
|
||||
gpx->vN = vy;
|
||||
gpx->vU = vz;
|
||||
|
||||
|
||||
gpx->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)
|
||||
gpx->vD2 = dir;
|
||||
*/
|
||||
dir = atan2(vx, vy) * 180 / M_PI;
|
||||
if (dir < 0) dir += 360;
|
||||
gpx->vD = dir;
|
||||
|
||||
gpx->vV = vz;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// RS(255,223)-CCSDS
|
||||
#define rs_N 255
|
||||
#define rs_K 223
|
||||
#define rs_R (rs_N-rs_K) // 32
|
||||
|
||||
static int lms6_ecc(gpx_t *gpx, ui8_t *cw) {
|
||||
int errors;
|
||||
ui8_t err_pos[rs_R],
|
||||
err_val[rs_R];
|
||||
|
||||
errors = rs_decode(&gpx->RS, cw, err_pos, err_val);
|
||||
|
||||
return errors;
|
||||
}
|
||||
|
||||
static void print_frame(gpx_t *gpx, int crc_err, int len) {
|
||||
int err=0;
|
||||
|
||||
if (gpx->frame[0] != 0)
|
||||
{
|
||||
//if ((gpx->frame[pos_SondeSN+1] & 0xF0) == 0x70) // ? beginnen alle SNs mit 0x7A.... bzw 80..... ?
|
||||
if ( gpx->frame[pos_SondeSN+1] )
|
||||
{
|
||||
get_SondeSN(gpx);
|
||||
get_FrameNb(gpx);
|
||||
printf(" (%7d) ", gpx->sn);
|
||||
printf(" [%5d] ", gpx->frnr);
|
||||
err = get_GPStime(gpx);
|
||||
if (!err) printf("%s ", weekday[gpx->wday]);
|
||||
printf("%02d:%02d:%06.3f ", gpx->std, gpx->min, gpx->sek); // falls Rundung auf 60s: Ueberlauf
|
||||
|
||||
get_GPSlat(gpx);
|
||||
get_GPSlon(gpx);
|
||||
err = get_GPSalt(gpx);
|
||||
if (!err) {
|
||||
printf(" lat: %.5f ", gpx->lat);
|
||||
printf(" lon: %.5f ", gpx->lon);
|
||||
printf(" alt: %.2fm ", gpx->alt);
|
||||
get_GPSvel24(gpx);
|
||||
//if (gpx->option.vbs == 2) printf(" (%.1f ,%.1f,%.1f) ", gpx->vE, gpx->vN, gpx->vU);
|
||||
printf(" vH: %.1fm/s D: %.1f vV: %.1fm/s ", gpx->vH, gpx->vD, gpx->vV);
|
||||
}
|
||||
|
||||
if (crc_err==0) printf(" [OK]"); else printf(" [NO]");
|
||||
|
||||
printf("\n");
|
||||
|
||||
|
||||
if (gpx->option.jsn) {
|
||||
// Print JSON output required by auto_rx.
|
||||
if (crc_err==0) { // CRC-OK
|
||||
// UTC oder GPS?
|
||||
printf("{ \"frame\": %d, \"id\": \"%d\", \"time\": \"%02d:%02d:%06.3fZ\", \"lat\": %.5f, \"lon\": %.5f, \"alt\": %.5f, \"vel_h\": %.5f, \"heading\": %.5f, \"vel_v\": %.5f }\n",
|
||||
gpx->frnr, gpx->sn, gpx->std, gpx->min, gpx->sek, gpx->lat, gpx->lon, gpx->alt, gpx->vH, gpx->vD, gpx->vV );
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void proc_frame(gpx_t *gpx, int len) {
|
||||
int blk_pos = SYNC_LEN;
|
||||
ui8_t block_bytes[BLOCK_LEN+8];
|
||||
ui8_t rs_cw[rs_N];
|
||||
char frame_bits[BITFRAME_LEN+OVERLAP*BITS +8]; // init L-1 bits mit 0
|
||||
char *rawbits = NULL;
|
||||
int i, j;
|
||||
int err = 0;
|
||||
int errs = 0;
|
||||
int crc_err = 0;
|
||||
int flen, blen;
|
||||
|
||||
|
||||
if ((len % 8) > 4) {
|
||||
while (len % 8) gpx->blk_rawbits[len++] = '0';
|
||||
}
|
||||
gpx->blk_rawbits[len] = '\0';
|
||||
|
||||
flen = len / (2*BITS);
|
||||
|
||||
if (gpx->option.vit == 1) {
|
||||
viterbi(gpx->vit, gpx->blk_rawbits);
|
||||
rawbits = gpx->vit->rawbits;
|
||||
}
|
||||
else rawbits = gpx->blk_rawbits;
|
||||
|
||||
err = deconv(rawbits, frame_bits);
|
||||
|
||||
if (err) { for (i=err; i < RAWBITBLOCK_LEN/2; i++) frame_bits[i] = 0; }
|
||||
|
||||
|
||||
blen = bits2bytes(frame_bits, block_bytes);
|
||||
for (j = blen; j < BLOCK_LEN+8; j++) block_bytes[j] = 0;
|
||||
|
||||
|
||||
if (gpx->option.ecc) {
|
||||
for (j = 0; j < rs_N; j++) rs_cw[rs_N-1-j] = block_bytes[SYNC_LEN+j];
|
||||
errs = lms6_ecc(gpx, rs_cw);
|
||||
for (j = 0; j < rs_N; j++) block_bytes[SYNC_LEN+j] = rs_cw[rs_N-1-j];
|
||||
}
|
||||
|
||||
if (gpx->option.raw == 2) {
|
||||
for (i = 0; i < flen; i++) printf("%02x ", block_bytes[i]);
|
||||
if (gpx->option.ecc) printf("(%d)", errs);
|
||||
printf("\n");
|
||||
}
|
||||
else if (gpx->option.raw == 4 && gpx->option.ecc) {
|
||||
for (i = 0; i < rs_N; i++) printf("%02x", block_bytes[SYNC_LEN+i]);
|
||||
printf(" (%d)", errs);
|
||||
printf("\n");
|
||||
}
|
||||
else if (gpx->option.raw == 8) {
|
||||
if (gpx->option.vit == 1) {
|
||||
for (i = 0; i < len; i++) printf("%c", gpx->vit->rawbits[i]); printf("\n");
|
||||
}
|
||||
else {
|
||||
for (i = 0; i < len; i++) printf("%c", gpx->blk_rawbits[i]); printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
blk_pos = SYNC_LEN;
|
||||
|
||||
while ( blk_pos-SYNC_LEN < FRM_LEN ) {
|
||||
|
||||
if (gpx->sf == 0) {
|
||||
while ( blk_pos-SYNC_LEN < FRM_LEN ) {
|
||||
gpx->sf = 0;
|
||||
for (j = 0; j < 4; j++) gpx->sf += (block_bytes[blk_pos+j] == frm_sync[j]);
|
||||
if (gpx->sf == 4) {
|
||||
gpx->frm_pos = 0;
|
||||
break;
|
||||
}
|
||||
blk_pos++;
|
||||
}
|
||||
}
|
||||
|
||||
if ( gpx->sf && gpx->frm_pos < FRM_LEN ) {
|
||||
gpx->frame[gpx->frm_pos] = block_bytes[blk_pos];
|
||||
gpx->frm_pos++;
|
||||
blk_pos++;
|
||||
}
|
||||
|
||||
if (gpx->frm_pos == FRM_LEN) {
|
||||
|
||||
crc_err = check_CRC(gpx->frame);
|
||||
|
||||
if (gpx->option.raw == 1) {
|
||||
for (i = 0; i < FRM_LEN; i++) printf("%02x ", gpx->frame[i]);
|
||||
if (crc_err==0) printf(" [OK]"); else printf(" [NO]");
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
if (gpx->option.raw == 0) print_frame(gpx, crc_err, len);
|
||||
|
||||
gpx->frm_pos = 0;
|
||||
gpx->sf = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
|
||||
int option_inv = 0; // invertiert Signal
|
||||
int option_iq = 0;
|
||||
int option_dc = 0;
|
||||
int wavloaded = 0;
|
||||
int sel_wavch = 0; // audio channel: left
|
||||
|
||||
FILE *fp = NULL;
|
||||
char *fpname = NULL;
|
||||
|
||||
int k;
|
||||
|
||||
int bit, rbit;
|
||||
int bitpos = 0;
|
||||
int bitQ;
|
||||
int pos;
|
||||
//int headerlen = 0;
|
||||
|
||||
int header_found = 0;
|
||||
|
||||
float thres = 0.76;
|
||||
float _mv = 0.0;
|
||||
|
||||
int symlen = 1;
|
||||
int bitofs = 1; // +1 .. +2
|
||||
int shift = 0;
|
||||
|
||||
unsigned int bc = 0;
|
||||
|
||||
pcm_t pcm = {0};
|
||||
dsp_t dsp = {0}; //memset(&dsp, 0, sizeof(dsp));
|
||||
/*
|
||||
// gpx_t _gpx = {0}; gpx_t *gpx = &_gpx; // stack size ...
|
||||
gpx_t *gpx = NULL;
|
||||
gpx = calloc(1, sizeof(gpx_t));
|
||||
//memset(gpx, 0, sizeof(gpx_t));
|
||||
*/
|
||||
gpx_t _gpx = {0}; gpx_t *gpx = &_gpx;
|
||||
|
||||
|
||||
#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, " --vit (Viterbi)\n");
|
||||
fprintf(stderr, " --ecc (Reed-Solomon)\n");
|
||||
return 0;
|
||||
}
|
||||
else if ( (strcmp(*argv, "-v") == 0) || (strcmp(*argv, "--verbose") == 0) ) {
|
||||
gpx->option.vbs = 1;
|
||||
}
|
||||
else if ( (strcmp(*argv, "-r") == 0) || (strcmp(*argv, "--raw") == 0) ) {
|
||||
gpx->option.raw = 1; // bytes - rs_ecc_codewords
|
||||
}
|
||||
else if ( (strcmp(*argv, "-r0") == 0) || (strcmp(*argv, "--raw0") == 0) ) {
|
||||
gpx->option.raw = 2; // bytes: sync + codewords
|
||||
}
|
||||
else if ( (strcmp(*argv, "-rc") == 0) || (strcmp(*argv, "--rawecc") == 0) ) {
|
||||
gpx->option.raw = 4; // rs_ecc_codewords
|
||||
}
|
||||
else if ( (strcmp(*argv, "-R") == 0) || (strcmp(*argv, "--RAW") == 0) ) {
|
||||
gpx->option.raw = 8; // rawbits
|
||||
}
|
||||
else if (strcmp(*argv, "--ecc" ) == 0) { gpx->option.ecc = 1; } // RS-ECC
|
||||
else if (strcmp(*argv, "--vit" ) == 0) { gpx->option.vit = 1; } // viterbi
|
||||
else if ( (strcmp(*argv, "-i") == 0) || (strcmp(*argv, "--invert") == 0) ) {
|
||||
option_inv = 1; // nicht noetig
|
||||
}
|
||||
else if ( (strcmp(*argv, "--dc") == 0) ) {
|
||||
option_dc = 1;
|
||||
}
|
||||
else if ( (strcmp(*argv, "--ch2") == 0) ) { sel_wavch = 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 if (strcmp(*argv, "--iq0") == 0) { option_iq = 1; } // differential/FM-demod
|
||||
else if (strcmp(*argv, "--iq2") == 0) { option_iq = 2; }
|
||||
else if (strcmp(*argv, "--iq3") == 0) { option_iq = 3; } // iq2==iq3
|
||||
else if (strcmp(*argv, "--json") == 0) {
|
||||
gpx->option.jsn = 1;
|
||||
gpx->option.ecc = 1;
|
||||
gpx->option.vit = 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;
|
||||
|
||||
|
||||
if (gpx->option.raw == 4) gpx->option.ecc = 1;
|
||||
|
||||
// init gpx
|
||||
memcpy(gpx->blk_rawbits, blk_syncbits, sizeof(blk_syncbits));
|
||||
memcpy(gpx->frame, frm_sync, sizeof(frm_sync));
|
||||
gpx->frm_pos = 0; // ecc_blk <-> frm_blk
|
||||
gpx->sf = 0;
|
||||
|
||||
gpx->option.inv = option_inv; // irrelevant
|
||||
|
||||
if (option_iq) sel_wavch = 0;
|
||||
|
||||
pcm.sel_ch = sel_wavch;
|
||||
k = read_wav_header(&pcm, fp);
|
||||
if ( k < 0 ) {
|
||||
fclose(fp);
|
||||
fprintf(stderr, "error: wav header\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
symlen = 1;
|
||||
|
||||
// init dsp
|
||||
//
|
||||
dsp.fp = fp;
|
||||
dsp.sr = pcm.sr;
|
||||
dsp.bps = pcm.bps;
|
||||
dsp.nch = pcm.nch;
|
||||
dsp.ch = pcm.sel_ch;
|
||||
dsp.br = (float)BAUD_RATE;
|
||||
dsp.sps = (float)dsp.sr/dsp.br;
|
||||
dsp.symlen = symlen;
|
||||
dsp.symhd = 1;
|
||||
dsp._spb = dsp.sps*symlen;
|
||||
dsp.hdr = rawheader;
|
||||
dsp.hdrlen = strlen(rawheader);
|
||||
dsp.BT = 1.5; // bw/time (ISI) // 1.0..2.0
|
||||
dsp.h = 0.9; // 1.0 modulation index
|
||||
dsp.opt_iq = option_iq;
|
||||
|
||||
if ( dsp.sps < 8 ) {
|
||||
fprintf(stderr, "note: sample rate low (%.1f sps)\n", dsp.sps);
|
||||
}
|
||||
|
||||
//headerlen = dsp.hdrlen;
|
||||
|
||||
k = init_buffers(&dsp);
|
||||
if ( k < 0 ) {
|
||||
fprintf(stderr, "error: init buffers\n");
|
||||
return -1;
|
||||
};
|
||||
|
||||
|
||||
if (gpx->option.vit) {
|
||||
k = vit_initCodes(gpx);
|
||||
if (k < 0) return -1;
|
||||
}
|
||||
if (gpx->option.ecc) {
|
||||
rs_init_RS255ccsds(&gpx->RS); // bch_ecc.c
|
||||
}
|
||||
|
||||
|
||||
bitofs += shift;
|
||||
|
||||
|
||||
while ( 1 )
|
||||
{
|
||||
|
||||
header_found = find_header(&dsp, thres, 3, bitofs, option_dc);
|
||||
_mv = dsp.mv;
|
||||
|
||||
if (header_found == EOF) break;
|
||||
|
||||
// mv == correlation score
|
||||
if (_mv*(0.5-gpx->option.inv) < 0) {
|
||||
gpx->option.inv ^= 0x1; // LMS6: irrelevant
|
||||
}
|
||||
|
||||
if (header_found) {
|
||||
|
||||
bitpos = 0;
|
||||
pos = BLOCKSTART;
|
||||
|
||||
if (_mv > 0) bc = 0; else bc = 1;
|
||||
|
||||
while ( pos < RAWBITBLOCK_LEN ) {
|
||||
|
||||
bitQ = read_slbit(&dsp, &rbit, 0, bitofs, bitpos, -1, 0); // symlen=1
|
||||
|
||||
if (bitQ == EOF) { break; }
|
||||
|
||||
bit = rbit ^ (bc%2); // (c0,inv(c1))
|
||||
gpx->blk_rawbits[pos] = 0x30 + bit;
|
||||
|
||||
bc++;
|
||||
pos++;
|
||||
bitpos += 1;
|
||||
}
|
||||
|
||||
gpx->blk_rawbits[pos] = '\0';
|
||||
|
||||
proc_frame(gpx, pos);
|
||||
|
||||
if (pos < RAWBITBLOCK_LEN) break;
|
||||
|
||||
pos = BLOCKSTART;
|
||||
header_found = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
free_buffers(&dsp);
|
||||
if (gpx->vit) { free(gpx->vit); gpx->vit = NULL; }
|
||||
|
||||
fclose(fp);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
Plik diff jest za duży
Load Diff
Plik diff jest za duży
Load Diff
Plik diff jest za duży
Load Diff
Plik diff jest za duży
Load Diff
Ładowanie…
Reference in New Issue