diff --git a/demod/README.md b/demod/README.md deleted file mode 100644 index a638107..0000000 --- a/demod/README.md +++ /dev/null @@ -1,27 +0,0 @@ - -## Radiosonde decoders - -alternative decoders using cross-correlation for better header-synchronization - -#### Files - - * `demod_dft.c`, `demod_dft.h`,
- `rs41dm_dft.c`, `rs92dm_dft.c`, `dfm09dm_dft.c`, `m10dm_dft.c`, `lms6dm_dft.c`,
- `RS/ecc/bch_ecc.c` - -#### Compile - (copy `bch_ecc.c`)
- `gcc -c demod_dft.c`
- `gcc rs41dm_dft.c demod_dft.o -lm -o rs41dm_dft`
- `gcc dfm09dm_dft.c demod_dft.o -lm -o dfm09dm_dft`
- `gcc m10dm_dft.c demod_dft.o -lm -o m10dm_dft`
- `gcc lms6dm_dft.c demod_dft.o -lm -o lms6dm_dft`
- `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 `
- `./dfm09dm_dft --ecc -v --ptu ` (add `-i` for dfm06)
- `./m10dm_dft --dc -vv --ptu -c `
- `./lms6dm_dft --vit --ecc -v `
- - diff --git a/demod/demod.c b/demod/demod.c deleted file mode 100644 index 925ebd4..0000000 --- a/demod/demod.c +++ /dev/null @@ -1,434 +0,0 @@ - -/* - * sync header: correlation/matched filter - * compile: - * gcc -c demod.c - * - * author: zilog80 - */ - -/* ------------------------------------------------------------------------------------ */ - -#include -#include -#include -#include - - -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; -} - diff --git a/demod/demod.h b/demod/demod.h deleted file mode 100644 index f0d7175..0000000 --- a/demod/demod.h +++ /dev/null @@ -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); - diff --git a/demod/demod_dft.c b/demod/demod_dft.c deleted file mode 100644 index 435ac84..0000000 --- a/demod/demod_dft.c +++ /dev/null @@ -1,695 +0,0 @@ - -/* - * sync header: correlation/matched filter - * compile: - * gcc -c demod_dft.c - * - * author: zilog80 - */ - -/* ------------------------------------------------------------------------------------ */ - -#include -#include -#include -#include -#include - - -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; -} - diff --git a/demod/demod_dft.h b/demod/demod_dft.h deleted file mode 100644 index 923e4ef..0000000 --- a/demod/demod_dft.h +++ /dev/null @@ -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); - diff --git a/demod/demod_iq.c b/demod/demod_iq.c deleted file mode 100644 index 03b660d..0000000 --- a/demod/demod_iq.c +++ /dev/null @@ -1,773 +0,0 @@ - -/* - * sync header: correlation/matched filter - * compile: - * gcc -c demod_iq.c - * - * author: zilog80 - */ - -/* ------------------------------------------------------------------------------------ */ - -#include -#include -#include - -#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|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; -} - diff --git a/demod/demod_iq.h b/demod/demod_iq.h deleted file mode 100644 index 605a931..0000000 --- a/demod/demod_iq.h +++ /dev/null @@ -1,115 +0,0 @@ - - -#include -#include - -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 *); - diff --git a/demod/dfm09dm.c b/demod/dfm09dm.c deleted file mode 100644 index 3de08f0..0000000 --- a/demod/dfm09dm.c +++ /dev/null @@ -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 -#include -#include -#include - -#ifdef CYGWIN - #include // cygwin: _setmode() - #include -#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<> 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< 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 (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; -} - diff --git a/demod/dfm09dm_dft.c b/demod/dfm09dm_dft.c deleted file mode 100644 index abd4210..0000000 --- a/demod/dfm09dm_dft.c +++ /dev/null @@ -1,1038 +0,0 @@ - -/* - * dfm09 (dfm06) - * sync header: correlation/matched filter - * files: dfm09dm_dft.c demod_dft.h demod_dft.c - * compile: - * gcc -c demod_dft.c - * gcc dfm09dm_dft.c demod_dft.o -lm -o dfm09dm_dft - * - * author: zilog80 - */ - -#include -#include -#include -#include - -#ifdef CYGWIN - #include // cygwin: _setmode() - #include -#endif - - -typedef unsigned char ui8_t; -typedef unsigned int ui32_t; - -//#include "demod_dft.c" -#include "demod_dft.h" - - -typedef struct { - int frnr; - int sonde_typ; - ui32_t SN6; - ui32_t SN; - 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]; - float _frmcnt; - char sonde_id[16]; // "ID__:xxxxxxxx\0\0" -} gpx_t; - -gpx_t gpx; - -typedef struct { - int ec; - float ts; -} pcksts_t; - -pcksts_t pck[9]; - -char dat_str[9][13+1]; - -// JSON Buffer to store sonde ID -char json_sonde_id[] = "DFMxx-xxxxxxxx\0\0"; - -int option_verbose = 0, // ausfuehrliche Anzeige - option_raw = 0, // rohe Frames - option_inv = 0, // invertiert Signal - option_auto = 0, - option_dist = 0, // continuous pcks 0..8 - option_ecc = 0, - option_ptu = 0, - option_ths = 0, - option_json = 0, // JSON blob output (for auto_rx) - wavloaded = 0; -int wav_channel = 0; // audio channel: left - -int ptu_out = 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 ecc = 0, ret = 0; // L = 7, 13 - for (i = 0; i < L; i++) { // L * 2 nibble (data+parity) - if (option_ecc) { - ecc = check(ham+B*i); - if (ecc > 0) ret |= (1<= 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'; - - pck[fr_id].ts = gpx._frmcnt; // time_stamp,frame_count,... - if (option_ecc) { - pck[fr_id].ec = ec; // option_ecc laesst -1 garnicht durch - if (ec > 0) { - ui8_t ecn = 0; - for (i = 0; i < 15; i++) { - if ( (ec>>i)&1 ) ecn++; - } - pck[fr_id].ec = ecn; - if ((option_dist || option_json) && ecn > 4) pck[fr_id].ec = -2; // threshold: #errors > 4 - } - } - } - - if (fr_id == 0) { - start = 0x1000; - 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); // UTC (= GPS - 18sec ab 1.1.2017) - 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) { // sat data - } - - if (fr_id == 7) { // sat data - } - - 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<> 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< 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 && ptu_out && option_verbose == 3) { - 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 ec) { - int ret = 0; - int val; - ui8_t conf_id; - ui8_t hl; - ui32_t SN6, SN; - static int chAbit, chA[2]; - static int chCbit, chC[2]; - static int chDbit, chD[2]; - static int ch7bit, ch7[2]; - static ui32_t SN_A, SN_C, SN_D, SN_7; - static ui8_t max_ch; - static ui8_t nul_ch; - static ui8_t sn2_ch, sn_ch; - static ui32_t SN_X; - static int chXbit, chX[2]; - static ui8_t dfm6typ; - - conf_id = bits2val(conf_bits, 4); - - if (conf_id > 4 && bits2val(conf_bits+8, 4*5) == 0) nul_ch = bits2val(conf_bits, 8); - - dfm6typ = ((nul_ch & 0xF0)==0x50) && (nul_ch & 0x0F); - if (dfm6typ) ptu_out = 6; - if (dfm6typ && (gpx.sonde_typ & 0xF) > 6) - { // reset if 0x5A, 0x5B (DFM-06) - gpx.sonde_typ = 0; - max_ch = conf_id; - } - - if (conf_id > 4 && conf_id > max_ch) max_ch = conf_id; // mind. 5 Kanaele // reset? lower 0xsCaaaab? - - if (conf_id > 4 && conf_id == (nul_ch>>4)+1) - { - sn2_ch = bits2val(conf_bits, 8); - - if (option_auto) - { - sn_ch = ((sn2_ch>>4) & 0xF); - if (conf_id == sn_ch) - { - if ( (nul_ch & 0x58) == 0x58 ) { // 0x5A, 0x5B - SN6 = bits2val(conf_bits+4, 4*6); // DFM-06: Kanal 6 - if (SN6 == gpx.SN6 && SN6 != 0) { // nur Nibble-Werte 0..9 - gpx.sonde_typ = SNbit | 6; - ptu_out = 6; - sprintf(gpx.sonde_id, "ID06:%6X", gpx.SN6); - sprintf(json_sonde_id, "DFM06-%6X", gpx.SN6); - } - else { // reset - gpx.sonde_typ = 0; - sprintf(json_sonde_id, "DFMxx-xxxxxxxx"); //json_sonde_id[0] = '\0'; - } - gpx.SN6 = SN6; - } - else if ( (sn2_ch & 0xF) == 0xC // 0xsCaaaab, s==sn_ch , s: 0xA=DFM-09 , 0xC=DFM-17? 0xD=? - || (sn2_ch & 0xF) == 0x0 ) // 0xs0aaaab, s==sn_ch , s: 0x7,0x8: pilotsonde PS-15? - { - val = bits2val(conf_bits+8, 4*5); - hl = (val & 1); - chX[hl] = (val >> 4) & 0xFFFF; - chXbit |= 1 << hl; - if (chXbit == 3) { - SN = (chX[0] << 16) | chX[1]; - if ( SN == SN_X || SN_X == 0 ) { - - gpx.sonde_typ = SNbit | sn_ch; - gpx.SN = SN; - - if (sn_ch == 0xA /*&& (sn2_ch & 0xF) == 0xC*/) ptu_out = 9; else ptu_out = 0; - // PS-15 ? (sn2_ch & 0xF) == 0x0 : ptu_out = 0 - // DFM-17? (sn_ch == 0xC) ptu_out = 9 ? // test 0xD ...? - - if ( (gpx.sonde_typ & 0xF) == 0xA) { - sprintf(gpx.sonde_id, "ID09:%6u", gpx.SN); - sprintf(json_sonde_id, "DFM09-%6u", gpx.SN); - } - else { - sprintf(gpx.sonde_id, "ID-%1X:%6u", gpx.sonde_typ & 0xF, gpx.SN); - sprintf(json_sonde_id, "DFMx%1X-%6u", gpx.sonde_typ & 0xF,gpx.SN); - } - } - else { // reset - gpx.sonde_typ = 0; - sprintf(json_sonde_id, "DFMxx-xxxxxxxx"); //json_sonde_id[0] = '\0'; - } - SN_X = SN; - chXbit = 0; - } - } - ret = (gpx.sonde_typ & 0xF); - } - } - } - - if (option_auto == 0) { - - // gibt es Kanaele > 6 (2-teilige ID)? - // if (conf_id > 6) gpx.SN6 = 0; // -> DFM-09,PS-15 // SNbit? - // - // SN/ID immer im letzten Kanal? davor xy00000-Kanal? (mind. 1) - if ((gpx.sonde_typ & 0xF) < 7 && conf_id == 6) { - SN6 = bits2val(conf_bits+4, 4*6); // DFM-06: Kanal 6 - if (SN6 == gpx.SN6 && SN6 != 0) { // nur Nibble-Werte 0..9 - gpx.sonde_typ = SNbit | 6; - ptu_out = 6; - ret = 6; - sprintf(gpx.sonde_id, "ID06:%6X", gpx.SN6); - sprintf(json_sonde_id, "DFM06-%6X", gpx.SN6); - } - else { - gpx.sonde_typ = 0; - } - gpx.SN6 = SN6; - } - if (conf_id == 0xA) { // 0xACxxxxy , DFM-09 - val = bits2val(conf_bits+8, 4*5); - hl = (val & 1); // val&0xF 0,1? - chA[hl] = (val >> 4) & 0xFFFF; - chAbit |= 1 << hl; - if (chAbit == 3) { // DFM-09: Kanal A - SN = (chA[0] << 16) | chA[1]; - if ( SN == SN_A ) { - gpx.sonde_typ = SNbit | 0xA; - gpx.SN = SN; - ptu_out = 9; - ret = 9; - sprintf(gpx.sonde_id, "ID09:%6u", gpx.SN); - sprintf(json_sonde_id, "DFM09-%6u", gpx.SN); - } - else { - gpx.sonde_typ = 0; - } - SN_A = SN; - chAbit = 0; - } - } - if (conf_id == 0xC) { // 0xCCxxxxy , DFM-17? - val = bits2val(conf_bits+8, 4*5); - hl = (val & 1); - chC[hl] = (val >> 4) & 0xFFFF; - chCbit |= 1 << hl; - if (chCbit == 3) { // DFM-17? Kanal C - SN = (chC[0] << 16) | chC[1]; - if ( SN == SN_C ) { - gpx.sonde_typ = SNbit | 0xC; - gpx.SN = SN; - ptu_out = 9; // ? - ret = 17; - sprintf(gpx.sonde_id, "ID-%1X:%6u", gpx.sonde_typ & 0xF, gpx.SN); - sprintf(json_sonde_id, "DFM17-%6u", gpx.SN); - } - else { - gpx.sonde_typ = 0; - } - SN_C = SN; - chCbit = 0; - } - } - if (conf_id == 0xD) { // 0xDCxxxxy , DFM-17? - val = bits2val(conf_bits+8, 4*5); - hl = (val & 1); - chD[hl] = (val >> 4) & 0xFFFF; - chDbit |= 1 << hl; - if (chDbit == 3) { // DFM-17? Kanal D - SN = (chD[0] << 16) | chD[1]; - if ( SN == SN_D ) { - gpx.sonde_typ = SNbit | 0xD; - gpx.SN = SN; - ptu_out = 0; // ... - ret = 18; - sprintf(gpx.sonde_id, "ID-%1X:%6u", gpx.sonde_typ & 0xF, gpx.SN); - sprintf(json_sonde_id, "DFM17-%6u", gpx.SN); - } - else { - gpx.sonde_typ = 0; - } - SN_D = SN; - chDbit = 0; - } - } - if (conf_id == 0x7) { // 0x70xxxxy , pilotsonde PS-15? - val = bits2val(conf_bits+8, 4*5); - hl = (val & 1); - ch7[hl] = (val >> 4) & 0xFFFF; - ch7bit |= 1 << hl; - if (ch7bit == 3) { // PS-15: Kanal 7 - SN = (ch7[0] << 16) | ch7[1]; - if ( SN == SN_7 ) { - gpx.sonde_typ = SNbit | 0x7; - gpx.SN = SN; - ptu_out = 0; - ret = 15; - sprintf(gpx.sonde_id, "ID15:%6u", gpx.SN); - sprintf(json_sonde_id, "DFM15-%6u", gpx.SN); - } - else { - gpx.sonde_typ = 0; - } - SN_7 = SN; - ch7bit = 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 & 0xF) == 0xA) { // 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; - int contgps = 0; - int output = 0; - int jsonout = 0; - - - output |= start; - - if (option_json && start == 0) { // JSON: initial reset - sprintf(json_sonde_id, "DFMxx-xxxxxxxx"); //json_sonde_id[0] = '\0'; - } - - - for (i = 0; i < 9/*8*/; i++) { // trigger: pck8 - if ( !( (option_dist || option_json) && pck[i].ec < 0) ) - { - if (pck[8].ts - pck[i].ts < 6.0) { output |= (1<= 2) 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); - if (option_verbose >= 2 && option_ecc) printf("(%1X,%1X,%1X) ", pck[0].ec&0xF, pck[8].ec&0xF, pck[1].ec&0xF); - printf(" "); - printf(" lat: %.5f ", gpx.lat); if (option_verbose >= 2 && option_ecc) printf("(%1X) ", pck[2].ec&0xF); - printf(" lon: %.5f ", gpx.lon); if (option_verbose >= 2 && option_ecc) printf("(%1X) ", pck[3].ec&0xF); - printf(" alt: %.1f ", gpx.alt); if (option_verbose >= 2 && option_ecc) printf("(%1X) ", pck[4].ec&0xF); - printf(" vH: %5.2f ", gpx.horiV); - printf(" D: %5.1f ", gpx.dir); - printf(" vV: %5.2f ", gpx.vertV); - if (option_ptu && ptu_out) { - float t = get_Temp(gpx.meas24); - if (t > -270.0) printf(" T=%.1fC ", t); - if (option_verbose == 3) { - 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 == 3 && (gpx.sonde_typ & 0xF) == 0xA) { - printf(" U: %.2fV ", gpx.status[0]); - printf(" Ti: %.1fK ", gpx.status[1]); - } - if (option_verbose) - { - if (gpx.sonde_typ & SNbit) { - printf(" (%s) ", gpx.sonde_id); - gpx.sonde_typ ^= SNbit; - } - } - } - printf("\n"); - - if (option_json && jsonout) - { - // Print JSON blob // valid sonde_ID? - printf("{ \"frame\": %d, \"id\": \"%s\", \"datetime\": \"%04d-%02d-%02dT%02d:%02d:%06.3fZ\", \"lat\": %.5f, \"lon\": %.5f, \"alt\": %.5f, \"vel_h\": %.5f, \"heading\": %.5f, \"vel_v\": %.5f", - gpx.frnr, json_sonde_id, gpx.jahr, gpx.monat, gpx.tag, gpx.std, gpx.min, gpx.sek, gpx.lat, gpx.lon, gpx.alt, gpx.horiV, gpx.dir, gpx.vertV); - if (ptu_out) { // get temperature - float t = get_Temp(gpx.meas24); // ecc-valid temperature? - if (t > -270.0) printf(", \"temp\": %.1f", t); - } - printf(" }\n"); - printf("\n"); - } - - } - - for (i = 0; i < 9; i++) pck[i].ec = -1; -} - -int print_frame(float frmcnt) { - int i; - int nib = 0; - int frid = -1; - int ret0, ret1, ret2; - int ret = 0; - - gpx._frmcnt = frmcnt; - - 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, ret0); - } - if (ret1 == 0 || ret1 > 0) { - frid = dat_out(block_dat1, ret1); - if (frid == 8) print_gpx(); - } - if (ret2 == 0 || ret2 > 0) { - frid = dat_out(block_dat2, ret2); - if (frid == 8) print_gpx(); - } - - } - else { - - conf_out(block_conf, ret0); - frid = dat_out(block_dat1, ret1); - if (frid == 8) print_gpx(); - frid = dat_out(block_dat2, ret2); - 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 - - int k, K; - float mv; - unsigned int mv_pos, mv0_pos; - int mp = 0; - - float frm_cnt = 0.0; - - float thres = 0.65; - - int symlen = 2; - int bitofs = 2; // +1 .. +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, -vv\n"); - fprintf(stderr, " -r, --raw\n"); - fprintf(stderr, " -i, --invert\n"); - fprintf(stderr, " --ecc (Hamming ECC)\n"); - fprintf(stderr, " --ths (peak threshold; default=%.1f)\n", thres); - fprintf(stderr, " --json (JSON output)\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, "-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; - //ptu_out = 1; // force ptu (non PS-15) - } - else if ( (strcmp(*argv, "--auto") == 0) ) { option_auto = 1; } - else if ( (strcmp(*argv, "--dist") == 0) ) { option_dist = 1; option_ecc = 1; } - else if ( (strcmp(*argv, "--json") == 0) ) { option_json = 1; option_ecc = 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"); - } - - - for (k = 0; k < 9; k++) pck[k].ec = -1; // init ecc-status - - - symlen = 2; - bitofs += shift; - - headerlen = strlen(rawheader); - K = init_buffers(rawheader, headerlen, 0); // 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); - if (option_auto == 0 && mv < 0) mv = 0; - k = 0; - } - else { - mv = 0.0; - continue; - } - - if ( mp > 0 && (mv > thres || mv < -thres)) { - - if (mv_pos > mv0_pos) { - - header_found = 0; - herrs = headcmp(symlen, rawheader, headerlen, mv_pos, mv<0, 0); // symlen=2 - herr1 = 0; - if (herrs <= 3 && herrs > 0) { - herr1 = headcmp(symlen, 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 (mv < 0) header_found = -header_found; - } - - if (header_found < 0) { - // read_sbit(option_inv) buffer reset? - if (option_auto) option_inv ^= 0x1; - else header_found = 0; - } - - if (header_found) { - - bitpos = 0; - pos = headerlen; - pos /= 2; - - //if (fabs(mv) > 0.85) nfrms = 8; else nfrms = 4; // test OK/KO/NO count - - frm = 0; - while ( frm < nfrms ) { // nfrms=1,2,4,8 - frm_cnt = mv_pos/(spb*2.0*BITFRAME_LEN) + frm; - while ( pos < BITFRAME_LEN ) { - bitQ = read_sbit(fp, symlen, &bit, option_inv, bitofs, bitpos==0); // symlen=2 - if (bitQ == EOF) { frm = nfrms; break; } - frame_bits[pos] = 0x30 + bit; - pos++; - bitpos += 1; - } - frame_bits[pos] = '\0'; - ret = print_frame(frm_cnt); - 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; -} - diff --git a/demod/lms6dm_dft.c b/demod/lms6dm_dft.c deleted file mode 100644 index 6d92335..0000000 --- a/demod/lms6dm_dft.c +++ /dev/null @@ -1,1078 +0,0 @@ - -/* - * LMS6 - * (403 MHz) - * - * sync header: correlation/matched filter - * files: lms6dm_dft.c demod_dft.h demod_dft.c bch_ecc.c - * compile: - * gcc -c demod_dft.c - * gcc lms6dm_dft.c demod_dft.o -lm -o lms6dm_dft - * usage: - * ./lms6dm_dft -v --vit --ecc - * - * author: zilog80 - */ - -#include -#include -#include -#include - -#ifdef CYGWIN - #include // cygwin: _setmode() - #include -#endif - - -typedef unsigned char ui8_t; -typedef unsigned short ui16_t; -typedef unsigned int ui32_t; - -#include "demod_dft.h" - -#include "bch_ecc.c" // RS/ecc/ - - -int option_verbose = 0, // ausfuehrliche Anzeige - option_raw = 0, // rohe Frames - option_ecc = 0, - option_vit = 0, - option_inv = 0, // invertiert Signal - option_dc = 0, - wavloaded = 0; -int wav_channel = 0; // audio channel: left - - -/* -------------------------------------------------------------------------- */ - -#define BAUD_RATE 4800 - -#define BITS 8 -#define HEADOFS 0 //16 -#define HEADLEN ((4*16)-HEADOFS) - -char rawheader[] = "0101011000001000""0001110010010111""0001101010100111""0011110100111110"; // (c0,inv(c1)) -// (00) 58 f3 3f b8 -//char header[] = "0000001101011101""0100100111000010""0100111111110010""0110100001101011"; // (c0,c1) -ui8_t rs_sync[] = { 0x00, 0x58, 0xf3, 0x3f, 0xb8}; -// 0x58f33fb8 little-endian <-> 0x1ACFFC1D big-endian bytes - -#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) - -// (00) 58 f3 3f b8 -char blk_rawbits[RAWBITBLOCK_LEN+SYNC_LEN*BITS*2 +8] = "0000000000000000""0000001101011101""0100100111000010""0100111111110010""0110100001101011"; -//char *block_rawbits = blk_rawbits+SYNC_LEN*BITS*2; - -float soft_rawbits[RAWBITBLOCK_LEN+SYNC_LEN*BITS*2 +8] = - { -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, - -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 1.0, 1.0, 1.0, -1.0, 1.0, - -1.0, 1.0, -1.0, -1.0, 1.0, -1.0, -1.0, 1.0, 1.0, 1.0, -1.0, -1.0, -1.0, -1.0, 1.0, -1.0, - -1.0, 1.0, -1.0, -1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, -1.0, - -1.0, 1.0, 1.0, -1.0, 1.0, -1.0, -1.0, -1.0, -1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 1.0, 1.0 }; - -ui8_t block_bytes[BLOCK_LEN+8]; - - -ui8_t frm_sync[] = { 0x24, 0x54, 0x00, 0x00}; -ui8_t frame[FRM_LEN] = { 0x24, 0x54, 0x00, 0x00}; // dataheader - -ui8_t *p_frame = frame; - - -#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 - - -char frame_bits[BITFRAME_LEN+OVERLAP*BITS +8]; // init L-1 bits mit 0 - -#define L 7 // d_f=10 - char polyA[] = "1001111"; // 0x4f: x^6+x^3+x^2+x+1 - 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 -*/ - -char vit_rawbits[RAWBITFRAME_LEN+OVERLAP*BITS*2 +8]; -char vits_rawbits[RAWBITFRAME_LEN+OVERLAP*BITS*2 +8]; -char vits_bits[BITFRAME_LEN+OVERLAP*BITS +8]; - -#define N (1 << L) -#define M (1 << (L-1)) - -typedef struct { - ui8_t bIn; - ui8_t codeIn; - int w; - int prevState; - float sw; -} states_t; - -states_t vit_state[RAWBITFRAME_LEN+OVERLAP +8][M]; - -states_t vit_d[N]; - -ui8_t vit_code[N]; - - -int vit_initCodes() { - int cA, cB; - int i, bits; - - 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; - } - - return 0; -} - -int vit_dist(int c, char *rc) { - return (((c>>1)^rc[0])&1) + ((c^rc[1])&1); -} - -int vit_start(char *rc) { - int t, m, j, c, d; - - t = L-1; - m = M; - while ( t > 0 ) { // t=0..L-2: nextState 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; -} - -int viterbi(char *rc) { - int t, tmax; - int j, j_min, w_min; - - vit_start(rc); - - tmax = strlen(rc)/2; - - for (t = L-1; t < tmax; t++) - { - vit_next(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(j_min, tmax); - - return 0; -} - - -float vits_dist(int c, float *rc) { - int bit0 = ((c>>1)&1) * 2 - 1; - int bit1 = (c&1) * 2 - 1; - return sqrt( (bit0-rc[0])*(bit0-rc[0]) + (bit1-rc[1])*(bit1-rc[1]) ); -} - -int vits_start(float *rc) { - int t, m, j, c; - float d; - - t = L-1; - m = M; - while ( t > 0 ) { // t=0..L-2: nextState 0) { - dec = vit_state[t][j].bIn; - vits_bits[t-1] = 0x30 + dec; - c = vit_state[t][j].codeIn; - vits_rawbits[2*t -2] = 0x30 + ((c>>1) & 1); - vits_rawbits[2*t -1] = 0x30 + (c & 1); - j = vit_state[t][j].prevState; - t--; - } - - return 0; -} - -int viterbi_soft(float *rc, int len) { - int t, tmax; - int j, j_min; - float sw_min; - - vits_start(rc); - - tmax = len/2; - - for (t = L-1; t < tmax; t++) - { - vits_next(t, rc+2*t); - } - - sw_min = -1.0; - for (j = 0; j < M; j++) { - if (sw_min < 0.0) { - sw_min = vit_state[tmax][j].sw; - j_min = j; - } - if (vit_state[tmax][j].sw < sw_min) { - sw_min = vit_state[tmax][j].sw; - j_min = j; - } - } - vits_path(j_min, tmax); - - return 0; -} - -// ------------------------------------------------------------------------ - -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; -} - -// ------------------------------------------------------------------------ - -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; -} - -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 -} - -// ------------------------------------------------------------------------ - -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; -} - -/* -------------------------------------------------------------------------- */ - - -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 h; - double vH; double vD; double vV; - double vE; double vN; double vU; - //int freq; -} gpx_t; - -gpx_t gpx; - -gpx_t gpx0 = { 0 }; - - -#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 - - -int get_SondeSN() { - unsigned byte; - - byte = (p_frame[pos_SondeSN]<<24) | (p_frame[pos_SondeSN+1]<<16) - | (p_frame[pos_SondeSN+2]<<8) | p_frame[pos_SondeSN+3]; - gpx.sn = byte & 0xFFFFFF; - - return 0; -} - -int get_FrameNb() { - int i; - unsigned byte; - ui8_t frnr_bytes[2]; - int frnr; - - gpx = gpx0; - - for (i = 0; i < 2; i++) { - byte = p_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"}; -//char weekday[7][4] = { "Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat"}; - -int get_GPStime() { - int i; - unsigned byte; - ui8_t gpstime_bytes[4]; - int gpstime = 0, // 32bit - day; - float ms; - - for (i = 0; i < 4; i++) { - byte = p_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; -} - -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 = p_frame[pos_GPSlat + i]; - if (byte > 0xFF) return -1; - 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; -} - -int get_GPSlon() { - int i; - unsigned byte; - ui8_t gpslon_bytes[4]; - int gpslon; - double lon; - - for (i = 0; i < 4; i++) { - byte = p_frame[pos_GPSlon + i]; - if (byte > 0xFF) return -1; - 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; -} - -int get_GPSalt() { - int i; - unsigned byte; - ui8_t gpsheight_bytes[4]; - int gpsheight; - double height; - - for (i = 0; i < 4; i++) { - byte = p_frame[pos_GPSalt + i]; - if (byte > 0xFF) return -1; - gpsheight_bytes[i] = byte; - } - - gpsheight = 0; - for (i = 0; i < 4; i++) { - gpsheight |= gpsheight_bytes[i] << (8*(3-i)); - } - height = gpsheight / 1000.0; - gpx.h = height; - - if (height < -100 || height > 60000) return -1; - return 0; -} - -int get_GPSvel24() { - int i; - unsigned byte; - ui8_t gpsVel_bytes[3]; - int vel24; - double vx, vy, vz, dir; //, alpha; - - for (i = 0; i < 3; i++) { - byte = p_frame[pos_GPSvO + i]; - if (byte > 0xFF) return -1; - 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 = p_frame[pos_GPSvN + i]; - if (byte > 0xFF) return -1; - 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 = p_frame[pos_GPSvV + i]; - if (byte > 0xFF) return -1; - 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 -ui8_t rs_cw[rs_N]; - -int lms6_ecc(ui8_t *cw) { - int errors; - ui8_t err_pos[rs_R], - err_val[rs_R]; - - errors = rs_decode(cw, err_pos, err_val); - - return errors; -} - -void print_frame(int crc_err, int len) { - int err=0; - - if (p_frame[0] != 0) - { - //if ((p_frame[pos_SondeSN+1] & 0xF0) == 0x70) // ? beginnen alle SNs mit 0x7A.... bzw 80..... ? - if ( p_frame[pos_SondeSN+1] ) - { - get_FrameNb(); - get_GPStime(); - get_SondeSN(); - if (option_verbose) printf(" (%7d) ", gpx.sn); - printf(" [%5d] ", gpx.frnr); - printf("%s ", weekday[gpx.wday]); - printf("(%02d:%02d:%06.3f) ", gpx.std, gpx.min, gpx.sek); // falls Rundung auf 60s: Ueberlauf - - get_GPSlat(); - get_GPSlon(); - err = get_GPSalt(); - if (!err) { - printf(" lat: %.6f° ", gpx.lat); - printf(" lon: %.6f° ", gpx.lon); - printf(" alt: %.2fm ", gpx.h); - //if (option_verbose) - { - get_GPSvel24(); - //if (option_verbose == 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"); - } - } -} - -int blk_pos = SYNC_LEN; -int frm_pos = 0; -int sf = 0; - -void proc_frame(int len) { - - 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) blk_rawbits[len++] = '0'; - } - //if (len > RAWBITFRAME_LEN+OVERLAP*BITS*2) len = RAWBITFRAME_LEN+OVERLAP*BITS*2; - //for (i = len; i < RAWBITFRAME_LEN+OVERLAP*BITS*2; i++) frame_rawbits[i] = 0; // oder: '0' - blk_rawbits[len] = '\0'; - - flen = len / (2*BITS); - - if (option_vit == 1) { - viterbi(blk_rawbits); - rawbits = vit_rawbits; - } - else if (option_vit == 2) { - viterbi_soft(soft_rawbits, len); - rawbits = vits_rawbits; - } - else rawbits = 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 < flen; j++) block_bytes[j] = 0; - - - if (option_ecc) { - for (j = 0; j < rs_N; j++) rs_cw[rs_N-1-j] = block_bytes[SYNC_LEN+j]; - errs = lms6_ecc(rs_cw); - for (j = 0; j < rs_N; j++) block_bytes[SYNC_LEN+j] = rs_cw[rs_N-1-j]; - } - - if (option_raw == 2) { - for (i = 0; i < flen; i++) printf("%02x ", block_bytes[i]); - if (option_ecc) printf("(%d)", errs); - printf("\n"); - } - else if (option_raw == 4 && option_ecc) { - for (i = 0; i < rs_N; i++) printf("%02x", block_bytes[SYNC_LEN+i]); - printf(" (%d)", errs); - printf("\n"); - } - else if (option_raw == 8) { - if (option_vit == 1) { - for (i = 0; i < len; i++) printf("%c", vit_rawbits[i]); printf("\n"); - } - else if (option_vit == 2) { - for (i = 0; i < len; i++) printf("%c", vits_rawbits[i]); printf("\n"); - } - else { - for (i = 0; i < len; i++) printf("%c", blk_rawbits[i]); printf("\n"); - } - } - - blk_pos = SYNC_LEN; - - while ( blk_pos-SYNC_LEN < FRM_LEN ) { - - if (sf == 0) { - while ( blk_pos-SYNC_LEN < FRM_LEN ) { - sf = 0; - for (j = 0; j < 4; j++) sf += (block_bytes[blk_pos+j] == frm_sync[j]); - if (sf == 4) { - frm_pos = 0; - break; - } - blk_pos++; - } - } - - if ( sf && frm_pos < FRM_LEN ) { - frame[frm_pos] = block_bytes[blk_pos]; - frm_pos++; - blk_pos++; - } - - if (frm_pos == FRM_LEN) { - - crc_err = check_CRC(p_frame); - - if (option_raw == 1) { - for (i = 0; i < FRM_LEN; i++) printf("%02x ", p_frame[i]); - if (crc_err==0) printf(" [OK]"); else printf(" [NO]"); - printf("\n"); - } - - if (option_raw == 0) print_frame(crc_err, len); - - frm_pos = 0; - sf = 0; - } - - } - -} - - -int main(int argc, char **argv) { - - FILE *fp = NULL; - char *fpname = NULL; - float spb = 0.0; - int header_found = 0; - - int bit, rbit; - 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 = 1; - int bitofs = 1; // +1 .. +2 - int shift = 0; - - unsigned int bc = 0; - - float sb = 0.0; - float sbit = 0.0; - float level = -1.0, ll = -1.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, " --vit,--vit2 (Viterbi/soft)\n"); - fprintf(stderr, " --ecc (Reed-Solomon)\n"); - return 0; - } - else if ( (strcmp(*argv, "-v") == 0) || (strcmp(*argv, "--verbose") == 0) ) { - option_verbose = 1; - } - else if ( (strcmp(*argv, "-r") == 0) || (strcmp(*argv, "--raw") == 0) ) { - option_raw = 1; // bytes - rs_ecc_codewords - } - else if ( (strcmp(*argv, "-r0") == 0) || (strcmp(*argv, "--raw0") == 0) ) { - option_raw = 2; // bytes: sync + codewords - } - else if ( (strcmp(*argv, "-rc") == 0) || (strcmp(*argv, "--rawecc") == 0) ) { - option_raw = 4; // rs_ecc_codewords - } - else if ( (strcmp(*argv, "-R") == 0) || (strcmp(*argv, "--RAW") == 0) ) { - option_raw = 8; // rawbits - } - else if (strcmp(*argv, "--ecc" ) == 0) { option_ecc = 1; } // RS-ECC - else if (strcmp(*argv, "--vit" ) == 0) { option_vit = 1; } // viterbi-hard - else if (strcmp(*argv, "--vit2") == 0) { option_vit = 2; } // viterbi-soft - else if ( (strcmp(*argv, "-i") == 0) || (strcmp(*argv, "--invert") == 0) ) { - option_inv = 1; - } - else if ( (strcmp(*argv, "--dc") == 0) ) { - option_dc = 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 if ( (strcmp(*argv, "--level") == 0) ) { - ++argv; - if (*argv) { - ll = 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, 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"); - } - - - if (option_raw == 4) option_ecc = 1; - - if (option_vit) { - vit_initCodes(); - } - if (option_ecc) { - rs_init_RS255ccsds(); // bch_ecc.c - } - - - symlen = 1; - bitofs += shift; - - headerlen = strlen(rawheader); - K = init_buffers(rawheader, headerlen, 2); // shape=2 (alt. shape=1) - if ( K < 0 ) { - fprintf(stderr, "error: init buffers\n"); - return -1; - }; - - level = ll; - 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(symlen, rawheader, headerlen, mv_pos, mv<0, option_dc); // (symlen=1) - herr1 = 0; - - if (herrs <= 3 && herrs > 0) { - herr1 = headcmp(symlen, rawheader, headerlen, mv_pos+1, mv<0, option_dc); - if (herr1 < herrs) { - herrs = herr1; - herr1 = 1; - } - } - if (herrs <= 3) header_found = 1; // herrs <= 3 bitfehler in header - - if (header_found) { - - if (ll <= 0) level = header_level(rawheader, headerlen, mv_pos, mv<0) * 0.6; - - bitpos = 0; - pos = BLOCKSTART; - - if (mv > 0) bc = 0; else bc = 1; - - while ( pos < RAWBITBLOCK_LEN ) { - - //bitQ = read_sbit(fp, symlen, &rbit, option_inv, bitofs, bitpos==0); // symlen=1 - bitQ = read_softbit(fp, symlen, &rbit, &sb, level, option_inv, bitofs, bitpos==0); // symlen=1 - if (bitQ == EOF) { break; } - - bit = rbit ^ (bc%2); // (c0,inv(c1)) - blk_rawbits[pos] = 0x30 + bit; - - sbit = sb * (-(int)(bc%2)*2+1); - soft_rawbits[pos] = sbit; - - bc++; - pos++; - bitpos += 1; - } - - blk_rawbits[pos] = '\0'; - soft_rawbits[pos] = 0; - - proc_frame(pos); - - if (pos < RAWBITBLOCK_LEN) break; - - pos = BLOCKSTART; - header_found = 0; - } - } - } - - } - - - free_buffers(); - - fclose(fp); - - return 0; -} - diff --git a/demod/lmsXdm_dft.c b/demod/lmsXdm_dft.c deleted file mode 100644 index 671d9c8..0000000 --- a/demod/lmsXdm_dft.c +++ /dev/null @@ -1,1097 +0,0 @@ - -/* - * LMSx - * (403 MHz) - * - * sync header: correlation/matched filter - * files: lmsXdm_dft.c demod_dft.h demod_dft.c bch_ecc.c - * compile: - * gcc -c demod_dft.c - * gcc lmsXdm_dft.c demod_dft.o -lm -o lmsXdm_dft - * usage: - * ./lmsXdm_dft -v --vit --ecc - * - * author: zilog80 - */ - -#include -#include -#include -#include - -#ifdef CYGWIN - #include // cygwin: _setmode() - #include -#endif - - -typedef unsigned char ui8_t; -typedef unsigned short ui16_t; -typedef unsigned int ui32_t; - -#include "demod_dft.h" - -#include "bch_ecc.c" // RS/ecc/ - - -int option_verbose = 0, // ausfuehrliche Anzeige - option_raw = 0, // rohe Frames - option_ecc = 0, - option_vit = 0, - option_inv = 0, // invertiert Signal - option_dc = 0, - wavloaded = 0; -int wav_channel = 0; // audio channel: left - - -/* -------------------------------------------------------------------------- */ - -#define BAUD_RATE (4797.7) // = 4800 / (48023/48000) ? - -#define BITS 8 -#define HEADOFS 0 //16 -#define HEADLEN ((4*16)-HEADOFS) - -char rawheader[] = "0101011000001000""0001110010010111""0001101010100111""0011110100111110"; // (c0,inv(c1)) -// (00) 58 f3 3f b8 -//char header[] = "0000001101011101""0100100111000010""0100111111110010""0110100001101011"; // (c0,c1) -ui8_t rs_sync[] = { 0x00, 0x58, 0xf3, 0x3f, 0xb8}; -// 0x58f33fb8 little-endian <-> 0x1ACFFC1D big-endian bytes - -#define FRAME_LEN (300) // 4800baud, 16bits/byte -#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 RAWBITBLOCK_LEN ((300)*BITS*2) - -// (00) 58 f3 3f b8 -char blk_rawbits[RAWBITBLOCK_LEN+SYNC_LEN*BITS*2 +8] = "0000000000000000""0000001101011101""0100100111000010""0100111111110010""0110100001101011"; -//char *block_rawbits = blk_rawbits+SYNC_LEN*BITS*2; - -float soft_rawbits[RAWBITBLOCK_LEN+SYNC_LEN*BITS*2 +8] = - { -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, - -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 1.0, 1.0, 1.0, -1.0, 1.0, - -1.0, 1.0, -1.0, -1.0, 1.0, -1.0, -1.0, 1.0, 1.0, 1.0, -1.0, -1.0, -1.0, -1.0, 1.0, -1.0, - -1.0, 1.0, -1.0, -1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, -1.0, -1.0, 1.0, -1.0, - -1.0, 1.0, 1.0, -1.0, 1.0, -1.0, -1.0, -1.0, -1.0, 1.0, 1.0, -1.0, 1.0, -1.0, 1.0, 1.0 }; - -ui8_t block_bytes[FRAME_LEN+8]; // BLOCK_LEN + 40 - - -//ui8_t frm_sync[] = { 0x24, 0x54, 0x00, 0x00}; -ui8_t frm_sync[] = { 0x24, 0x46, 0x05, 0x00}; -ui8_t frame[FRM_LEN] = { 0x24, 0x54, 0x00, 0x00}; // dataheader - -ui8_t *p_frame = frame; - - -#define BITFRAME_LEN (FRAME_LEN*BITS) -#define RAWBITFRAME_LEN (BITFRAME_LEN*2) -#define OVERLAP 64 -#define OFS 4 - - -char frame_bits[BITFRAME_LEN+OVERLAP*BITS +8]; // init L-1 bits mit 0 - -#define L 7 // d_f=10 - char polyA[] = "1001111"; // 0x4f: x^6+x^3+x^2+x+1 - 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 -*/ - -char vit_rawbits[RAWBITFRAME_LEN+OVERLAP*BITS*2 +8]; -char vits_rawbits[RAWBITFRAME_LEN+OVERLAP*BITS*2 +8]; -char vits_bits[BITFRAME_LEN+OVERLAP*BITS +8]; - -#define N (1 << L) -#define M (1 << (L-1)) - -typedef struct { - ui8_t bIn; - ui8_t codeIn; - int w; - int prevState; - float sw; -} states_t; - -states_t vit_state[RAWBITFRAME_LEN+OVERLAP +8][M]; - -states_t vit_d[N]; - -ui8_t vit_code[N]; - - -int vit_initCodes() { - int cA, cB; - int i, bits; - - 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; - } - - return 0; -} - -int vit_dist(int c, char *rc) { - return (((c>>1)^rc[0])&1) + ((c^rc[1])&1); -} - -int vit_start(char *rc) { - int t, m, j, c, d; - - t = L-1; - m = M; - while ( t > 0 ) { // t=0..L-2: nextState 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; -} - -int viterbi(char *rc) { - int t, tmax; - int j, j_min, w_min; - - vit_start(rc); - - tmax = strlen(rc)/2; - - for (t = L-1; t < tmax; t++) - { - vit_next(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(j_min, tmax); - - return 0; -} - - -float vits_dist(int c, float *rc) { - int bit0 = ((c>>1)&1) * 2 - 1; - int bit1 = (c&1) * 2 - 1; - return sqrt( (bit0-rc[0])*(bit0-rc[0]) + (bit1-rc[1])*(bit1-rc[1]) ); -} - -int vits_start(float *rc) { - int t, m, j, c; - float d; - - t = L-1; - m = M; - while ( t > 0 ) { // t=0..L-2: nextState 0) { - dec = vit_state[t][j].bIn; - vits_bits[t-1] = 0x30 + dec; - c = vit_state[t][j].codeIn; - vits_rawbits[2*t -2] = 0x30 + ((c>>1) & 1); - vits_rawbits[2*t -1] = 0x30 + (c & 1); - j = vit_state[t][j].prevState; - t--; - } - - return 0; -} - -int viterbi_soft(float *rc, int len) { - int t, tmax; - int j, j_min; - float sw_min; - - vits_start(rc); - - tmax = len/2; - - for (t = L-1; t < tmax; t++) - { - vits_next(t, rc+2*t); - } - - sw_min = -1.0; - for (j = 0; j < M; j++) { - if (sw_min < 0.0) { - sw_min = vit_state[tmax][j].sw; - j_min = j; - } - if (vit_state[tmax][j].sw < sw_min) { - sw_min = vit_state[tmax][j].sw; - j_min = j; - } - } - vits_path(j_min, tmax); - - return 0; -} - -// ------------------------------------------------------------------------ - -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; -} - -// ------------------------------------------------------------------------ - -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; -} - -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 -} - -// ------------------------------------------------------------------------ - -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; -} - -/* -------------------------------------------------------------------------- */ - - -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 h; - double vH; double vD; double vV; - double vE; double vN; double vU; - //int freq; -} gpx_t; - -gpx_t gpx; - -gpx_t gpx0 = { 0 }; - - -#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) // 2 byte -#define pos_GPSvN (OFS+0x1C) // 2 byte -#define pos_GPSvV (OFS+0x1E) // 2 byte - - -int get_SondeSN() { - unsigned byte; - - byte = (p_frame[pos_SondeSN]<<24) | (p_frame[pos_SondeSN+1]<<16) - | (p_frame[pos_SondeSN+2]<<8) | p_frame[pos_SondeSN+3]; - gpx.sn = byte & 0xFFFFFF; - - return 0; -} - -int get_FrameNb() { - int i; - unsigned byte; - ui8_t frnr_bytes[2]; - int frnr; - - gpx = gpx0; - - for (i = 0; i < 2; i++) { - byte = p_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"}; -//char weekday[7][4] = { "Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat"}; - -int get_GPStime() { - int i; - unsigned byte; - ui8_t gpstime_bytes[4]; - int gpstime = 0, // 32bit - day; - float ms; - - for (i = 0; i < 4; i++) { - byte = p_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; -} - -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 = p_frame[pos_GPSlat + i]; - if (byte > 0xFF) return -1; - gpslat_bytes[i] = byte; - } - - gpslat = 0; - for (i = 0; i < 4; i++) { - gpslat |= gpslat_bytes[i] << (8*(3-i)); - } - lat = gpslat / 1e7; // / B60B60; - gpx.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 = p_frame[pos_GPSlon + i]; - if (byte > 0xFF) return -1; - gpslon_bytes[i] = byte; - } - - gpslon = 0; - for (i = 0; i < 4; i++) { - gpslon |= gpslon_bytes[i] << (8*(3-i)); - } - lon = gpslon / 1e7; // B60B60; - gpx.lon = lon; - - return 0; -} - -int get_GPSalt() { - int i; - unsigned byte; - ui8_t gpsheight_bytes[4]; - int gpsheight; - double height; - - for (i = 0; i < 4; i++) { - byte = p_frame[pos_GPSalt + i]; - if (byte > 0xFF) return -1; - gpsheight_bytes[i] = byte; - } - - gpsheight = 0; - for (i = 0; i < 4; i++) { - gpsheight |= gpsheight_bytes[i] << (8*(3-i)); - } - height = gpsheight / 100.0; - gpx.h = height; - - if (height < -100 || height > 60000) return -1; - return 0; -} - -int get_GPSvel24() { - int i; - unsigned byte; - ui8_t gpsVel_bytes[3]; - int vel24; - double vx, vy, vz, dir; //, alpha; - - for (i = 0; i < 3; i++) { - byte = p_frame[pos_GPSvO + i]; - if (byte > 0xFF) return -1; - 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 = p_frame[pos_GPSvN + i]; - if (byte > 0xFF) return -1; - 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 = p_frame[pos_GPSvV + i]; - if (byte > 0xFF) return -1; - 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; -} - -int get_GPSvel16() { - int i; - unsigned byte; - ui8_t gpsVel_bytes[2]; - short vel16; - double vx, vy, vz, dir; //, alpha; - - for (i = 0; i < 2; i++) { - byte = p_frame[pos_GPSvO + i]; - if (byte > 0xFF) return -1; - gpsVel_bytes[i] = byte; - } - vel16 = gpsVel_bytes[0] << 8 | gpsVel_bytes[1]; - vx = vel16 / 1e2; // ost - - for (i = 0; i < 2; i++) { - byte = p_frame[pos_GPSvN + i]; - if (byte > 0xFF) return -1; - gpsVel_bytes[i] = byte; - } - vel16 = gpsVel_bytes[0] << 8 | gpsVel_bytes[1]; - vy= vel16 / 1e2; // nord - - for (i = 0; i < 2; i++) { - byte = p_frame[pos_GPSvV + i]; - if (byte > 0xFF) return -1; - gpsVel_bytes[i] = byte; - } - vel16 = gpsVel_bytes[0] << 8 | gpsVel_bytes[1]; - vz = vel16 / 1e2; // hoch - - gpx.vH = vx; - gpx.vD = vy; - 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 -ui8_t rs_cw[rs_N]; - -int lms6_ecc(ui8_t *cw) { - int errors; - ui8_t err_pos[rs_R], - err_val[rs_R]; - - errors = rs_decode(cw, err_pos, err_val); - - return errors; -} - -void print_frame(int crc_err, int len) { - int err=0; - - if (p_frame[0] != 0) - { - //if ((p_frame[pos_SondeSN+1] & 0xF0) == 0x70) // ? beginnen alle SNs mit 0x7A.... bzw 80..... ? - if ( p_frame[pos_SondeSN+1] ) - { - get_FrameNb(); - get_GPStime(); - get_SondeSN(); - if (option_verbose) printf(" (%7d) ", gpx.sn); - printf(" [%5d] ", gpx.frnr); - // - get_GPSlat(); - get_GPSlon(); - err = get_GPSalt(); - if (!err) { - printf(" lat: %.6f° ", gpx.lat); - printf(" lon: %.6f° ", gpx.lon); - printf(" alt: %.2fm ", gpx.h); - //if (option_verbose) - { - get_GPSvel16(); - 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"); - } - } -} - -int blk_pos = SYNC_LEN; -int frm_pos = 0; -int sf = 0; - -void proc_frame(int len) { - - 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) blk_rawbits[len++] = '0'; - } - //if (len > RAWBITFRAME_LEN+OVERLAP*BITS*2) len = RAWBITFRAME_LEN+OVERLAP*BITS*2; - //for (i = len; i < RAWBITFRAME_LEN+OVERLAP*BITS*2; i++) frame_rawbits[i] = 0; // oder: '0' - blk_rawbits[len] = '\0'; - - flen = len / (2*BITS); - - if (option_vit == 1) { - viterbi(blk_rawbits); - rawbits = vit_rawbits; - } - else if (option_vit == 2) { - viterbi_soft(soft_rawbits, len); - rawbits = vits_rawbits; - } - else rawbits = 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 < flen; j++) block_bytes[j] = 0; - - sf = 0; - blk_pos = SYNC_LEN; - for (j = 0; j < 4; j++) sf += (block_bytes[SYNC_LEN+j] == frm_sync[j]); - if (sf < 4) { // scan 1..40 ? - sf = 0; - for (j = 0; j < 4; j++) sf += (block_bytes[SYNC_LEN+35+j] == frm_sync[j]); - if (sf == 4) blk_pos = SYNC_LEN+35; - else { - sf = 0; - for (j = 0; j < 4; j++) sf += (block_bytes[SYNC_LEN+40+j] == frm_sync[j]); - if (sf == 4) blk_pos = SYNC_LEN+40; // 300-260 - } - } - - if (blen > 100 && option_ecc) { - for (j = 0; j < rs_N; j++) rs_cw[rs_N-1-j] = block_bytes[blk_pos+j]; - errs = lms6_ecc(rs_cw); - for (j = 0; j < rs_N; j++) block_bytes[blk_pos+j] = rs_cw[rs_N-1-j]; - } - - if (option_raw == 2) { - for (i = 0; i < flen; i++) printf("%02x ", block_bytes[i]); - if (blen > 100 && option_ecc) printf("(%d)", errs); - printf("\n"); - } - else if (option_raw == 4 && option_ecc && blen > 100) { - for (i = 0; i < rs_N; i++) printf("%02x", block_bytes[blk_pos+i]); - printf(" (%d)", errs); - printf("\n"); - } - else if (option_raw == 8) { - if (option_vit) { - for (i = 0; i < len; i++) printf("%c", vit_rawbits[i]); printf("\n"); - } - else { - for (i = 0; i < len; i++) printf("%c", blk_rawbits[i]); printf("\n"); - } - } - - for (j = 0; j < rs_K; j++) frame[j] = block_bytes[blk_pos+j]; - - crc_err = check_CRC(p_frame); - - if (option_raw == 1) { - for (i = 0; i < FRM_LEN; i++) printf("%02x ", p_frame[i]); - if (crc_err==0) printf(" [OK]"); else printf(" [NO]"); - printf("\n"); - } - - if (option_raw == 0) print_frame(crc_err, len); - -} - - -int main(int argc, char **argv) { - - FILE *fp = NULL; - char *fpname = NULL; - float spb = 0.0; - int header_found = 0; - - int bit, rbit; - 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 = 1; - int bitofs = 1; // +1 .. +2 - int shift = 0; - - unsigned int bc = 0; - - float sb = 0.0; - float sbit = 0.0; - float level = -1.0, ll = -1.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, " --vit,--vit2 (Viterbi/soft)\n"); - fprintf(stderr, " --ecc (Reed-Solomon)\n"); - return 0; - } - else if ( (strcmp(*argv, "-v") == 0) || (strcmp(*argv, "--verbose") == 0) ) { - option_verbose = 1; - } - else if ( (strcmp(*argv, "-r") == 0) || (strcmp(*argv, "--raw") == 0) ) { - option_raw = 1; // bytes - rs_ecc_codewords - } - else if ( (strcmp(*argv, "-r0") == 0) || (strcmp(*argv, "--raw0") == 0) ) { - option_raw = 2; // bytes: sync + codewords - } - else if ( (strcmp(*argv, "-rc") == 0) || (strcmp(*argv, "--rawecc") == 0) ) { - option_raw = 4; // rs_ecc_codewords - } - else if ( (strcmp(*argv, "-R") == 0) || (strcmp(*argv, "--RAW") == 0) ) { - option_raw = 8; // rawbits - } - else if (strcmp(*argv, "--ecc" ) == 0) { option_ecc = 1; } // RS-ECC - else if (strcmp(*argv, "--vit" ) == 0) { option_vit = 1; } // viterbi-hard - else if (strcmp(*argv, "--vit2") == 0) { option_vit = 2; } // viterbi-soft - else if ( (strcmp(*argv, "-i") == 0) || (strcmp(*argv, "--invert") == 0) ) { - option_inv = 1; - } - else if ( (strcmp(*argv, "--dc") == 0) ) { - option_dc = 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 if ( (strcmp(*argv, "--level") == 0) ) { - ++argv; - if (*argv) { - ll = 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, 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"); - } - - - if (option_raw == 4) option_ecc = 1; - - if (option_vit) { - vit_initCodes(); - } - if (option_ecc) { - rs_init_RS255ccsds(); // bch_ecc.c - } - - - symlen = 1; - bitofs += shift; - - headerlen = strlen(rawheader); - K = init_buffers(rawheader, headerlen, 2); // shape=2 (alt. shape=1) - if ( K < 0 ) { - fprintf(stderr, "error: init buffers\n"); - return -1; - }; - - level = ll; - 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(symlen, rawheader, headerlen, mv_pos, mv<0, option_dc); // (symlen=1) - herr1 = 0; - - if (herrs <= 3 && herrs > 0) { - herr1 = headcmp(symlen, rawheader, headerlen, mv_pos+1, mv<0, option_dc); - if (herr1 < herrs) { - herrs = herr1; - herr1 = 1; - } - } - if (herrs <= 3) header_found = 1; // herrs <= 3 bitfehler in header - - if (header_found) { - - if (ll <= 0) level = header_level(rawheader, headerlen, mv_pos, mv<0) * 0.6; - - bitpos = 0; - pos = BLOCKSTART; - - if (mv > 0) bc = 0; else bc = 1; - - while ( pos < RAWBITBLOCK_LEN ) { - header_found = !(pos>=RAWBITBLOCK_LEN-10); - //bitQ = read_sbit(fp, symlen, &rbit, option_inv, bitofs, bitpos==0); // symlen=1 - bitQ = read_softbit(fp, symlen, &rbit, &sb, level, option_inv, bitofs, bitpos==0); // symlen=1 - if (bitQ == EOF) { break; } - - bit = rbit ^ (bc%2); // (c0,inv(c1)) - blk_rawbits[pos] = 0x30 + bit; - - sbit = sb * (-(int)(bc%2)*2+1); - soft_rawbits[pos] = sbit; - - bc++; - pos++; - bitpos += 1; - } - - blk_rawbits[pos] = '\0'; - soft_rawbits[pos] = 0; - - proc_frame(pos); - - if (pos < RAWBITBLOCK_LEN) break; - - pos = BLOCKSTART; - header_found = 0; - } - } - } - - } - - - free_buffers(); - - fclose(fp); - - return 0; -} - diff --git a/demod/m10dm_dft.c b/demod/m10dm_dft.c deleted file mode 100644 index 5f96dd4..0000000 --- a/demod/m10dm_dft.c +++ /dev/null @@ -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 -#include -#include -#include - -#ifdef CYGWIN - #include // cygwin: _setmode() - #include -#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; -} - diff --git a/demod/mod/demod_mod.c b/demod/mod/demod_mod.c index 529796f..51e3538 100644 --- a/demod/mod/demod_mod.c +++ b/demod/mod/demod_mod.c @@ -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; diff --git a/demod/rs41dm.c b/demod/rs41dm.c deleted file mode 100644 index 8be534f..0000000 --- a/demod/rs41dm.c +++ /dev/null @@ -1,1245 +0,0 @@ - -/* - * rs41 - * sync header: correlation/matched filter - * files: rs41dm.c bch_ecc.c demod.h demod.c - * compile: - * gcc -c demod.c - * gcc rs41dm.c demod.o -lm -o rs41dm - * - * author: zilog80 - */ - -#include -#include -#include -#include - -#ifdef CYGWIN - #include // cygwin: _setmode() - #include -#endif - -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.c" -#include "demod.h" - -#include "bch_ecc.c" // RS/ecc/ - - -typedef struct { - int typ; - int msglen; - int msgpos; - int parpos; - int hdrlen; - int frmlen; -} rscfg_t; - -rscfg_t cfg_rs41 = { 41, (320-56)/2, 56, 8, 8, 320}; - - -typedef struct { - int frnr; - char id[9]; - int week; int gpssec; - int jahr; int monat; int tag; - int wday; - int std; int min; float sek; - double lat; double lon; double alt; - double vN; double vE; double vU; - double vH; double vD; double vD2; - float T; - ui32_t crc; -} gpx_t; - -gpx_t gpx; - -int option_verbose = 0, // ausfuehrliche Anzeige - option_raw = 0, // rohe Frames - option_inv = 0, // invertiert Signal - option_res = 0, // genauere Bitmessung - option_crc = 0, // check CRC - option_ecc = 0, // Reed-Solomon ECC - option_sat = 0, // GPS sat data - option_ptu = 0, - option_ths = 0, - wavloaded = 0; - - -#define BITS 8 -#define HEADLEN 64 -#define FRAMESTART ((HEADLEN)/BITS) - -/* 10 B6 CA 11 22 96 12 F8 */ -char header[] = "0000100001101101010100111000100001000100011010010100100000011111"; - - -#define NDATA_LEN 320 // std framelen 320 -#define XDATA_LEN 198 -#define FRAME_LEN (NDATA_LEN+XDATA_LEN) // max framelen 518 -ui8_t //xframe[FRAME_LEN] = { 0x10, 0xB6, 0xCA, 0x11, 0x22, 0x96, 0x12, 0xF8}, = xorbyte( frame) - frame[FRAME_LEN] = { 0x86, 0x35, 0xf4, 0x40, 0x93, 0xdf, 0x1a, 0x60}; // = xorbyte(xframe) - -float byteQ[FRAME_LEN]; - -#define MASK_LEN 64 -ui8_t mask[MASK_LEN] = { 0x96, 0x83, 0x3E, 0x51, 0xB1, 0x49, 0x08, 0x98, - 0x32, 0x05, 0x59, 0x0E, 0xF9, 0x44, 0xC6, 0x26, - 0x21, 0x60, 0xC2, 0xEA, 0x79, 0x5D, 0x6D, 0xA1, - 0x54, 0x69, 0x47, 0x0C, 0xDC, 0xE8, 0x5C, 0xF1, - 0xF7, 0x76, 0x82, 0x7F, 0x07, 0x99, 0xA2, 0x2C, - 0x93, 0x7C, 0x30, 0x63, 0xF5, 0x10, 0x2E, 0x61, - 0xD0, 0xBC, 0xB4, 0xB6, 0x06, 0xAA, 0xF4, 0x23, - 0x78, 0x6E, 0x3B, 0xAE, 0xBF, 0x7B, 0x4C, 0xC1}; -/* LFSR: ab i=8 (mod 64): - * m[16+i] = m[i] ^ m[i+2] ^ m[i+4] ^ m[i+6] - * ________________3205590EF944C6262160C2EA795D6DA15469470CDCE85CF1 - * F776827F0799A22C937C3063F5102E61D0BCB4B606AAF423786E3BAEBF7B4CC196833E51B1490898 - */ - -/* ------------------------------------------------------------------------------------ */ - -#define BAUD_RATE 4800 - -/* ------------------------------------------------------------------------------------ */ - - -int bits2byte(char bits[]) { - int i, byteval=0, d=1; - for (i = 0; i < 8; i++) { // little endian - /* for (i = 7; i >= 0; i--) { // big endian */ - if (bits[i] == 1) byteval += d; - else if (bits[i] == 0) byteval += 0; - else return 0x100; - d <<= 1; - } - return byteval; -} - - -/* -ui8_t xorbyte(int pos) { - return xframe[pos] ^ mask[pos % MASK_LEN]; -} -*/ -ui8_t framebyte(int pos) { - return frame[pos]; -} - - -/* ------------------------------------------------------------------------------------ */ -/* - * 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; -} -/* ------------------------------------------------------------------------------------ */ - -ui32_t u4(ui8_t *bytes) { // 32bit unsigned int - ui32_t val = 0; - memcpy(&val, bytes, 4); - return val; -} - -ui32_t u3(ui8_t *bytes) { // 24bit unsigned int - int val24 = 0; - val24 = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16); - // = memcpy(&val, bytes, 3), val &= 0x00FFFFFF; - return val24; -} - -int i3(ui8_t *bytes) { // 24bit signed int - int val = 0, - val24 = 0; - val = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16); - val24 = val & 0xFFFFFF; if (val24 & 0x800000) val24 = val24 - 0x1000000; - return val24; -} - -ui32_t u2(ui8_t *bytes) { // 16bit unsigned int - return bytes[0] | (bytes[1]<<8); -} - -/* -double r8(ui8_t *bytes) { - double val = 0; - memcpy(&val, bytes, 8); - return val; -} - -float r4(ui8_t *bytes) { - float val = 0; - memcpy(&val, bytes, 4); - return val; -} -*/ - -/* -int crc16x(int start, int len) { - int crc16poly = 0x1021; - int rem = 0xFFFF, i, j; - int xbyte; - - if (start+len+2 > FRAME_LEN) return -1; - - for (i = 0; i < len; i++) { - xbyte = xorbyte(start+i); - rem = rem ^ (xbyte << 8); - for (j = 0; j < 8; j++) { - if (rem & 0x8000) { - rem = (rem << 1) ^ crc16poly; - } - else { - rem = (rem << 1); - } - rem &= 0xFFFF; - } - } - return rem; -} -*/ -int crc16(int start, int len) { - int crc16poly = 0x1021; - int rem = 0xFFFF, i, j; - int byte; - - if (start+len+2 > FRAME_LEN) return -1; - - for (i = 0; i < len; i++) { - byte = framebyte(start+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; -} - -int check_CRC(ui32_t pos, ui32_t pck) { - ui32_t crclen = 0, - crcdat = 0; - if (((pck>>8) & 0xFF) != frame[pos]) return -1; - crclen = frame[pos+1]; - if (pos + crclen + 4 > FRAME_LEN) return -1; - crcdat = u2(frame+pos+2+crclen); - if ( crcdat != crc16(pos+2, crclen) ) { - return 1; // CRC NO - } - else return 0; // CRC OK -} - - -/* - Pos: SubHeader, 1+1 byte (ID+LEN) -0x039: 7928 FrameNumber+SondeID - +(0x050: 0732 CalFrames 0x00..0x32) -0x065: 7A2A PTU -0x093: 7C1E GPS1: RXM-RAW (0x02 0x10) Week, TOW, Sats -0x0B5: 7D59 GPS2: RXM-RAW (0x02 0x10) pseudorange, doppler -0x112: 7B15 GPS3: NAV-SOL (0x01 0x06) ECEF-POS, ECEF-VEL -0x12B: 7611 00 -0x12B: 7Exx AUX-xdata -*/ - -#define crc_FRAME (1<<0) -#define xor_FRAME 0x1713 // ^0x6E3B=0x7928 -#define pck_FRAME 0x7928 -#define pos_FRAME 0x039 -#define pos_FrameNb 0x03B // 2 byte -#define pos_SondeID 0x03D // 8 byte -#define pos_CalData 0x052 // 1 byte, counter 0x00..0x32 -#define pos_Calfreq 0x055 // 2 byte, calfr 0x00 -#define pos_Calburst 0x05E // 1 byte, calfr 0x02 -// ? #define pos_Caltimer 0x05A // 2 byte, calfr 0x02 ? -#define pos_CalRSTyp 0x05B // 8 byte, calfr 0x21 (+2 byte in 0x22?) - // weitere chars in calfr 0x22/0x23; weitere ID - -#define crc_PTU (1<<1) -#define xor_PTU 0xE388 // ^0x99A2=0x0x7A2A -#define pck_PTU 0x7A2A // PTU -#define pos_PTU 0x065 - -#define crc_GPS1 (1<<2) -#define xor_GPS1 0x9667 // ^0xEA79=0x7C1E -#define pck_GPS1 0x7C1E // RXM-RAW (0x02 0x10) -#define pos_GPS1 0x093 -#define pos_GPSweek 0x095 // 2 byte -#define pos_GPSiTOW 0x097 // 4 byte -#define pos_satsN 0x09B // 12x2 byte (1: SV, 1: quality,strength) - -#define crc_GPS2 (1<<3) -#define xor_GPS2 0xD7AD // ^0xAAF4=0x7D59 -#define pck_GPS2 0x7D59 // RXM-RAW (0x02 0x10) -#define pos_GPS2 0x0B5 -#define pos_minPR 0x0B7 // 4 byte -#define pos_FF 0x0BB // 1 byte -#define pos_dataSats 0x0BC // 12x(4+3) byte (4: pseudorange, 3: doppler) - -#define crc_GPS3 (1<<4) -#define xor_GPS3 0xB9FF // ^0xC2EA=0x7B15 -#define pck_GPS3 0x7B15 // NAV-SOL (0x01 0x06) -#define pos_GPS3 0x112 -#define pos_GPSecefX 0x114 // 4 byte -#define pos_GPSecefY 0x118 // 4 byte -#define pos_GPSecefZ 0x11C // 4 byte -#define pos_GPSecefV 0x120 // 3*2 byte -#define pos_numSats 0x126 // 1 byte -#define pos_sAcc 0x127 // 1 byte -#define pos_pDOP 0x128 // 1 byte - -#define crc_AUX (1<<5) -#define pck_AUX 0x7E00 // LEN variable -#define pos_AUX 0x12B - -#define crc_ZERO (1<<6) // LEN variable -#define pck_ZERO 0x7600 - - -ui8_t calibytes[51*16]; -ui8_t calfrchk[51]; -float Rf1, // ref-resistor f1 (750 Ohm) - Rf2, // ref-resistor f2 (1100 Ohm) - co1[3], // { -243.911 , 0.187654 , 8.2e-06 } - calT1[3], // calibration T1 - co2[3], // { -243.911 , 0.187654 , 8.2e-06 } - calT2[3]; // calibration T2-Hum - - -double c = 299.792458e6; -double L1 = 1575.42e6; - -int get_SatData() { - int i, n; - int sv; - ui32_t minPR; - int Nfix; - double pDOP, sAcc; - - fprintf(stdout, "[%d]\n", u2(frame+pos_FrameNb)); - - fprintf(stdout, "iTOW: 0x%08X", u4(frame+pos_GPSiTOW)); - fprintf(stdout, " week: 0x%04X", u2(frame+pos_GPSweek)); - fprintf(stdout, "\n"); - minPR = u4(frame+pos_minPR); - fprintf(stdout, "minPR: %d", minPR); - fprintf(stdout, "\n"); - - for (i = 0; i < 12; i++) { - n = i*7; - sv = frame[pos_satsN+2*i]; - if (sv == 0xFF) break; - fprintf(stdout, " SV: %2d # ", sv); - fprintf(stdout, "prMes: %.1f", u4(frame+pos_dataSats+n)/100.0 + minPR); - fprintf(stdout, " "); - fprintf(stdout, "doMes: %.1f", -i3(frame+pos_dataSats+n+4)/100.0*L1/c); - fprintf(stdout, "\n"); - } - - fprintf(stdout, "ECEF-POS: (%d,%d,%d)\n", - (i32_t)u4(frame+pos_GPSecefX), - (i32_t)u4(frame+pos_GPSecefY), - (i32_t)u4(frame+pos_GPSecefZ)); - fprintf(stdout, "ECEF-VEL: (%d,%d,%d)\n", - (i16_t)u2(frame+pos_GPSecefV+0), - (i16_t)u2(frame+pos_GPSecefV+2), - (i16_t)u2(frame+pos_GPSecefV+4)); - - Nfix = frame[pos_numSats]; - sAcc = frame[pos_sAcc]/10.0; - pDOP = frame[pos_pDOP]/10.0; - fprintf(stdout, "numSatsFix: %2d sAcc: %.1f pDOP: %.1f\n", Nfix, sAcc, pDOP); - - - fprintf(stdout, "CRC: "); - fprintf(stdout, " %04X", pck_GPS1); - if (check_CRC(pos_GPS1, pck_GPS1)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); - //fprintf(stdout, "[%+d]", check_CRC(pos_GPS1, pck_GPS1)); - fprintf(stdout, " %04X", pck_GPS2); - if (check_CRC(pos_GPS2, pck_GPS2)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); - //fprintf(stdout, "[%+d]", check_CRC(pos_GPS2, pck_GPS2)); - fprintf(stdout, " %04X", pck_GPS3); - if (check_CRC(pos_GPS3, pck_GPS3)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); - //fprintf(stdout, "[%+d]", check_CRC(pos_GPS3, pck_GPS3)); - - fprintf(stdout, "\n"); - fprintf(stdout, "\n"); - - return 0; -} - - -int get_FrameNb() { - int i; - unsigned byte; - ui8_t frnr_bytes[2]; - int frnr; - - for (i = 0; i < 2; i++) { - byte = framebyte(pos_FrameNb + i); - frnr_bytes[i] = byte; - } - - frnr = frnr_bytes[0] + (frnr_bytes[1] << 8); - gpx.frnr = frnr; - - return 0; -} - -int get_SondeID(int crc) { - int i; - unsigned byte; - char sondeid_bytes[9]; - - if (crc == 0) { - for (i = 0; i < 8; i++) { - byte = framebyte(pos_SondeID + i); - //if ((byte < 0x20) || (byte > 0x7E)) return -1; - sondeid_bytes[i] = byte; - } - sondeid_bytes[8] = '\0'; - if ( strncmp(gpx.id, sondeid_bytes, 8) != 0 ) { - //for (i = 0; i < 51; i++) calfrchk[i] = 0; - memset(calfrchk, 0, 51); - memcpy(gpx.id, sondeid_bytes, 8); - gpx.id[8] = '\0'; - } - } - - return 0; -} - -int get_FrameConf() { - int crc, err; - ui8_t calfr; - int i; - - crc = check_CRC(pos_FRAME, pck_FRAME); - if (crc) gpx.crc |= crc_FRAME; - - err = crc; - err |= get_FrameNb(); - err |= get_SondeID(crc); - - if (crc == 0) { - calfr = framebyte(pos_CalData); - if (calfrchk[calfr] == 0) // const? - { - for (i = 0; i < 16; i++) { - calibytes[calfr*16 + i] = framebyte(pos_CalData+1+i); - } - calfrchk[calfr] = 1; - } - } - - return err; -} - -int get_CalData() { - - memcpy(&Rf1, calibytes+61, 4); // 0x03*0x10+13 - memcpy(&Rf2, calibytes+65, 4); // 0x04*0x10+ 1 - - memcpy(co1+0, calibytes+77, 4); // 0x04*0x10+13 - memcpy(co1+1, calibytes+81, 4); // 0x05*0x10+ 1 - memcpy(co1+2, calibytes+85, 4); // 0x05*0x10+ 5 - - memcpy(calT1+0, calibytes+89, 4); // 0x05*0x10+ 9 - memcpy(calT1+1, calibytes+93, 4); // 0x05*0x10+13 - memcpy(calT1+2, calibytes+97, 4); // 0x06*0x10+ 1 - - memcpy(co2+0, calibytes+293, 4); // 0x12*0x10+ 5 - memcpy(co2+1, calibytes+297, 4); // 0x12*0x10+ 9 - memcpy(co2+2, calibytes+301, 4); // 0x12*0x10+13 - - memcpy(calT2+0, calibytes+305, 4); // 0x13*0x10+ 1 - memcpy(calT2+1, calibytes+309, 4); // 0x13*0x10+ 5 - memcpy(calT2+2, calibytes+313, 4); // 0x13*0x10+ 9 - - return 0; -} - -float get_Tc0(ui32_t f, ui32_t f1, ui32_t f2) { - // y = (f - f1) / (f2 - f1); - // y1 = (f - f1) / f2; // = (1 - f1/f2)*y - float a = 3.9083e-3, // Pt1000 platinum resistance - b = -5.775e-7, - c = -4.183e-12; // below 0C, else C=0 - float *cal = calT1; - float Rb = (f1*Rf2-f2*Rf1)/(f2-f1), // ofs - Ra = f * (Rf2-Rf1)/(f2-f1) - Rb, - raw = Ra/1000.0, - g_r = 0.8024*cal[0] + 0.0176, // empirisch - r_o = 0.0705*cal[1] + 0.0011, // empirisch - r = raw * g_r + r_o, - t = (-a + sqrt(a*a + 4*b*(r-1)))/(2*b); // t>0: c=0 - // R/R0 = 1 + at + bt^2 + c(t-100)t^3 , R0 = 1000 Ohm, t/Celsius - return t; -} -float get_Tc(ui32_t f, ui32_t f1, ui32_t f2) { - float *p = co1; - float *c = calT1; - float g = (float)(f2-f1)/(Rf2-Rf1), // gain - Rb = (f1*Rf2-f2*Rf1)/(float)(f2-f1), // ofs - Rc = f/g - Rb, - //R = (Rc + c[1]) * c[0], - //T = p[0] + p[1]*R + p[2]*R*R; - R = Rc * c[0], - T = (p[0] + p[1]*R + p[2]*R*R + c[1])*(1.0 + c[2]); - return T; -} - -int get_PTU() { - int err=0, i; - int bR, bc1, bT1, - bc2, bT2; - ui32_t meas[12]; - float Tc = -273.15; - float Tc0 = -273.15; - - get_CalData(); - - err = check_CRC(pos_PTU, pck_PTU); - if (err) gpx.crc |= crc_PTU; - - if (err == 0) - { - - for (i = 0; i < 12; i++) { - meas[i] = u3(frame+pos_PTU+2+3*i); - } - - bR = calfrchk[0x03] && calfrchk[0x04]; - bc1 = calfrchk[0x04] && calfrchk[0x05]; - bT1 = calfrchk[0x05] && calfrchk[0x06]; - bc2 = calfrchk[0x12] && calfrchk[0x13]; - bT2 = calfrchk[0x13]; - - if (bR && bc1 && bT1) { - Tc = get_Tc(meas[0], meas[1], meas[2]); - Tc0 = get_Tc0(meas[0], meas[1], meas[2]); - } - gpx.T = Tc; - - if (option_verbose == 3) - { - printf(" h: %8.2f # ", gpx.alt); // crc_GPS3 ? - - printf("1: %8d %8d %8d", meas[0], meas[1], meas[2]); - printf(" # "); - printf("2: %8d %8d %8d", meas[3], meas[4], meas[5]); - printf(" # "); - printf("3: %8d %8d %8d", meas[6], meas[7], meas[8]); - printf(" # "); - if (Tc > -273.0) { - printf(" T: %8.4f , T0: %8.4f ", Tc, Tc0); - } - printf("\n"); - - if (gpx.alt > -100.0) { - printf(" %9.2f ; %6.1f ; %6.1f ", gpx.alt, Rf1, Rf2); - printf("; %10.6f ; %10.6f ; %10.6f ;", calT1[0], calT1[1], calT1[2]); - printf(" %8d ; %8d ; %8d ", meas[0], meas[1], meas[2]); - printf("; %10.6f ; %10.6f ; %10.6f ;", calT2[0], calT2[1], calT2[2]); - printf(" %8d ; %8d ; %8d" , meas[6], meas[7], meas[8]); - printf("\n"); - } - } - - } - - return err; -} - -int get_GPSweek() { - int i; - unsigned byte; - ui8_t gpsweek_bytes[2]; - int gpsweek; - - for (i = 0; i < 2; i++) { - byte = framebyte(pos_GPSweek + i); - gpsweek_bytes[i] = byte; - } - - gpsweek = gpsweek_bytes[0] + (gpsweek_bytes[1] << 8); - //if (gpsweek < 0) { gpx.week = -1; return -1; } // (short int) - gpx.week = gpsweek; - - 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 = 0, // 32bit - day; - int ms; - - for (i = 0; i < 4; i++) { - byte = framebyte(pos_GPSiTOW + i); - gpstime_bytes[i] = byte; - } - - memcpy(&gpstime, gpstime_bytes, 4); - ms = gpstime % 1000; - gpstime /= 1000; - - gpx.gpssec = gpstime; - - day = (gpstime / (24 * 3600)) % 7; - //if ((day < 0) || (day > 6)) return -1; // besser CRC-check - - gpstime %= (24*3600); - - gpx.wday = day; - gpx.std = gpstime / 3600; - gpx.min = (gpstime % 3600) / 60; - gpx.sek = gpstime % 60 + ms/1000.0; - - return 0; -} - -int get_GPS1() { - int err=0; - - // ((framebyte(pos_GPS1)<<8) | framebyte(pos_GPS1+1)) != pck_GPS1 ? - if ( framebyte(pos_GPS1) != ((pck_GPS1>>8) & 0xFF) ) { - gpx.crc |= crc_GPS1; - return -1; - } - - err = check_CRC(pos_GPS1, pck_GPS1); - if (err) gpx.crc |= crc_GPS1; - - //err = 0; - err |= get_GPSweek(); - err |= get_GPStime(); - - return err; -} - -int get_GPS2() { - int err=0; - - err = check_CRC(pos_GPS2, pck_GPS2); - if (err) gpx.crc |= crc_GPS2; - - return err; -} - -#define EARTH_a 6378137.0 -#define EARTH_b 6356752.31424518 -#define EARTH_a2_b2 (EARTH_a*EARTH_a - EARTH_b*EARTH_b) - -double a = EARTH_a, - b = EARTH_b, - a_b = EARTH_a2_b2, - e2 = EARTH_a2_b2 / (EARTH_a*EARTH_a), - ee2 = EARTH_a2_b2 / (EARTH_b*EARTH_b); - -void ecef2elli(double X[], double *lat, double *lon, double *alt) { - double phi, lam, R, p, t; - - lam = atan2( X[1] , X[0] ); - - p = sqrt( X[0]*X[0] + X[1]*X[1] ); - t = atan2( X[2]*a , p*b ); - - phi = atan2( X[2] + ee2 * b * sin(t)*sin(t)*sin(t) , - p - e2 * a * cos(t)*cos(t)*cos(t) ); - - R = a / sqrt( 1 - e2*sin(phi)*sin(phi) ); - *alt = p / cos(phi) - R; - - *lat = phi*180/M_PI; - *lon = lam*180/M_PI; -} - -int get_GPSkoord() { - int i, k; - unsigned byte; - ui8_t XYZ_bytes[4]; - int XYZ; // 32bit - double X[3], lat, lon, alt; - ui8_t gpsVel_bytes[2]; - short vel16; // 16bit - double V[3], phi, lam, dir; - - - for (k = 0; k < 3; k++) { - - for (i = 0; i < 4; i++) { - byte = frame[pos_GPSecefX + 4*k + i]; - XYZ_bytes[i] = byte; - } - memcpy(&XYZ, XYZ_bytes, 4); - X[k] = XYZ / 100.0; - - for (i = 0; i < 2; i++) { - byte = frame[pos_GPSecefV + 2*k + i]; - gpsVel_bytes[i] = byte; - } - vel16 = gpsVel_bytes[0] | gpsVel_bytes[1] << 8; - V[k] = vel16 / 100.0; - - } - - - // ECEF-Position - ecef2elli(X, &lat, &lon, &alt); - gpx.lat = lat; - gpx.lon = lon; - gpx.alt = alt; - if ((alt < -1000) || (alt > 80000)) return -3; - - - // ECEF-Velocities - // ECEF-Vel -> NorthEastUp - phi = lat*M_PI/180.0; - lam = lon*M_PI/180.0; - gpx.vN = -V[0]*sin(phi)*cos(lam) - V[1]*sin(phi)*sin(lam) + V[2]*cos(phi); - gpx.vE = -V[0]*sin(lam) + V[1]*cos(lam); - gpx.vU = V[0]*cos(phi)*cos(lam) + V[1]*cos(phi)*sin(lam) + V[2]*sin(phi); - - // NEU -> HorDirVer - gpx.vH = sqrt(gpx.vN*gpx.vN+gpx.vE*gpx.vE); -/* - double alpha; - alpha = atan2(gpx.vN, gpx.vE)*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(gpx.vE, gpx.vN) * 180 / M_PI; - if (dir < 0) dir += 360; - gpx.vD = dir; - - return 0; -} - -int get_GPS3() { - int err=0; - - // ((framebyte(pos_GPS3)<<8) | framebyte(pos_GPS3+1)) != pck_GPS3 ? - if ( framebyte(pos_GPS3) != ((pck_GPS3>>8) & 0xFF) ) { - gpx.crc |= crc_GPS3; - return -1; - } - - err = check_CRC(pos_GPS3, pck_GPS3); - if (err) gpx.crc |= crc_GPS3; - - err |= get_GPSkoord(); - - return err; -} - -int get_Aux() { -// -// "Ozone Sounding with Vaisala Radiosonde RS41" user's guide -// - int i, auxlen, auxcrc, count7E, pos7E; - - count7E = 0; - pos7E = pos_AUX; - - // 7Exx: xdata - while ( pos7E < FRAME_LEN && framebyte(pos7E) == 0x7E ) { - - auxlen = framebyte(pos7E+1); - auxcrc = framebyte(pos7E+2+auxlen) | (framebyte(pos7E+2+auxlen+1)<<8); - - if (count7E == 0) fprintf(stdout, "\n # xdata = "); - else fprintf(stdout, " # "); - - if ( auxcrc == crc16(pos7E+2, auxlen) ) { - //fprintf(stdout, " # %02x : ", framebyte(pos7E+2)); - for (i = 1; i < auxlen; i++) { - fprintf(stdout, "%c", framebyte(pos7E+2+i)); - } - count7E++; - pos7E += 2+auxlen+2; - } - else { - pos7E = FRAME_LEN; - gpx.crc |= crc_AUX; - } - } - - i = check_CRC(pos7E, 0x7600); // 0x76xx: 00-padding block - if (i) gpx.crc |= crc_ZERO; - - return count7E; -} - -int get_Calconf(int out) { - int i; - unsigned byte; - ui8_t calfr = 0; - ui8_t burst = 0; - ui16_t fw = 0; - int freq = 0, f0 = 0, f1 = 0; - char sondetyp[9]; - - byte = framebyte(pos_CalData); - calfr = byte; - - if (option_verbose == 3) { - fprintf(stdout, "\n"); // fflush(stdout); - fprintf(stdout, "[%5d] ", gpx.frnr); - fprintf(stdout, " 0x%02x: ", calfr); - for (i = 0; i < 16; i++) { - byte = framebyte(pos_CalData+1+i); - fprintf(stdout, "%02x ", byte); - } - if (check_CRC(pos_FRAME, pck_FRAME)==0) fprintf(stdout, "[OK]"); - else fprintf(stdout, "[NO]"); - fprintf(stdout, " "); - } - - if (out) - { - if (calfr == 0x01 && option_verbose /*== 2*/) { - fw = framebyte(pos_CalData+6) | (framebyte(pos_CalData+7)<<8); - fprintf(stdout, ": fw 0x%04x ", fw); - } - - if (calfr == 0x02 && option_verbose /*== 2*/) { - byte = framebyte(pos_Calburst); - burst = byte; // fw >= 0x4ef5, BK irrelevant? (killtimer in 0x31?) - fprintf(stdout, ": BK %02X ", burst); - } - - if (calfr == 0x00 && option_verbose) { - byte = framebyte(pos_Calfreq) & 0xC0; // erstmal nur oberste beiden bits - f0 = (byte * 10) / 64; // 0x80 -> 1/2, 0x40 -> 1/4 ; dann mal 40 - byte = framebyte(pos_Calfreq+1); - f1 = 40 * byte; - freq = 400000 + f1+f0; // kHz; - fprintf(stdout, ": fq %d ", freq); - } - - if (calfr == 0x21 && option_verbose /*== 2*/) { // eventuell noch zwei bytes in 0x22 - for (i = 0; i < 9; i++) sondetyp[i] = 0; - for (i = 0; i < 8; i++) { - byte = framebyte(pos_CalRSTyp + i); - if ((byte >= 0x20) && (byte < 0x7F)) sondetyp[i] = byte; - else if (byte == 0x00) sondetyp[i] = '\0'; - } - fprintf(stdout, ": %s ", sondetyp); - } - } - - return 0; -} - -/* ------------------------------------------------------------------------------------ */ -/* - (uses fec-lib by KA9Q) - ka9q-fec: - gcc -c init_rs_char.c - gcc -c decode_rs_char.c - -#include "fec.h" // ka9q-fec - - -void *rs; -unsigned char codeword1[rs_N], codeword2[rs_N]; - - rs = init_rs_char( 8, 0x11d, 0, 1, rs_R, 0); - - // ka9q-fec301: p(x) = p[0]x^(N-1) + ... + p[N-2]x + p[N-1] - // -> cw[i] = codeword[RS.N-1-i] - -*/ - -#define rs_N 255 -#define rs_R 24 -#define rs_K (rs_N-rs_R) - -ui8_t cw1[rs_N], cw2[rs_N]; - -int rs41_ecc(int frmlen) { -// richtige framelen wichtig fuer 0-padding - - int i, leak, ret = 0; - int errors1, errors2; - ui8_t err_pos1[rs_R], err_pos2[rs_R], - err_val1[rs_R], err_val2[rs_R]; - - - if (frmlen > FRAME_LEN) frmlen = FRAME_LEN; - cfg_rs41.frmlen = frmlen; - cfg_rs41.msglen = (frmlen-56)/2; // msgpos=56; - leak = frmlen % 2; - - for (i = frmlen; i < FRAME_LEN; i++) frame[i] = 0; // FRAME_LEN-HDR = 510 = 2*255 - - - for (i = 0; i < rs_R; i++) cw1[i] = frame[cfg_rs41.parpos+i ]; - for (i = 0; i < rs_R; i++) cw2[i] = frame[cfg_rs41.parpos+i+rs_R]; - for (i = 0; i < rs_K; i++) cw1[rs_R+i] = frame[cfg_rs41.msgpos+2*i ]; - for (i = 0; i < rs_K; i++) cw2[rs_R+i] = frame[cfg_rs41.msgpos+2*i+1]; - - errors1 = rs_decode(cw1, err_pos1, err_val1); - errors2 = rs_decode(cw2, err_pos2, err_val2); - - - // Wenn Fehler im 00-padding korrigiert wurden, - // war entweder der frame zu kurz, oder - // Fehler wurden falsch korrigiert; - // allerdings ist bei t=12 die Wahrscheinlichkeit, - // dass falsch korrigiert wurde mit 1/t! sehr gering. - - // check CRC32 - // CRC32 OK: - //for (i = 0; i < cfg_rs41.hdrlen; i++) frame[i] = data[i]; - for (i = 0; i < rs_R; i++) { - frame[cfg_rs41.parpos+ i] = cw1[i]; - frame[cfg_rs41.parpos+rs_R+i] = cw2[i]; - } - for (i = 0; i < rs_K; i++) { // cfg_rs41.msglen <= rs_K - frame[cfg_rs41.msgpos+ 2*i] = cw1[rs_R+i]; - frame[cfg_rs41.msgpos+1+2*i] = cw2[rs_R+i]; - } - if (leak) { - frame[cfg_rs41.msgpos+2*i] = cw1[rs_R+i]; - } - - - ret = errors1 + errors2; - if (errors1 < 0 || errors2 < 0) ret = -1; - - return ret; -} - -/* ------------------------------------------------------------------------------------ */ - - -int print_position() { - int i; - int err, err0, err1, err2, err3; - int output, out_mask; - - err = get_FrameConf(); - - err1 = get_GPS1(); - err2 = get_GPS2(); - err3 = get_GPS3(); - - err0 = get_PTU(); - - out_mask = crc_FRAME|crc_GPS1|crc_GPS3; - output = ((gpx.crc & out_mask) != out_mask); // (!err || !err1 || !err3); - - if (output) { - - if (!err) { - fprintf(stdout, "[%5d] ", gpx.frnr); - fprintf(stdout, "(%s) ", gpx.id); - } - if (!err1) { - Gps2Date(gpx.week, gpx.gpssec, &gpx.jahr, &gpx.monat, &gpx.tag); - fprintf(stdout, "%s ", weekday[gpx.wday]); - fprintf(stdout, "%04d-%02d-%02d %02d:%02d:%06.3f", - gpx.jahr, gpx.monat, gpx.tag, gpx.std, gpx.min, gpx.sek); - if (option_verbose == 3) fprintf(stdout, " (W %d)", gpx.week); - } - if (!err3) { - fprintf(stdout, " "); - fprintf(stdout, " lat: %.5f ", gpx.lat); - fprintf(stdout, " lon: %.5f ", gpx.lon); - fprintf(stdout, " alt: %.2f ", gpx.alt); - //if (option_verbose) - { - //fprintf(stdout, " (%.1f %.1f %.1f) ", gpx.vN, gpx.vE, gpx.vU); - fprintf(stdout," vH: %4.1f D: %5.1f° vV: %3.1f ", gpx.vH, gpx.vD, gpx.vU); - } - } - if (option_ptu && !err0) { - if (gpx.T > -273.0) printf(" T=%.1fC ", gpx.T); - } - - - if (output) - { - if (option_crc) { - fprintf(stdout, " # ["); - for (i=0; i<5; i++) fprintf(stdout, "%d", (gpx.crc>>i)&1); - fprintf(stdout, "]"); - } - } - - get_Calconf(output); - - //if (output) - { - if (option_verbose > 1) get_Aux(); - fprintf(stdout, "\n"); // fflush(stdout); - } - } - - err |= err1 | err3; - - return err; -} - -void print_frame(int len) { - int i, ret = 0; - - gpx.crc = 0; - - for (i = len; i < FRAME_LEN; i++) { - //xframe[i] = 0; - frame[i] = 0; - } - - if (option_ecc) { - ret = rs41_ecc(len); - } - - - if (option_raw) { - for (i = 0; i < len; i++) { - fprintf(stdout, "%02x", frame[i]); - } - if (option_ecc) { - if (ret >= 0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); - if (option_ecc == 2 && ret > 0) fprintf(stdout, " (%d)", ret); - } - fprintf(stdout, "\n"); - } - else if (option_sat) { - get_SatData(); - } - else { - print_position(); - } -} - - -int main(int argc, char *argv[]) { - - FILE *fp; - char *fpname = NULL; - float spb = 0.0; - char bitbuf[8]; - int bit_count = 0, - bitpos = 0, - byte_count = FRAMESTART, - header_found = 0; - int bit, byte; - int frmlen = FRAME_LEN; - int headerlen; - int herrs, herr1; - int bitQ, Qerror_count; - - float mv; - unsigned int mv_pos, mv0_pos; - - float thres = 0.7; - - int bitofs = 0, dif = 0; - int symlen = 1; - - -#ifdef CYGWIN - _setmode(fileno(stdin), _O_BINARY); // _fileno(stdin) -#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, -vx, -vv (info, aux, info/conf)\n"); - fprintf(stderr, " -r, --raw\n"); - fprintf(stderr, " -i, --invert\n"); - fprintf(stderr, " --crc (check CRC)\n"); - fprintf(stderr, " --ecc (Reed-Solomon)\n"); - fprintf(stderr, " --std (std framelen)\n"); - fprintf(stderr, " --ths (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, "-vx") == 0) { option_verbose = 2; } - else if (strcmp(*argv, "-vv") == 0) { option_verbose = 3; } - else if (strcmp(*argv, "--crc") == 0) { option_crc = 1; } - else if (strcmp(*argv, "--res") == 0) { option_res = 1; } - 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; - } - else if (strcmp(*argv, "--ecc" ) == 0) { option_ecc = 1; } - else if (strcmp(*argv, "--ecc2") == 0) { option_ecc = 2; } - else if (strcmp(*argv, "--std" ) == 0) { frmlen = 320; } // NDATA_LEN - else if (strcmp(*argv, "--std2") == 0) { frmlen = 518; } // NDATA_LEN+XDATA_LEN - else if (strcmp(*argv, "--sat") == 0) { option_sat = 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"); - } - - - if (option_ecc) { - rs_init_RS255(); - } - - - symlen = 1; - headerlen = strlen(header); - bitofs = 2; // +1 .. +2 - if (init_buffers(header, headerlen, 2) < 0) { // shape=2 - 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, header, headerlen, mv_pos); // symlen=1 - herr1 = 0; - if (herrs <= 3 && herrs > 0) { - herr1 = headcmp(symlen, header, headerlen, mv_pos+1); - if (herr1 < herrs) { - herrs = herr1; - herr1 = 1; - } - } - if (herrs <= 2) header_found = 1; // herrs <= 2 bitfehler in header - - if (header_found) { - - byte_count = FRAMESTART; - bit_count = 0; // byte_count*8-HEADLEN - bitpos = 0; - - Qerror_count = 0; - - while ( byte_count < frmlen ) { - bitQ = read_sbit(fp, symlen, &bit, option_inv, bitofs, bit_count==0, 0); // symlen=1, return: zeroX/bit - if ( bitQ == EOF) break; - bit_count += 1; - bitbuf[bitpos] = bit; - bitpos++; - if (bitpos == BITS) { - bitpos = 0; - byte = bits2byte(bitbuf); - frame[byte_count] = byte ^ mask[byte_count % MASK_LEN]; - - byteQ[byte_count] = get_bufvar(0); - - if (byte_count > NDATA_LEN) { // Fehler erst ab minimaler framelen Zaehlen - if (byteQ[byte_count]*2 > byteQ[byte_count-300]*3) { // Var(frame)/Var(noise) ca. 1:2 - Qerror_count += 1; - } - } - - byte_count++; - } - if (Qerror_count > 4) { // ab byte 320 entscheiden, ob framelen = 320 oder 518 - if (byte_count > NDATA_LEN && byte_count < NDATA_LEN+XDATA_LEN-10) { - byte_count = NDATA_LEN; - } // in print_frame() wird ab byte_count mit 00 aufgefuellt fuer Fehlerkorrektur - break; - } - } - header_found = 0; - print_frame(byte_count); - - while ( bit_count < BITS*FRAME_LEN ) { - bitQ = read_sbit(fp, symlen, &bit, option_inv, bitofs, bit_count==0, 0); // symlen=1, return: zeroX/bit - if ( bitQ == EOF) break; - bit_count++; - } - - byte_count = FRAMESTART; - } - } - } - - } - - - free_buffers(); - - fclose(fp); - - return 0; -} - diff --git a/demod/rs41dm_dft.c b/demod/rs41dm_dft.c deleted file mode 100644 index ff8d581..0000000 --- a/demod/rs41dm_dft.c +++ /dev/null @@ -1,1489 +0,0 @@ - -/* - * rs41 - * sync header: correlation/matched filter - * files: rs41dm_dft.c bch_ecc.c demod_dft.h demod_dft.c - * compile: - * gcc -c demod_dft.c - * gcc rs41dm_dft.c demod_dft.o -lm -o rs41dm_dft - * - * author: zilog80 - */ - -#include -#include -#include -#include - -#ifdef CYGWIN - #include // cygwin: _setmode() - #include -#endif - -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.c" -#include "demod_dft.h" - -#include "bch_ecc.c" // RS/ecc/ - - -typedef struct { - int typ; - int msglen; - int msgpos; - int parpos; - int hdrlen; - int frmlen; -} rscfg_t; - -rscfg_t cfg_rs41 = { 41, (320-56)/2, 56, 8, 8, 320}; - - -typedef struct { - int frnr; - char id[9]; - ui8_t numSV; - int week; int gpssec; - int jahr; int monat; int tag; - int wday; - int std; int min; float sek; - double lat; double lon; double alt; - double vN; double vE; double vU; - double vH; double vD; double vD2; - float T; float RH; - ui32_t crc; -} gpx_t; - -gpx_t gpx; - -int option_verbose = 0, // ausfuehrliche Anzeige - option_raw = 0, // rohe Frames - option_inv = 0, // invertiert Signal - option_res = 0, // genauere Bitmessung - option_crc = 0, // check CRC - option_ecc = 0, // Reed-Solomon ECC - option_sat = 0, // GPS sat data - option_ptu = 0, - option_ths = 0, - option_json = 0, // JSON output (auto_rx) - wavloaded = 0; -int wav_channel = 0; // audio channel: left - - -#define BITS 8 -#define HEADLEN 64 -#define FRAMESTART ((HEADLEN)/BITS) - -/* 10 B6 CA 11 22 96 12 F8 */ -char header[] = "0000100001101101010100111000100001000100011010010100100000011111"; - - -#define NDATA_LEN 320 // std framelen 320 -#define XDATA_LEN 198 -#define FRAME_LEN (NDATA_LEN+XDATA_LEN) // max framelen 518 -ui8_t //xframe[FRAME_LEN] = { 0x10, 0xB6, 0xCA, 0x11, 0x22, 0x96, 0x12, 0xF8}, = xorbyte( frame) - frame[FRAME_LEN] = { 0x86, 0x35, 0xf4, 0x40, 0x93, 0xdf, 0x1a, 0x60}; // = xorbyte(xframe) - -float byteQ[FRAME_LEN]; - -#define MASK_LEN 64 -ui8_t mask[MASK_LEN] = { 0x96, 0x83, 0x3E, 0x51, 0xB1, 0x49, 0x08, 0x98, - 0x32, 0x05, 0x59, 0x0E, 0xF9, 0x44, 0xC6, 0x26, - 0x21, 0x60, 0xC2, 0xEA, 0x79, 0x5D, 0x6D, 0xA1, - 0x54, 0x69, 0x47, 0x0C, 0xDC, 0xE8, 0x5C, 0xF1, - 0xF7, 0x76, 0x82, 0x7F, 0x07, 0x99, 0xA2, 0x2C, - 0x93, 0x7C, 0x30, 0x63, 0xF5, 0x10, 0x2E, 0x61, - 0xD0, 0xBC, 0xB4, 0xB6, 0x06, 0xAA, 0xF4, 0x23, - 0x78, 0x6E, 0x3B, 0xAE, 0xBF, 0x7B, 0x4C, 0xC1}; -/* LFSR: ab i=8 (mod 64): - * m[16+i] = m[i] ^ m[i+2] ^ m[i+4] ^ m[i+6] - * ________________3205590EF944C6262160C2EA795D6DA15469470CDCE85CF1 - * F776827F0799A22C937C3063F5102E61D0BCB4B606AAF423786E3BAEBF7B4CC196833E51B1490898 - */ - -/* ------------------------------------------------------------------------------------ */ - -#define BAUD_RATE 4800 - -/* ------------------------------------------------------------------------------------ */ - - -int bits2byte(char bits[]) { - int i, byteval=0, d=1; - for (i = 0; i < 8; i++) { // little endian - /* for (i = 7; i >= 0; i--) { // big endian */ - if (bits[i] == 1) byteval += d; - else if (bits[i] == 0) byteval += 0; - else return 0x100; - d <<= 1; - } - return byteval; -} - - -/* -ui8_t xorbyte(int pos) { - return xframe[pos] ^ mask[pos % MASK_LEN]; -} -*/ -ui8_t framebyte(int pos) { - return frame[pos]; -} - - -/* ------------------------------------------------------------------------------------ */ -/* - * 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; -} -/* ------------------------------------------------------------------------------------ */ - -ui32_t u4(ui8_t *bytes) { // 32bit unsigned int - ui32_t val = 0; - memcpy(&val, bytes, 4); - return val; -} - -ui32_t u3(ui8_t *bytes) { // 24bit unsigned int - int val24 = 0; - val24 = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16); - // = memcpy(&val, bytes, 3), val &= 0x00FFFFFF; - return val24; -} - -int i3(ui8_t *bytes) { // 24bit signed int - int val = 0, - val24 = 0; - val = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16); - val24 = val & 0xFFFFFF; if (val24 & 0x800000) val24 = val24 - 0x1000000; - return val24; -} - -ui32_t u2(ui8_t *bytes) { // 16bit unsigned int - return bytes[0] | (bytes[1]<<8); -} - -/* -double r8(ui8_t *bytes) { - double val = 0; - memcpy(&val, bytes, 8); - return val; -} - -float r4(ui8_t *bytes) { - float val = 0; - memcpy(&val, bytes, 4); - return val; -} -*/ - -/* -int crc16x(int start, int len) { - int crc16poly = 0x1021; - int rem = 0xFFFF, i, j; - int xbyte; - - if (start+len+2 > FRAME_LEN) return -1; - - for (i = 0; i < len; i++) { - xbyte = xorbyte(start+i); - rem = rem ^ (xbyte << 8); - for (j = 0; j < 8; j++) { - if (rem & 0x8000) { - rem = (rem << 1) ^ crc16poly; - } - else { - rem = (rem << 1); - } - rem &= 0xFFFF; - } - } - return rem; -} -*/ -int crc16(int start, int len) { - int crc16poly = 0x1021; - int rem = 0xFFFF, i, j; - int byte; - - if (start+len+2 > FRAME_LEN) return -1; - - for (i = 0; i < len; i++) { - byte = framebyte(start+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; -} - -int check_CRC(ui32_t pos, ui32_t pck) { - ui32_t crclen = 0, - crcdat = 0; - if (((pck>>8) & 0xFF) != frame[pos]) return -1; - crclen = frame[pos+1]; - if (pos + crclen + 4 > FRAME_LEN) return -1; - crcdat = u2(frame+pos+2+crclen); - if ( crcdat != crc16(pos+2, crclen) ) { - return 1; // CRC NO - } - else return 0; // CRC OK -} - - -/* - Pos: SubHeader, 1+1 byte (ID+LEN) -0x039: 7928 FrameNumber+SondeID - +(0x050: 0732 CalFrames 0x00..0x32) -0x065: 7A2A PTU -0x093: 7C1E GPS1: RXM-RAW (0x02 0x10) Week, TOW, Sats -0x0B5: 7D59 GPS2: RXM-RAW (0x02 0x10) pseudorange, doppler -0x112: 7B15 GPS3: NAV-SOL (0x01 0x06) ECEF-POS, ECEF-VEL -0x12B: 7611 00 -0x12B: 7Exx AUX-xdata -*/ - -#define crc_FRAME (1<<0) -#define xor_FRAME 0x1713 // ^0x6E3B=0x7928 -#define pck_FRAME 0x7928 -#define pos_FRAME 0x039 -#define pos_FrameNb 0x03B // 2 byte -#define pos_SondeID 0x03D // 8 byte -#define pos_CalData 0x052 // 1 byte, counter 0x00..0x32 -#define pos_Calfreq 0x055 // 2 byte, calfr 0x00 -#define pos_Calburst 0x05E // 1 byte, calfr 0x02 -// ? #define pos_Caltimer 0x05A // 2 byte, calfr 0x02 ? -#define pos_CalRSTyp 0x05B // 8 byte, calfr 0x21 (+2 byte in 0x22?) - // weitere chars in calfr 0x22/0x23; weitere ID - -#define crc_PTU (1<<1) -#define xor_PTU 0xE388 // ^0x99A2=0x0x7A2A -#define pck_PTU 0x7A2A // PTU -#define pos_PTU 0x065 - -#define crc_GPS1 (1<<2) -#define xor_GPS1 0x9667 // ^0xEA79=0x7C1E -#define pck_GPS1 0x7C1E // RXM-RAW (0x02 0x10) -#define pos_GPS1 0x093 -#define pos_GPSweek 0x095 // 2 byte -#define pos_GPSiTOW 0x097 // 4 byte -#define pos_satsN 0x09B // 12x2 byte (1: SV, 1: quality,strength) - -#define crc_GPS2 (1<<3) -#define xor_GPS2 0xD7AD // ^0xAAF4=0x7D59 -#define pck_GPS2 0x7D59 // RXM-RAW (0x02 0x10) -#define pos_GPS2 0x0B5 -#define pos_minPR 0x0B7 // 4 byte -#define pos_FF 0x0BB // 1 byte -#define pos_dataSats 0x0BC // 12x(4+3) byte (4: pseudorange, 3: doppler) - -#define crc_GPS3 (1<<4) -#define xor_GPS3 0xB9FF // ^0xC2EA=0x7B15 -#define pck_GPS3 0x7B15 // NAV-SOL (0x01 0x06) -#define pos_GPS3 0x112 -#define pos_GPSecefX 0x114 // 4 byte -#define pos_GPSecefY 0x118 // 4 byte -#define pos_GPSecefZ 0x11C // 4 byte -#define pos_GPSecefV 0x120 // 3*2 byte -#define pos_numSats 0x126 // 1 byte -#define pos_sAcc 0x127 // 1 byte -#define pos_pDOP 0x128 // 1 byte - -#define crc_AUX (1<<5) -#define pck_AUX 0x7E00 // LEN variable -#define pos_AUX 0x12B - -#define crc_ZERO (1<<6) // LEN variable -#define pck_ZERO 0x7600 -#define pck_ZEROstd 0x7611 // NDATA std-frm, no aux -#define pos_ZEROstd 0x12B // pos_AUX(0) - -#define pck_ENCRYPTED 0x80 // Packet type for an Encrypted payload - -/* - frame[pos_FRAME-1] == 0x0F: len == NDATA_LEN(320) - frame[pos_FRAME-1] == 0xF0: len == FRAME_LEN(518) -*/ -int frametype() { // -4..+4: 0xF0 -> -4 , 0x0F -> +4 - int i; - ui8_t b = frame[pos_FRAME-1]; - int ft = 0; - for (i = 0; i < 4; i++) { - ft += ((b>>i)&1) - ((b>>(i+4))&1); - } - return ft; -} - - -ui8_t calibytes[51*16]; -ui8_t calfrchk[51]; -float Rf1, // ref-resistor f1 (750 Ohm) - Rf2, // ref-resistor f2 (1100 Ohm) - co1[3], // { -243.911 , 0.187654 , 8.2e-06 } - calT1[3], // calibration T1 - co2[3], // { -243.911 , 0.187654 , 8.2e-06 } - calT2[3]; // calibration T2-Hum -float calH[2]; // calibration Hum - - -double c = 299.792458e6; -double L1 = 1575.42e6; - -int get_SatData() { - int i, n; - int sv; - ui32_t minPR; - int numSV; - double pDOP, sAcc; - - fprintf(stdout, "[%d]\n", u2(frame+pos_FrameNb)); - - fprintf(stdout, "iTOW: 0x%08X", u4(frame+pos_GPSiTOW)); - fprintf(stdout, " week: 0x%04X", u2(frame+pos_GPSweek)); - fprintf(stdout, "\n"); - minPR = u4(frame+pos_minPR); - fprintf(stdout, "minPR: %d", minPR); - fprintf(stdout, "\n"); - - for (i = 0; i < 12; i++) { - n = i*7; - sv = frame[pos_satsN+2*i]; - if (sv == 0xFF) break; - fprintf(stdout, " SV: %2d # ", sv); - fprintf(stdout, "prMes: %.1f", u4(frame+pos_dataSats+n)/100.0 + minPR); - fprintf(stdout, " "); - fprintf(stdout, "doMes: %.1f", -i3(frame+pos_dataSats+n+4)/100.0*L1/c); - fprintf(stdout, "\n"); - } - - fprintf(stdout, "ECEF-POS: (%d,%d,%d)\n", - (i32_t)u4(frame+pos_GPSecefX), - (i32_t)u4(frame+pos_GPSecefY), - (i32_t)u4(frame+pos_GPSecefZ)); - fprintf(stdout, "ECEF-VEL: (%d,%d,%d)\n", - (i16_t)u2(frame+pos_GPSecefV+0), - (i16_t)u2(frame+pos_GPSecefV+2), - (i16_t)u2(frame+pos_GPSecefV+4)); - - numSV = frame[pos_numSats]; - sAcc = frame[pos_sAcc]/10.0; if (frame[pos_sAcc] == 0xFF) sAcc = -1.0; - pDOP = frame[pos_pDOP]/10.0; if (frame[pos_pDOP] == 0xFF) pDOP = -1.0; - fprintf(stdout, "numSatsFix: %2d sAcc: %.1f pDOP: %.1f\n", numSV, sAcc, pDOP); - - - fprintf(stdout, "CRC: "); - fprintf(stdout, " %04X", pck_GPS1); - if (check_CRC(pos_GPS1, pck_GPS1)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); - //fprintf(stdout, "[%+d]", check_CRC(pos_GPS1, pck_GPS1)); - fprintf(stdout, " %04X", pck_GPS2); - if (check_CRC(pos_GPS2, pck_GPS2)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); - //fprintf(stdout, "[%+d]", check_CRC(pos_GPS2, pck_GPS2)); - fprintf(stdout, " %04X", pck_GPS3); - if (check_CRC(pos_GPS3, pck_GPS3)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); - //fprintf(stdout, "[%+d]", check_CRC(pos_GPS3, pck_GPS3)); - - fprintf(stdout, "\n"); - fprintf(stdout, "\n"); - - return 0; -} - - -int get_FrameNb() { - int i; - unsigned byte; - ui8_t frnr_bytes[2]; - int frnr; - - for (i = 0; i < 2; i++) { - byte = framebyte(pos_FrameNb + i); - frnr_bytes[i] = byte; - } - - frnr = frnr_bytes[0] + (frnr_bytes[1] << 8); - gpx.frnr = frnr; - - return 0; -} - -int get_SondeID(int crc) { - int i; - unsigned byte; - char sondeid_bytes[9]; - - if (crc == 0) { - for (i = 0; i < 8; i++) { - byte = framebyte(pos_SondeID + i); - //if ((byte < 0x20) || (byte > 0x7E)) return -1; - sondeid_bytes[i] = byte; - } - sondeid_bytes[8] = '\0'; - if ( strncmp(gpx.id, sondeid_bytes, 8) != 0 ) { - //for (i = 0; i < 51; i++) calfrchk[i] = 0; - memset(calfrchk, 0, 51); - memcpy(gpx.id, sondeid_bytes, 8); - gpx.id[8] = '\0'; - } - } - - return 0; -} - -int get_FrameConf() { - int crc, err; - ui8_t calfr; - int i; - - crc = check_CRC(pos_FRAME, pck_FRAME); - if (crc) gpx.crc |= crc_FRAME; - - err = crc; - err |= get_FrameNb(); - err |= get_SondeID(crc); - - if (crc == 0) { - calfr = framebyte(pos_CalData); - if (calfrchk[calfr] == 0) // const? - { // 0x32 not constant - for (i = 0; i < 16; i++) { - calibytes[calfr*16 + i] = framebyte(pos_CalData+1+i); - } - calfrchk[calfr] = 1; - } - } - - return err; -} - -int get_CalData() { - - memcpy(&Rf1, calibytes+61, 4); // 0x03*0x10+13 - memcpy(&Rf2, calibytes+65, 4); // 0x04*0x10+ 1 - - memcpy(co1+0, calibytes+77, 4); // 0x04*0x10+13 - memcpy(co1+1, calibytes+81, 4); // 0x05*0x10+ 1 - memcpy(co1+2, calibytes+85, 4); // 0x05*0x10+ 5 - - memcpy(calT1+0, calibytes+89, 4); // 0x05*0x10+ 9 - memcpy(calT1+1, calibytes+93, 4); // 0x05*0x10+13 - memcpy(calT1+2, calibytes+97, 4); // 0x06*0x10+ 1 - - memcpy(calH+0, calibytes+117, 4); // 0x07*0x10+ 5 - memcpy(calH+1, calibytes+121, 4); // 0x07*0x10+ 9 - - memcpy(co2+0, calibytes+293, 4); // 0x12*0x10+ 5 - memcpy(co2+1, calibytes+297, 4); // 0x12*0x10+ 9 - memcpy(co2+2, calibytes+301, 4); // 0x12*0x10+13 - - memcpy(calT2+0, calibytes+305, 4); // 0x13*0x10+ 1 - memcpy(calT2+1, calibytes+309, 4); // 0x13*0x10+ 5 - memcpy(calT2+2, calibytes+313, 4); // 0x13*0x10+ 9 - - return 0; -} - -/* -float get_Tc0(ui32_t f, ui32_t f1, ui32_t f2) { - // y = (f - f1) / (f2 - f1); - // y1 = (f - f1) / f2; // = (1 - f1/f2)*y - float a = 3.9083e-3, // Pt1000 platinum resistance - b = -5.775e-7, - c = -4.183e-12; // below 0C, else C=0 - float *cal = calT1; - float Rb = (f1*Rf2-f2*Rf1)/(f2-f1), // ofs - Ra = f * (Rf2-Rf1)/(f2-f1) - Rb, - raw = Ra/1000.0, - g_r = 0.8024*cal[0] + 0.0176, // empirisch - r_o = 0.0705*cal[1] + 0.0011, // empirisch - r = raw * g_r + r_o, - t = (-a + sqrt(a*a + 4*b*(r-1)))/(2*b); // t>0: c=0 - // R/R0 = 1 + at + bt^2 + c(t-100)t^3 , R0 = 1000 Ohm, t/Celsius - return t; -} -*/ -// T_RH-sensor -float get_TH(ui32_t f, ui32_t f1, ui32_t f2) { - float *p = co2; - float *c = calT2; - float g = (float)(f2-f1)/(Rf2-Rf1), // gain - Rb = (f1*Rf2-f2*Rf1)/(float)(f2-f1), // ofs - Rc = f/g - Rb, - //R = (Rc + c[1]) * c[0], - //T = p[0] + p[1]*R + p[2]*R*R; - R = Rc * c[0], - T = (p[0] + p[1]*R + p[2]*R*R + c[1])*(1.0 + c[2]); - return T; -} -// T-sensor, platinum resistor -float get_Tc(ui32_t f, ui32_t f1, ui32_t f2) { - float *p = co1; - float *c = calT1; - float g = (float)(f2-f1)/(Rf2-Rf1), // gain - Rb = (f1*Rf2-f2*Rf1)/(float)(f2-f1), // ofs - Rc = f/g - Rb, - //R = (Rc + c[1]) * c[0], - //T = p[0] + p[1]*R + p[2]*R*R; - R = Rc * c[0], - T = (p[0] + p[1]*R + p[2]*R*R + c[1])*(1.0 + c[2]); - return T; -} - -// rel.hum., capacitor -// (data:) ftp://ftp-cdc.dwd.de/pub/CDC/observations_germany/radiosondes/ -// (diffAlt: Ellipsoid-Geoid) -float get_RH(ui32_t f, ui32_t f1, ui32_t f2, float T) { - float b0 = calH[0]/46.64; // empirical - float b1 = 0.1276; // empirical - float fh = (f-f1)/(float)(f2-f1); - float rh = 100.0 * (fh-b0)/b1; - float T0 = 0.0, T1 = -30.0; // T/C - if (T < T0) rh += T0 - T/5.5; // empir. temperature compensation - if (T < T1) rh *= 1.0 + (T1-T)/75.0; // empir. temperature compensation - if (rh < 0.0) rh = 0.0; - if (rh > 100.0) rh = 100.0; - if (T < -273.0) rh = -1.0; - return rh; -} - -int get_PTU() { - int err=0, i; - int bR, bc1, bT1, - bc2, bT2; - int bH; - ui32_t meas[12]; - float Tc = -273.15; - float TH = -273.15; - float RH = -1.0; - - get_CalData(); - - err = check_CRC(pos_PTU, pck_PTU); - if (err) gpx.crc |= crc_PTU; - - if (err == 0) - { - - for (i = 0; i < 12; i++) { - meas[i] = u3(frame+pos_PTU+2+3*i); - } - - bR = calfrchk[0x03] && calfrchk[0x04]; - bc1 = calfrchk[0x04] && calfrchk[0x05]; - bT1 = calfrchk[0x05] && calfrchk[0x06]; - bc2 = calfrchk[0x12] && calfrchk[0x13]; - bT2 = calfrchk[0x13]; - bH = calfrchk[0x07]; - - if (bR && bc1 && bT1) { - Tc = get_Tc(meas[0], meas[1], meas[2]); - //Tc0 = get_Tc0(meas[0], meas[1], meas[2]); - } - gpx.T = Tc; - - if (bR && bc2 && bT2) { - TH = get_TH(meas[6], meas[7], meas[8]); - } - - if (bH) { - RH = get_RH(meas[3], meas[4], meas[5], Tc); // TH, TH-Tc (sensorT - T) - } - gpx.RH = RH; - - - if (option_verbose == 4 && (gpx.crc & (crc_PTU | crc_GPS3))==0) - { - printf(" h: %8.2f # ", gpx.alt); // crc_GPS3 ? - - printf("1: %8d %8d %8d", meas[0], meas[1], meas[2]); - printf(" # "); - printf("2: %8d %8d %8d", meas[3], meas[4], meas[5]); - printf(" # "); - printf("3: %8d %8d %8d", meas[6], meas[7], meas[8]); - printf(" # "); - - //if (Tc > -273.0 && RH > -0.5) - { - printf(" "); - printf(" Tc:%.2f ", Tc); - printf(" RH:%.1f ", RH); - printf(" TH:%.2f ", TH); - } - printf("\n"); - - //if (gpx.alt > -400.0) - { - printf(" %9.2f ; %6.1f ; %6.1f ", gpx.alt, Rf1, Rf2); - printf("; %10.6f ; %10.6f ; %10.6f ", calT1[0], calT1[1], calT1[2]); - //printf("; %8d ; %8d ; %8d ", meas[0], meas[1], meas[2]); - printf("; %10.6f ; %10.6f ", calH[0], calH[1]); - //printf("; %8d ; %8d ; %8d" , meas[3], meas[4], meas[5]); - printf("; %10.6f ; %10.6f ; %10.6f ", calT2[0], calT2[1], calT2[2]); - //printf("; %8d ; %8d ; %8d" , meas[6], meas[7], meas[8]); - printf("\n"); - } - } - - } - - return err; -} - -int get_GPSweek() { - int i; - unsigned byte; - ui8_t gpsweek_bytes[2]; - int gpsweek; - - for (i = 0; i < 2; i++) { - byte = framebyte(pos_GPSweek + i); - gpsweek_bytes[i] = byte; - } - - gpsweek = gpsweek_bytes[0] + (gpsweek_bytes[1] << 8); - //if (gpsweek < 0) { gpx.week = -1; return -1; } // (short int) - gpx.week = gpsweek; - - 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 = 0, // 32bit - day; - int ms; - - for (i = 0; i < 4; i++) { - byte = framebyte(pos_GPSiTOW + i); - gpstime_bytes[i] = byte; - } - - memcpy(&gpstime, gpstime_bytes, 4); - ms = gpstime % 1000; - gpstime /= 1000; - - gpx.gpssec = gpstime; - - day = (gpstime / (24 * 3600)) % 7; - //if ((day < 0) || (day > 6)) return -1; // besser CRC-check - - gpstime %= (24*3600); - - gpx.wday = day; - gpx.std = gpstime / 3600; - gpx.min = (gpstime % 3600) / 60; - gpx.sek = gpstime % 60 + ms/1000.0; - - return 0; -} - -int get_GPS1() { - int err=0; - - // ((framebyte(pos_GPS1)<<8) | framebyte(pos_GPS1+1)) != pck_GPS1 ? - if ( framebyte(pos_GPS1) != ((pck_GPS1>>8) & 0xFF) ) { - gpx.crc |= crc_GPS1; - return -1; - } - - err = check_CRC(pos_GPS1, pck_GPS1); - if (err) gpx.crc |= crc_GPS1; - - err |= get_GPSweek(); // no plausibility-check - err |= get_GPStime(); // no plausibility-check - - return err; -} - -int get_GPS2() { - int err=0; - - err = check_CRC(pos_GPS2, pck_GPS2); - if (err) gpx.crc |= crc_GPS2; - - return err; -} - -#define EARTH_a 6378137.0 -#define EARTH_b 6356752.31424518 -#define EARTH_a2_b2 (EARTH_a*EARTH_a - EARTH_b*EARTH_b) - -double a = EARTH_a, - b = EARTH_b, - a_b = EARTH_a2_b2, - e2 = EARTH_a2_b2 / (EARTH_a*EARTH_a), - ee2 = EARTH_a2_b2 / (EARTH_b*EARTH_b); - -void ecef2elli(double X[], double *lat, double *lon, double *alt) { - double phi, lam, R, p, t; - - lam = atan2( X[1] , X[0] ); - - p = sqrt( X[0]*X[0] + X[1]*X[1] ); - t = atan2( X[2]*a , p*b ); - - phi = atan2( X[2] + ee2 * b * sin(t)*sin(t)*sin(t) , - p - e2 * a * cos(t)*cos(t)*cos(t) ); - - R = a / sqrt( 1 - e2*sin(phi)*sin(phi) ); - *alt = p / cos(phi) - R; - - *lat = phi*180/M_PI; - *lon = lam*180/M_PI; -} - -int get_GPSkoord() { - int i, k; - unsigned byte; - ui8_t XYZ_bytes[4]; - int XYZ; // 32bit - double X[3], lat, lon, alt; - ui8_t gpsVel_bytes[2]; - short vel16; // 16bit - double V[3], phi, lam, dir; - - - for (k = 0; k < 3; k++) { - - for (i = 0; i < 4; i++) { - byte = frame[pos_GPSecefX + 4*k + i]; - XYZ_bytes[i] = byte; - } - memcpy(&XYZ, XYZ_bytes, 4); - X[k] = XYZ / 100.0; - - for (i = 0; i < 2; i++) { - byte = frame[pos_GPSecefV + 2*k + i]; - gpsVel_bytes[i] = byte; - } - vel16 = gpsVel_bytes[0] | gpsVel_bytes[1] << 8; - V[k] = vel16 / 100.0; - - } - - - // ECEF-Position - ecef2elli(X, &lat, &lon, &alt); - gpx.lat = lat; - gpx.lon = lon; - gpx.alt = alt; - if ((alt < -1000) || (alt > 80000)) return -3; // plausibility-check: altitude, if ecef=(0,0,0) - - - // ECEF-Velocities - // ECEF-Vel -> NorthEastUp - phi = lat*M_PI/180.0; - lam = lon*M_PI/180.0; - gpx.vN = -V[0]*sin(phi)*cos(lam) - V[1]*sin(phi)*sin(lam) + V[2]*cos(phi); - gpx.vE = -V[0]*sin(lam) + V[1]*cos(lam); - gpx.vU = V[0]*cos(phi)*cos(lam) + V[1]*cos(phi)*sin(lam) + V[2]*sin(phi); - - // NEU -> HorDirVer - gpx.vH = sqrt(gpx.vN*gpx.vN+gpx.vE*gpx.vE); -/* - double alpha; - alpha = atan2(gpx.vN, gpx.vE)*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(gpx.vE, gpx.vN) * 180 / M_PI; - if (dir < 0) dir += 360; - gpx.vD = dir; - - gpx.numSV = frame[pos_numSats]; - - return 0; -} - -int get_GPS3() { - int err=0; - - // ((framebyte(pos_GPS3)<<8) | framebyte(pos_GPS3+1)) != pck_GPS3 ? - if ( framebyte(pos_GPS3) != ((pck_GPS3>>8) & 0xFF) ) { - gpx.crc |= crc_GPS3; - return -1; - } - - err = check_CRC(pos_GPS3, pck_GPS3); - if (err) gpx.crc |= crc_GPS3; - - err |= get_GPSkoord(); // plausibility-check: altitude, if ecef=(0,0,0) - - return err; -} - -int get_Aux() { -// -// "Ozone Sounding with Vaisala Radiosonde RS41" user's guide -// - int i, auxlen, auxcrc, count7E, pos7E; - - count7E = 0; - pos7E = pos_AUX; - - if (frametype(gpx) > 0) return 0; //pos7E == pos7611 ... - - // 7Exx: xdata - while ( pos7E < FRAME_LEN && framebyte(pos7E) == 0x7E ) { - - auxlen = framebyte(pos7E+1); - auxcrc = framebyte(pos7E+2+auxlen) | (framebyte(pos7E+2+auxlen+1)<<8); - - if ( auxcrc == crc16(pos7E+2, auxlen) ) { - if (count7E == 0) fprintf(stdout, "\n # xdata = "); - else fprintf(stdout, " # "); - - //fprintf(stdout, " # %02x : ", framebyte(pos7E+2)); - for (i = 1; i < auxlen; i++) { - ui8_t c = framebyte(pos7E+2+i); - if (c > 0x1E) fprintf(stdout, "%c", c); - } - count7E++; - pos7E += 2+auxlen+2; - } - else { - pos7E = FRAME_LEN; - gpx.crc |= crc_AUX; - } - } - - i = check_CRC(pos7E, 0x7600); // 0x76xx: 00-padding block - if (i) gpx.crc |= crc_ZERO; - - return count7E; -} - -int get_Calconf(int out) { - int i; - unsigned byte; - ui8_t calfr = 0; - ui8_t burst = 0; - ui16_t fw = 0; - int freq = 0, f0 = 0, f1 = 0; - char sondetyp[9]; - int err = 0; - - byte = framebyte(pos_CalData); - calfr = byte; - err = check_CRC(pos_FRAME, pck_FRAME); - - if (option_verbose == 3) { - fprintf(stdout, "\n"); // fflush(stdout); - fprintf(stdout, "[%5d] ", gpx.frnr); - fprintf(stdout, " 0x%02x: ", calfr); - for (i = 0; i < 16; i++) { - byte = framebyte(pos_CalData+1+i); - fprintf(stdout, "%02x ", byte); - } - if (err == 0) fprintf(stdout, "[OK]"); - else fprintf(stdout, "[NO]"); - fprintf(stdout, " "); - } - - if (out && err == 0) - { - if (calfr == 0x01 && option_verbose /*== 2*/) { - fw = framebyte(pos_CalData+6) | (framebyte(pos_CalData+7)<<8); - fprintf(stdout, ": fw 0x%04x ", fw); - } - - if (calfr == 0x02 && option_verbose /*== 2*/) { - byte = framebyte(pos_Calburst); - burst = byte; // fw >= 0x4ef5, BK irrelevant? (burst-killtimer in 0x31?) - fprintf(stdout, ": BK %02X ", burst); - if (option_verbose == 3) { // killtimer - int kt = frame[0x5A] + (frame[0x5B] << 8); // short? - if ( kt != 0xFFFF ) fprintf(stdout, ": kt 0x%04x = %dsec = %.1fmin ", kt, kt, kt/60.0); - } - } - - if (calfr == 0x00 && option_verbose) { - byte = framebyte(pos_Calfreq) & 0xC0; // erstmal nur oberste beiden bits - f0 = (byte * 10) / 64; // 0x80 -> 1/2, 0x40 -> 1/4 ; dann mal 40 - byte = framebyte(pos_Calfreq+1); - f1 = 40 * byte; - freq = 400000 + f1+f0; // kHz; - fprintf(stdout, ": fq %d ", freq); - } - - if (calfr == 0x31 && option_verbose == 3) { - int bt = frame[0x59] + (frame[0x5A] << 8); // short? - // fw >= 0x4ef5: default=[88 77]=0x7788sec=510min - if ( bt != 0x0000 ) fprintf(stdout, ": bt 0x%04x = %dsec = %.1fmin ", bt, bt, bt/60.0); - } - - if (calfr == 0x32 && option_verbose) { - ui16_t cd = frame[pos_CalData+1] + (frame[pos_CalData+2] << 8); // countdown (bt or kt) (short?) - if ( cd != 0xFFFF ) fprintf(stdout, ": cd %.1fmin ", cd/60.0); - } - - if (calfr == 0x21 && option_verbose /*== 2*/) { // eventuell noch zwei bytes in 0x22 - for (i = 0; i < 9; i++) sondetyp[i] = 0; - for (i = 0; i < 8; i++) { - byte = framebyte(pos_CalRSTyp + i); - if ((byte >= 0x20) && (byte < 0x7F)) sondetyp[i] = byte; - else if (byte == 0x00) sondetyp[i] = '\0'; - } - fprintf(stdout, ": %s ", sondetyp); - } - } - - return 0; -} - -/* ------------------------------------------------------------------------------------ */ -/* - (uses fec-lib by KA9Q) - ka9q-fec: - gcc -c init_rs_char.c - gcc -c decode_rs_char.c - -#include "fec.h" // ka9q-fec - - -void *rs; -unsigned char codeword1[rs_N], codeword2[rs_N]; - - rs = init_rs_char( 8, 0x11d, 0, 1, rs_R, 0); - - // ka9q-fec301: p(x) = p[0]x^(N-1) + ... + p[N-2]x + p[N-1] - // -> cw[i] = codeword[RS.N-1-i] - -*/ - -#define rs_N 255 -#define rs_R 24 -#define rs_K (rs_N-rs_R) - -ui8_t cw1[rs_N], cw2[rs_N]; - -int rs41_ecc(int frmlen) { -// richtige framelen wichtig fuer 0-padding - - int i, leak, ret = 0; - int errors1, errors2; - ui8_t err_pos1[rs_R], err_pos2[rs_R], - err_val1[rs_R], err_val2[rs_R]; - - - if (frmlen > FRAME_LEN) frmlen = FRAME_LEN; - cfg_rs41.frmlen = frmlen; - cfg_rs41.msglen = (frmlen-56)/2; // msgpos=56; - leak = frmlen % 2; - - for (i = frmlen; i < FRAME_LEN; i++) frame[i] = 0; // FRAME_LEN-HDR = 510 = 2*255 - - - for (i = 0; i < rs_R; i++) cw1[i] = frame[cfg_rs41.parpos+i ]; - for (i = 0; i < rs_R; i++) cw2[i] = frame[cfg_rs41.parpos+i+rs_R]; - for (i = 0; i < rs_K; i++) cw1[rs_R+i] = frame[cfg_rs41.msgpos+2*i ]; - for (i = 0; i < rs_K; i++) cw2[rs_R+i] = frame[cfg_rs41.msgpos+2*i+1]; - - errors1 = rs_decode(cw1, err_pos1, err_val1); - errors2 = rs_decode(cw2, err_pos2, err_val2); - - - if (option_ecc == 2 && (errors1 < 0 || errors2 < 0)) - { // 2nd pass - frame[pos_FRAME] = (pck_FRAME>>8)&0xFF; frame[pos_FRAME+1] = pck_FRAME&0xFF; - frame[pos_PTU] = (pck_PTU >>8)&0xFF; frame[pos_PTU +1] = pck_PTU &0xFF; - frame[pos_GPS1] = (pck_GPS1 >>8)&0xFF; frame[pos_GPS1 +1] = pck_GPS1 &0xFF; - frame[pos_GPS2] = (pck_GPS2 >>8)&0xFF; frame[pos_GPS2 +1] = pck_GPS2 &0xFF; - frame[pos_GPS3] = (pck_GPS3 >>8)&0xFF; frame[pos_GPS3 +1] = pck_GPS3 &0xFF; - // AUX-frames mit vielen Fehlern besser mit 00 auffuellen - // std-O3-AUX-frame: NDATA+7 - if (frametype() < -2) { // ft >= 0: NDATA_LEN , ft < 0: FRAME_LEN - for (i = NDATA_LEN + 7; i < FRAME_LEN-2; i++) frame[i] = 0; - } - else { // std-frm (len=320): std_ZERO-frame (7611 00..00 ECC7) - for (i = NDATA_LEN; i < FRAME_LEN; i++) frame[i] = 0; - frame[pos_ZEROstd ] = 0x76; // pck_ZEROstd - frame[pos_ZEROstd+1] = 0x11; // pck_ZEROstd - for (i = pos_ZEROstd+2; i < NDATA_LEN-2; i++) frame[i] = 0; - frame[NDATA_LEN-2] = 0xEC; // crc(pck_ZEROstd) - frame[NDATA_LEN-1] = 0xC7; // crc(pck_ZEROstd) - } - for (i = 0; i < rs_K; i++) cw1[rs_R+i] = frame[cfg_rs41.msgpos+2*i ]; - for (i = 0; i < rs_K; i++) cw2[rs_R+i] = frame[cfg_rs41.msgpos+2*i+1]; - errors1 = rs_decode(cw1, err_pos1, err_val1); - errors2 = rs_decode(cw2, err_pos2, err_val2); - } - - - // Wenn Fehler im 00-padding korrigiert wurden, - // war entweder der frame zu kurz, oder - // Fehler wurden falsch korrigiert; - // allerdings ist bei t=12 die Wahrscheinlichkeit, - // dass falsch korrigiert wurde mit 1/t! sehr gering. - - // check CRC32 - // CRC32 OK: - //for (i = 0; i < cfg_rs41.hdrlen; i++) frame[i] = data[i]; - for (i = 0; i < rs_R; i++) { - frame[cfg_rs41.parpos+ i] = cw1[i]; - frame[cfg_rs41.parpos+rs_R+i] = cw2[i]; - } - for (i = 0; i < rs_K; i++) { // cfg_rs41.msglen <= rs_K - frame[cfg_rs41.msgpos+ 2*i] = cw1[rs_R+i]; - frame[cfg_rs41.msgpos+1+2*i] = cw2[rs_R+i]; - } - if (leak) { - frame[cfg_rs41.msgpos+2*i] = cw1[rs_R+i]; - } - - - ret = errors1 + errors2; - if (errors1 < 0 || errors2 < 0) { - ret = 0; - if (errors1 < 0) ret |= 0x1; - if (errors2 < 0) ret |= 0x2; - ret = -ret; - } - - return ret; -} - -/* ------------------------------------------------------------------------------------ */ - - -int print_position(int ec) { - int i; - int err, err0, err1, err2, err3; - int output, out_mask; - int encrypted; - - err = get_FrameConf(); - - // Quick check for an encrypted packet (RS41-SGM) - // These sondes have a type 0x80 packet in place of the regular PTU packet. - if (frame[pos_PTU] == pck_ENCRYPTED){ - encrypted = 1; - // Continue with the rest of the extraction (which will result in null data) - } - - err1 = get_GPS1(); - err2 = get_GPS2(); - err3 = get_GPS3(); - - err0 = get_PTU(); - - out_mask = crc_FRAME|crc_GPS1|crc_GPS3; - output = ((gpx.crc & out_mask) != out_mask); // (!err || !err1 || !err3); - - if (output) { - - if (!err) { - fprintf(stdout, "[%5d] ", gpx.frnr); - fprintf(stdout, "(%s) ", gpx.id); - } - if (!err1) { - Gps2Date(gpx.week, gpx.gpssec, &gpx.jahr, &gpx.monat, &gpx.tag); - fprintf(stdout, "%s ", weekday[gpx.wday]); - fprintf(stdout, "%04d-%02d-%02d %02d:%02d:%06.3f", - gpx.jahr, gpx.monat, gpx.tag, gpx.std, gpx.min, gpx.sek); - if (option_verbose == 3) fprintf(stdout, " (W %d)", gpx.week); - } - if (!err3) { - fprintf(stdout, " "); - fprintf(stdout, " lat: %.5f ", gpx.lat); - fprintf(stdout, " lon: %.5f ", gpx.lon); - fprintf(stdout, " alt: %.2f ", gpx.alt); - //if (option_verbose) - { - //fprintf(stdout, " (%.1f %.1f %.1f) ", gpx.vN, gpx.vE, gpx.vU); - fprintf(stdout," vH: %4.1f D: %5.1f vV: %3.1f ", gpx.vH, gpx.vD, gpx.vU); - if (option_verbose == 3) fprintf(stdout," numSV: %02d ", gpx.numSV); - } - } - - if (encrypted) { - fprintf(stdout, " Encrypted payload (RS41-SGM) "); - } - - if (option_ptu && !err0) { - printf(" "); - if (gpx.T > -273.0) printf(" T=%.1fC ", gpx.T); - if (gpx.RH > -0.5) printf(" RH=%.0f%% ", gpx.RH); - } - - - if (option_crc) { // show CRC-checks (and RS-check) - fprintf(stdout, " # "); - if (option_ecc && ec >= 0 && (gpx.crc & 0x1F) != 0) { - int pos, blk, len, crc; // unexpected blocks - int flen = NDATA_LEN; - if (frametype() < 0) flen += XDATA_LEN; - pos = pos_FRAME; - while (pos < flen-1) { - blk = frame[pos]; // 0x80XX: encrypted block - len = frame[pos+1]; // 0x76XX: 00-padding block - crc = check_CRC(pos, blk<<8); - fprintf(stdout, " %02X%02X", frame[pos], frame[pos+1]); - fprintf(stdout, "[%d]", crc&1); - pos = pos+2+len+2; - } - } - else { - fprintf(stdout, "["); - for (i=0; i<5; i++) fprintf(stdout, "%d", (gpx.crc>>i)&1); - fprintf(stdout, "]"); - } - if (option_ecc == 2) { - if (ec > 0) fprintf(stdout, " (%d)", ec); - if (ec < 0) { - if (ec == -1) fprintf(stdout, " (-+)"); - else if (ec == -2) fprintf(stdout, " (+-)"); - else /*ec == -3*/ fprintf(stdout, " (--)"); - } - } - } - - get_Calconf(output); - - if (option_verbose > 1) get_Aux(); - - fprintf(stdout, "\n"); // fflush(stdout); - - - if (option_json) { - // Print JSON output required by auto_rx. - if ((!err && !err1 && !err3) || (!err && encrypted)) { // frame-nb/id && gps-time && gps-position (crc-)ok; 3 CRCs, RS not needed - if (option_ptu && !err0 && gpx.T > -273.0) { - printf("{ \"frame\": %d, \"id\": \"%s\", \"datetime\": \"%04d-%02d-%02dT%02d:%02d:%06.3fZ\", \"lat\": %.5f, \"lon\": %.5f, \"alt\": %.5f, \"vel_h\": %.5f, \"heading\": %.5f, \"vel_v\": %.5f, \"sats\": %d, \"temp\":%.1f ", gpx.frnr, gpx.id, gpx.jahr, gpx.monat, gpx.tag, gpx.std, gpx.min, gpx.sek, gpx.lat, gpx.lon, gpx.alt, gpx.vH, gpx.vD, gpx.vU, gpx.numSV, gpx.T ); - } else { - printf("{ \"frame\": %d, \"id\": \"%s\", \"datetime\": \"%04d-%02d-%02dT%02d:%02d:%06.3fZ\", \"lat\": %.5f, \"lon\": %.5f, \"alt\": %.5f, \"vel_h\": %.5f, \"heading\": %.5f, \"vel_v\": %.5f, \"sats\": %d ", gpx.frnr, gpx.id, gpx.jahr, gpx.monat, gpx.tag, gpx.std, gpx.min, gpx.sek, gpx.lat, gpx.lon, gpx.alt, gpx.vH, gpx.vD, gpx.vU, gpx.numSV ); - } - // Add on a field if the payload data is encrypted (and is hence invalid...) - if (encrypted){ - printf(",\"encrypted\": true"); - } - - printf("}\n"); - } - } - - } - - err |= err1 | err3; - - return err; -} - -void print_frame(int len) { - int i, ec = 0, ft; - - gpx.crc = 0; - - //frame[pos_FRAME-1] == 0x0F: len == NDATA_LEN(320) - //frame[pos_FRAME-1] == 0xF0: len == FRAME_LEN(518) - ft = frametype(); - if (ft > 2) len = NDATA_LEN; - // STD-frames mit 00 auffuellen fuer Fehlerkorrektur - if (len > NDATA_LEN && len < NDATA_LEN+XDATA_LEN-10) { - if (ft < -2) { - len = NDATA_LEN + 7; // std-O3-AUX-frame - } - } - // AUX-frames mit vielen Fehlern besser mit 00 auffuellen - - for (i = len; i < FRAME_LEN-2; i++) { - frame[i] = 0; - } - if (ft > 2 || len == NDATA_LEN) { - frame[FRAME_LEN-2] = 0; - frame[FRAME_LEN-1] = 0; - } - if (len > NDATA_LEN) len = FRAME_LEN; - else len = NDATA_LEN; - - - if (option_ecc) { - ec = rs41_ecc(len); - } - - - if (option_raw) { - if (option_ecc == 2 && ec >= 0) { - if (len < FRAME_LEN && frame[FRAME_LEN-1] != 0) len = FRAME_LEN; - } - for (i = 0; i < len; i++) { - fprintf(stdout, "%02x", frame[i]); - } - if (option_ecc) { - if (ec >= 0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); - if (option_ecc == 2) { - if (ec > 0) fprintf(stdout, " (%d)", ec); - if (ec < 0) { - if (ec == -1) fprintf(stdout, " (-+)"); - else if (ec == -2) fprintf(stdout, " (+-)"); - else /*ec == -3*/ fprintf(stdout, " (--)"); - } - } - } - fprintf(stdout, "\n"); - } - else if (option_sat) { - get_SatData(); - } - else { - print_position(ec); - } -} - - -int main(int argc, char *argv[]) { - - FILE *fp; - char *fpname = NULL; - float spb = 0.0; - char bitbuf[8]; - int bit_count = 0, - bitpos = 0, - byte_count = FRAMESTART, - ft_len = FRAME_LEN, - header_found = 0; - int bit, byte; - int frmlen = FRAME_LEN; - int headerlen; - int herrs, herr1; - int bitQ, Qerror_count; - - int k, K; - float mv; - unsigned int mv_pos, mv0_pos; - int mp = 0; - - float thres = 0.7; - - int symlen = 1; - int bitofs = 2; - int shift = 0; - - -#ifdef CYGWIN - _setmode(fileno(stdin), _O_BINARY); // _fileno(stdin) -#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, -vx, -vv (info, aux, info/conf)\n"); - fprintf(stderr, " -r, --raw\n"); - fprintf(stderr, " -i, --invert\n"); - fprintf(stderr, " --crc (check CRC)\n"); - fprintf(stderr, " --ecc (Reed-Solomon)\n"); - fprintf(stderr, " --std (std framelen)\n"); - fprintf(stderr, " --ths (peak threshold; default=%.1f)\n", thres); - fprintf(stderr, " --json (JSON output)\n"); - return 0; - } - else if ( (strcmp(*argv, "-v") == 0) || (strcmp(*argv, "--verbose") == 0) ) { - option_verbose = 1; - } - else if (strcmp(*argv, "-vx") == 0) { option_verbose = 2; } - else if (strcmp(*argv, "-vv") == 0) { option_verbose = 3; } - else if (strcmp(*argv, "-vvv") == 0) { option_verbose = 4; } - else if (strcmp(*argv, "--crc") == 0) { option_crc = 1; } - else if (strcmp(*argv, "--res") == 0) { option_res = 1; } - 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; - } - else if (strcmp(*argv, "--ecc" ) == 0) { option_ecc = 1; } - else if (strcmp(*argv, "--ecc2") == 0) { option_ecc = 2; } - else if (strcmp(*argv, "--std" ) == 0) { frmlen = 320; } // NDATA_LEN - else if (strcmp(*argv, "--std2") == 0) { frmlen = 518; } // NDATA_LEN+XDATA_LEN - else if (strcmp(*argv, "--sat") == 0) { option_sat = 1; } - else if (strcmp(*argv, "--ptu") == 0) { option_ptu = 1; } - else if (strcmp(*argv, "--json") == 0) { option_json = 1; option_ecc = 2; option_crc = 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"); - } - - - if (option_ecc) { - rs_init_RS255(); - } - - - symlen = 1; - bitofs += shift; - - headerlen = strlen(header); - K = init_buffers(header, headerlen, 2); // shape=2 - 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 (mv > thres && mp > 0) { - if (mv_pos > mv0_pos) { - - header_found = 0; - herrs = headcmp(symlen, header, headerlen, mv_pos, mv<0, 0); // symlen=1 - herr1 = 0; - if (herrs <= 3 && herrs > 0) { - herr1 = headcmp(symlen, header, headerlen, mv_pos+1, mv<0, 0); - if (herr1 < herrs) { - herrs = herr1; - herr1 = 1; - } - } - if (herrs <= 2) header_found = 1; // herrs <= 2 bitfehler in header - - if (header_found) { - - byte_count = FRAMESTART; - bit_count = 0; // byte_count*8-HEADLEN - bitpos = 0; - - Qerror_count = 0; - ft_len = frmlen; - - while ( byte_count < frmlen ) { - bitQ = read_sbit(fp, symlen, &bit, option_inv, bitofs, bit_count==0); // symlen=1 - if ( bitQ == EOF) break; - bit_count += 1; - bitbuf[bitpos] = bit; - bitpos++; - if (bitpos == BITS) { - bitpos = 0; - byte = bits2byte(bitbuf); - frame[byte_count] = byte ^ mask[byte_count % MASK_LEN]; - - byteQ[byte_count] = get_bufvar(0); - - if (byte_count > NDATA_LEN) { // Fehler erst ab minimaler framelen Zaehlen - if (byteQ[byte_count]*2 > byteQ[byte_count-300]*3) { // Var(frame)/Var(noise) ca. 1:2 - Qerror_count += 1; - } - } - - byte_count++; - } - if (Qerror_count == 4) { // framelen = 320 oder 518 - ft_len = byte_count; - Qerror_count += 1; - } - } - - print_frame(ft_len); - header_found = 0; - - while ( bit_count < BITS*(FRAME_LEN-8+24) ) { - bitQ = read_sbit(fp, symlen, &bit, option_inv, bitofs, bit_count==0); // symlen=1 - if ( bitQ == EOF) break; - bit_count++; - } - - byte_count = FRAMESTART; - } - } - } - - } - - - free_buffers(); - - fclose(fp); - - return 0; -} - diff --git a/demod/rs41dm_iq.c b/demod/rs41dm_iq.c deleted file mode 100644 index 3fba9e2..0000000 --- a/demod/rs41dm_iq.c +++ /dev/null @@ -1,1430 +0,0 @@ - -/* - * rs41 - * sync header: correlation/matched filter - * files: rs41dm_iq.c bch_ecc.c demod_iq.c demod_iq.h - * compile: - * gcc -c demod_iq.c - * gcc rs41dm_iq.c demod_iq.o -lm -o rs41dm_iq - * - * author: zilog80 - */ - -#include -#include -#include -#include - -#ifdef CYGWIN - #include // cygwin: _setmode() - #include -#endif - - -#include "demod_iq.h" - -#include "bch_ecc.c" // RS/ecc/ - - -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 jsn; // JSON output (auto_rx) -} option_t; - -typedef struct { - int typ; - int msglen; - int msgpos; - int parpos; - int hdrlen; - int frmlen; -} rscfg_t; - -rscfg_t cfg_rs41 = { 41, (320-56)/2, 56, 8, 8, 320}; // const: msgpos, parpos - - -#define NDATA_LEN 320 // std framelen 320 -#define XDATA_LEN 198 -#define FRAME_LEN (NDATA_LEN+XDATA_LEN) // max framelen 518 -/* -ui8_t //xframe[FRAME_LEN] = { 0x10, 0xB6, 0xCA, 0x11, 0x22, 0x96, 0x12, 0xF8}, = xorbyte( frame) - frame[FRAME_LEN] = { 0x86, 0x35, 0xf4, 0x40, 0x93, 0xdf, 0x1a, 0x60}; // = xorbyte(xframe) -*/ -typedef struct { - int out; - int frnr; - char id[9]; - ui8_t numSV; - int week; int gpssec; - int jahr; int monat; int tag; - int wday; - int std; int min; float sek; - double lat; double lon; double alt; - double vN; double vE; double vU; - double vH; double vD; double vD2; - float T; - ui32_t crc; - ui8_t frame[FRAME_LEN]; - ui8_t calibytes[51*16]; - ui8_t calfrchk[51]; - float ptu_Rf1; // ref-resistor f1 (750 Ohm) - float ptu_Rf2; // ref-resistor f2 (1100 Ohm) - float ptu_co1[3]; // { -243.911 , 0.187654 , 8.2e-06 } - float ptu_calT1[3]; // calibration T1 - float ptu_co2[3]; // { -243.911 , 0.187654 , 8.2e-06 } - float ptu_calT2[3]; // calibration T2-Hum - ui16_t conf_fw; // firmware - ui16_t conf_kt; // kill timer (sec) - ui16_t conf_bt; // burst timer (sec) - ui8_t conf_bk; // burst kill - ui32_t freq; // freq/kHz - char rstyp[9]; // RS41-SG, RS41-SGP - int aux; - char xdata[XDATA_LEN+16]; // xdata: aux_str1#aux_str2 ... - option_t option; -} gpx_t; - - -#define BITS 8 -#define HEADLEN 64 -#define FRAMESTART ((HEADLEN)/BITS) - -/* 10 B6 CA 11 22 96 12 F8 */ -static char header[] = "0000100001101101010100111000100001000100011010010100100000011111"; - -static ui8_t header_bytes[8] = { 0x86, 0x35, 0xf4, 0x40, 0x93, 0xdf, 0x1a, 0x60}; - -#define MASK_LEN 64 -static ui8_t mask[MASK_LEN] = { 0x96, 0x83, 0x3E, 0x51, 0xB1, 0x49, 0x08, 0x98, - 0x32, 0x05, 0x59, 0x0E, 0xF9, 0x44, 0xC6, 0x26, - 0x21, 0x60, 0xC2, 0xEA, 0x79, 0x5D, 0x6D, 0xA1, - 0x54, 0x69, 0x47, 0x0C, 0xDC, 0xE8, 0x5C, 0xF1, - 0xF7, 0x76, 0x82, 0x7F, 0x07, 0x99, 0xA2, 0x2C, - 0x93, 0x7C, 0x30, 0x63, 0xF5, 0x10, 0x2E, 0x61, - 0xD0, 0xBC, 0xB4, 0xB6, 0x06, 0xAA, 0xF4, 0x23, - 0x78, 0x6E, 0x3B, 0xAE, 0xBF, 0x7B, 0x4C, 0xC1}; -/* LFSR: ab i=8 (mod 64): - * m[16+i] = m[i] ^ m[i+2] ^ m[i+4] ^ m[i+6] - * ________________3205590EF944C6262160C2EA795D6DA15469470CDCE85CF1 - * F776827F0799A22C937C3063F5102E61D0BCB4B606AAF423786E3BAEBF7B4CC196833E51B1490898 - */ -/* - frame[pos] = xframe[pos] ^ mask[pos % MASK_LEN]; -*/ - -/* ------------------------------------------------------------------------------------ */ - -#define BAUD_RATE 4800 - -/* ------------------------------------------------------------------------------------ */ -/* - * Convert GPS Week and Seconds to Modified Julian Day. - * - Adapted from sci.astro FAQ. - * - Ignores UTC leap seconds. - */ -// in : week, gpssec -// out: jahr, monat, tag -static void Gps2Date(gpx_t *gpx) { - long GpsDays, Mjd; - long J, C, Y, M; - - GpsDays = gpx->week * 7 + (gpx->gpssec / 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; - gpx->tag = J - 2447 * M / 80; - J = M / 11; - gpx->monat = M + 2 - (12 * J); - gpx->jahr = 100 * (C - 49) + Y + J; -} -/* ------------------------------------------------------------------------------------ */ - -static int bits2byte(char bits[]) { - int i, byteval=0, d=1; - for (i = 0; i < 8; i++) { // little endian - /* for (i = 7; i >= 0; i--) { // big endian */ - if (bits[i] == 1) byteval += d; - else if (bits[i] == 0) byteval += 0; - else return 0x100; - d <<= 1; - } - return byteval; -} - -/* ------------------------------------------------------------------------------------ */ - -static ui32_t u4(ui8_t *bytes) { // 32bit unsigned int - ui32_t val = 0; - memcpy(&val, bytes, 4); - return val; -} - -static ui32_t u3(ui8_t *bytes) { // 24bit unsigned int - int val24 = 0; - val24 = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16); - // = memcpy(&val, bytes, 3), val &= 0x00FFFFFF; - return val24; -} - -static int i3(ui8_t *bytes) { // 24bit signed int - int val = 0, - val24 = 0; - val = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16); - val24 = val & 0xFFFFFF; if (val24 & 0x800000) val24 = val24 - 0x1000000; - return val24; -} - -static ui32_t u2(ui8_t *bytes) { // 16bit unsigned int - return bytes[0] | (bytes[1]<<8); -} - -/* -double r8(ui8_t *bytes) { - double val = 0; - memcpy(&val, bytes, 8); - return val; -} - -float r4(ui8_t *bytes) { - float val = 0; - memcpy(&val, bytes, 4); - return val; -} -*/ - -static int crc16(gpx_t *gpx, int start, int len) { - int crc16poly = 0x1021; - int rem = 0xFFFF, i, j; - int byte; - - if (start+len+2 > FRAME_LEN) return -1; - - for (i = 0; i < len; i++) { - byte = gpx->frame[start+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(gpx_t *gpx, ui32_t pos, ui32_t pck) { - ui32_t crclen = 0, - crcdat = 0; - if (((pck>>8) & 0xFF) != gpx->frame[pos]) return -1; - crclen = gpx->frame[pos+1]; - if (pos + crclen + 4 > FRAME_LEN) return -1; - crcdat = u2(gpx->frame+pos+2+crclen); - if ( crcdat != crc16(gpx, pos+2, crclen) ) { - return 1; // CRC NO - } - else return 0; // CRC OK -} - - -/* -GPS chip: ublox UBX-G6010-ST - - Pos: SubHeader, 1+1 byte (ID+LEN) -0x039: 7928 FrameNumber+SondeID - +(0x050: 0732 CalFrames 0x00..0x32) -0x065: 7A2A PTU -0x093: 7C1E GPS1: RXM-RAW (0x02 0x10) Week, TOW, Sats -0x0B5: 7D59 GPS2: RXM-RAW (0x02 0x10) pseudorange, doppler -0x112: 7B15 GPS3: NAV-SOL (0x01 0x06) ECEF-POS, ECEF-VEL -0x12B: 7611 00 -0x12B: 7Exx AUX-xdata -*/ - -#define crc_FRAME (1<<0) -#define xor_FRAME 0x1713 // ^0x6E3B=0x7928 -#define pck_FRAME 0x7928 -#define pos_FRAME 0x039 -#define pos_FrameNb 0x03B // 2 byte -#define pos_SondeID 0x03D // 8 byte -#define pos_CalData 0x052 // 1 byte, counter 0x00..0x32 -#define pos_Calfreq 0x055 // 2 byte, calfr 0x00 -#define pos_Calburst 0x05E // 1 byte, calfr 0x02 -// ? #define pos_Caltimer 0x05A // 2 byte, calfr 0x02 ? -#define pos_CalRSTyp 0x05B // 8 byte, calfr 0x21 (+2 byte in 0x22?) - // weitere chars in calfr 0x22/0x23; weitere ID - -#define crc_PTU (1<<1) -#define xor_PTU 0xE388 // ^0x99A2=0x0x7A2A -#define pck_PTU 0x7A2A // PTU -#define pos_PTU 0x065 - -#define crc_GPS1 (1<<2) -#define xor_GPS1 0x9667 // ^0xEA79=0x7C1E -#define pck_GPS1 0x7C1E // RXM-RAW (0x02 0x10) -#define pos_GPS1 0x093 -#define pos_GPSweek 0x095 // 2 byte -#define pos_GPSiTOW 0x097 // 4 byte -#define pos_satsN 0x09B // 12x2 byte (1: SV, 1: quality,strength) - -#define crc_GPS2 (1<<3) -#define xor_GPS2 0xD7AD // ^0xAAF4=0x7D59 -#define pck_GPS2 0x7D59 // RXM-RAW (0x02 0x10) -#define pos_GPS2 0x0B5 -#define pos_minPR 0x0B7 // 4 byte -#define pos_FF 0x0BB // 1 byte -#define pos_dataSats 0x0BC // 12x(4+3) byte (4: pseudorange, 3: doppler) - -#define crc_GPS3 (1<<4) -#define xor_GPS3 0xB9FF // ^0xC2EA=0x7B15 -#define pck_GPS3 0x7B15 // NAV-SOL (0x01 0x06) -#define pos_GPS3 0x112 -#define pos_GPSecefX 0x114 // 4 byte -#define pos_GPSecefY 0x118 // 4 byte -#define pos_GPSecefZ 0x11C // 4 byte -#define pos_GPSecefV 0x120 // 3*2 byte -#define pos_numSats 0x126 // 1 byte -#define pos_sAcc 0x127 // 1 byte -#define pos_pDOP 0x128 // 1 byte - -#define crc_AUX (1<<5) -#define pck_AUX 0x7E00 // LEN variable -#define pos_AUX 0x12B - -#define crc_ZERO (1<<6) // LEN variable -#define pck_ZERO 0x7600 -#define pck_ZEROstd 0x7611 // NDATA std-frm, no aux -#define pos_ZEROstd 0x12B // pos_AUX(0) - - -/* - frame[pos_FRAME-1] == 0x0F: len == NDATA_LEN(320) - frame[pos_FRAME-1] == 0xF0: len == FRAME_LEN(518) -*/ -static int frametype(gpx_t *gpx) { // -4..+4: 0xF0 -> -4 , 0x0F -> +4 - int i; - ui8_t b = gpx->frame[pos_FRAME-1]; - int ft = 0; - for (i = 0; i < 4; i++) { - ft += ((b>>i)&1) - ((b>>(i+4))&1); - } - return ft; -} - - -const double c = 299.792458e6; -const double L1 = 1575.42e6; - -static int get_SatData(gpx_t *gpx) { - int i, n; - int sv; - ui32_t minPR; - int numSV; - double pDOP, sAcc; - - fprintf(stdout, "[%d]\n", u2(gpx->frame+pos_FrameNb)); - - fprintf(stdout, "iTOW: 0x%08X", u4(gpx->frame+pos_GPSiTOW)); - fprintf(stdout, " week: 0x%04X", u2(gpx->frame+pos_GPSweek)); - fprintf(stdout, "\n"); - minPR = u4(gpx->frame+pos_minPR); - fprintf(stdout, "minPR: %d", minPR); - fprintf(stdout, "\n"); - - for (i = 0; i < 12; i++) { - n = i*7; - sv = gpx->frame[pos_satsN+2*i]; - if (sv == 0xFF) break; - fprintf(stdout, " SV: %2d ", sv); - //fprintf(stdout, " (%02x) ", gpx->frame[pos_satsN+2*i+1]); - fprintf(stdout, "# "); - fprintf(stdout, "prMes: %.1f", u4(gpx->frame+pos_dataSats+n)/100.0 + minPR); - fprintf(stdout, " "); - fprintf(stdout, "doMes: %.1f", -i3(gpx->frame+pos_dataSats+n+4)/100.0*L1/c); - fprintf(stdout, "\n"); - } - - fprintf(stdout, "ECEF-POS: (%d,%d,%d)\n", - (i32_t)u4(gpx->frame+pos_GPSecefX), - (i32_t)u4(gpx->frame+pos_GPSecefY), - (i32_t)u4(gpx->frame+pos_GPSecefZ)); - fprintf(stdout, "ECEF-VEL: (%d,%d,%d)\n", - (i16_t)u2(gpx->frame+pos_GPSecefV+0), - (i16_t)u2(gpx->frame+pos_GPSecefV+2), - (i16_t)u2(gpx->frame+pos_GPSecefV+4)); - - numSV = gpx->frame[pos_numSats]; - sAcc = gpx->frame[pos_sAcc]/10.0; if (gpx->frame[pos_sAcc] == 0xFF) sAcc = -1.0; - pDOP = gpx->frame[pos_pDOP]/10.0; if (gpx->frame[pos_pDOP] == 0xFF) pDOP = -1.0; - fprintf(stdout, "numSatsFix: %2d sAcc: %.1f pDOP: %.1f\n", numSV, sAcc, pDOP); - - - fprintf(stdout, "CRC: "); - fprintf(stdout, " %04X", pck_GPS1); - if (check_CRC(gpx, pos_GPS1, pck_GPS1)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); - //fprintf(stdout, "[%+d]", check_CRC(gpx, pos_GPS1, pck_GPS1)); - fprintf(stdout, " %04X", pck_GPS2); - if (check_CRC(gpx, pos_GPS2, pck_GPS2)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); - //fprintf(stdout, "[%+d]", check_CRC(gpx, pos_GPS2, pck_GPS2)); - fprintf(stdout, " %04X", pck_GPS3); - if (check_CRC(gpx, pos_GPS3, pck_GPS3)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); - //fprintf(stdout, "[%+d]", check_CRC(gpx, pos_GPS3, pck_GPS3)); - - fprintf(stdout, "\n"); - fprintf(stdout, "\n"); - - 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] + (frnr_bytes[1] << 8); - gpx->frnr = frnr; - - return 0; -} - -static int get_SondeID(gpx_t *gpx, int crc) { - int i; - unsigned byte; - char sondeid_bytes[9]; - - if (crc == 0) { - for (i = 0; i < 8; i++) { - byte = gpx->frame[pos_SondeID + i]; - //if ((byte < 0x20) || (byte > 0x7E)) return -1; - sondeid_bytes[i] = byte; - } - sondeid_bytes[8] = '\0'; - if ( strncmp(gpx->id, sondeid_bytes, 8) != 0 ) { - //for (i = 0; i < 51; i++) gpx->calfrchk[i] = 0; - memset(gpx->calfrchk, 0, 51); - memcpy(gpx->id, sondeid_bytes, 8); - gpx->id[8] = '\0'; - // conf data - gpx->conf_fw = 0; - gpx->conf_kt = -1; - gpx->conf_bt = 0; - gpx->conf_bk = 0; - gpx->freq = 0; - memset(gpx->rstyp, 0, 9); - } - } - - return 0; -} - -static int get_FrameConf(gpx_t *gpx) { - int crc, err; - ui8_t calfr; - int i; - - crc = check_CRC(gpx, pos_FRAME, pck_FRAME); - if (crc) gpx->crc |= crc_FRAME; - - err = crc; - err |= get_FrameNb(gpx); - err |= get_SondeID(gpx, crc); - - if (crc == 0) { - calfr = gpx->frame[pos_CalData]; - if (gpx->calfrchk[calfr] == 0) // const? - { // 0x32 not constant - for (i = 0; i < 16; i++) { - gpx->calibytes[calfr*16 + i] = gpx->frame[pos_CalData+1+i]; - } - gpx->calfrchk[calfr] = 1; - } - } - - return err; -} - -static int get_CalData(gpx_t *gpx) { - - memcpy(&(gpx->ptu_Rf1), gpx->calibytes+61, 4); // 0x03*0x10+13 - memcpy(&(gpx->ptu_Rf2), gpx->calibytes+65, 4); // 0x04*0x10+ 1 - - memcpy(gpx->ptu_co1+0, gpx->calibytes+77, 4); // 0x04*0x10+13 - memcpy(gpx->ptu_co1+1, gpx->calibytes+81, 4); // 0x05*0x10+ 1 - memcpy(gpx->ptu_co1+2, gpx->calibytes+85, 4); // 0x05*0x10+ 5 - - memcpy(gpx->ptu_calT1+0, gpx->calibytes+89, 4); // 0x05*0x10+ 9 - memcpy(gpx->ptu_calT1+1, gpx->calibytes+93, 4); // 0x05*0x10+13 - memcpy(gpx->ptu_calT1+2, gpx->calibytes+97, 4); // 0x06*0x10+ 1 - - memcpy(gpx->ptu_co2+0, gpx->calibytes+293, 4); // 0x12*0x10+ 5 - memcpy(gpx->ptu_co2+1, gpx->calibytes+297, 4); // 0x12*0x10+ 9 - memcpy(gpx->ptu_co2+2, gpx->calibytes+301, 4); // 0x12*0x10+13 - - memcpy(gpx->ptu_calT2+0, gpx->calibytes+305, 4); // 0x13*0x10+ 1 - memcpy(gpx->ptu_calT2+1, gpx->calibytes+309, 4); // 0x13*0x10+ 5 - memcpy(gpx->ptu_calT2+2, gpx->calibytes+313, 4); // 0x13*0x10+ 9 - - return 0; -} - -static float get_Tc0(gpx_t *gpx, ui32_t f, ui32_t f1, ui32_t f2) { - // y = (f - f1) / (f2 - f1); - // y1 = (f - f1) / f2; // = (1 - f1/f2)*y - float a = 3.9083e-3, // Pt1000 platinum resistance - b = -5.775e-7, - c = -4.183e-12; // below 0C, else C=0 - float *cal = gpx->ptu_calT1; - float Rb = (f1*gpx->ptu_Rf2-f2*gpx->ptu_Rf1)/(f2-f1), // ofs - Ra = f * (gpx->ptu_Rf2-gpx->ptu_Rf1)/(f2-f1) - Rb, - raw = Ra/1000.0, - g_r = 0.8024*cal[0] + 0.0176, // empirisch - r_o = 0.0705*cal[1] + 0.0011, // empirisch - r = raw * g_r + r_o, - t = (-a + sqrt(a*a + 4*b*(r-1)))/(2*b); // t>0: c=0 - // R/R0 = 1 + at + bt^2 + c(t-100)t^3 , R0 = 1000 Ohm, t/Celsius - return t; -} -static float get_Tc(gpx_t *gpx, ui32_t f, ui32_t f1, ui32_t f2) { - float *p = gpx->ptu_co1; - float *c = gpx->ptu_calT1; - float g = (float)(f2-f1)/(gpx->ptu_Rf2-gpx->ptu_Rf1), // gain - Rb = (f1*gpx->ptu_Rf2-f2*gpx->ptu_Rf1)/(float)(f2-f1), // ofs - Rc = f/g - Rb, - //R = (Rc + c[1]) * c[0], - //T = p[0] + p[1]*R + p[2]*R*R; - R = Rc * c[0], - T = (p[0] + p[1]*R + p[2]*R*R + c[1])*(1.0 + c[2]); - return T; -} - -static int get_PTU(gpx_t *gpx) { - int err=0, i; - int bR, bc1, bT1, - bc2, bT2; - ui32_t meas[12]; - float Tc = -273.15; - float Tc0 = -273.15; - - get_CalData(gpx); - - err = check_CRC(gpx, pos_PTU, pck_PTU); - if (err) gpx->crc |= crc_PTU; - - if (err == 0) - { - - for (i = 0; i < 12; i++) { - meas[i] = u3(gpx->frame+pos_PTU+2+3*i); - } - - bR = gpx->calfrchk[0x03] && gpx->calfrchk[0x04]; - bc1 = gpx->calfrchk[0x04] && gpx->calfrchk[0x05]; - bT1 = gpx->calfrchk[0x05] && gpx->calfrchk[0x06]; - bc2 = gpx->calfrchk[0x12] && gpx->calfrchk[0x13]; - bT2 = gpx->calfrchk[0x13]; - - if (bR && bc1 && bT1) { - Tc = get_Tc(gpx, meas[0], meas[1], meas[2]); - Tc0 = get_Tc0(gpx, meas[0], meas[1], meas[2]); - } - gpx->T = Tc; - - if (gpx->option.vbs == 4) - { - printf(" h: %8.2f # ", gpx->alt); // crc_GPS3 ? - - printf("1: %8d %8d %8d", meas[0], meas[1], meas[2]); - printf(" # "); - printf("2: %8d %8d %8d", meas[3], meas[4], meas[5]); - printf(" # "); - printf("3: %8d %8d %8d", meas[6], meas[7], meas[8]); - printf(" # "); - if (Tc > -273.0) { - printf(" T: %8.4f , T0: %8.4f ", Tc, Tc0); - } - printf("\n"); - - if (gpx->alt > -100.0) { - printf(" %9.2f ; %6.1f ; %6.1f ", gpx->alt, gpx->ptu_Rf1, gpx->ptu_Rf2); - printf("; %10.6f ; %10.6f ; %10.6f ;", gpx->ptu_calT1[0], gpx->ptu_calT1[1], gpx->ptu_calT1[2]); - printf(" %8d ; %8d ; %8d ", meas[0], meas[1], meas[2]); - printf("; %10.6f ; %10.6f ; %10.6f ;", gpx->ptu_calT2[0], gpx->ptu_calT2[1], gpx->ptu_calT2[2]); - printf(" %8d ; %8d ; %8d" , meas[6], meas[7], meas[8]); - printf("\n"); - } - } - - } - - return err; -} - -static int get_GPSweek(gpx_t *gpx) { - int i; - unsigned byte; - ui8_t gpsweek_bytes[2]; - int gpsweek; - - for (i = 0; i < 2; i++) { - byte = gpx->frame[pos_GPSweek + i]; - gpsweek_bytes[i] = byte; - } - - gpsweek = gpsweek_bytes[0] + (gpsweek_bytes[1] << 8); - //if (gpsweek < 0) { gpx->week = -1; return -1; } // (short int) - gpx->week = gpsweek; - - 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; - int ms; - - for (i = 0; i < 4; i++) { - byte = gpx->frame[pos_GPSiTOW + i]; - gpstime_bytes[i] = byte; - } - - memcpy(&gpstime, gpstime_bytes, 4); - ms = gpstime % 1000; - gpstime /= 1000; - - gpx->gpssec = gpstime; - - day = (gpstime / (24 * 3600)) % 7; - //if ((day < 0) || (day > 6)) return -1; // besser CRC-check - - gpstime %= (24*3600); - - gpx->wday = day; - gpx->std = gpstime / 3600; - gpx->min = (gpstime % 3600) / 60; - gpx->sek = gpstime % 60 + ms/1000.0; - - return 0; -} - -static int get_GPS1(gpx_t *gpx) { - int err=0; - - // ((gpx->frame[pos_GPS1]<<8) | gpx->frame[pos_GPS1+1]) != pck_GPS1 ? - if ( gpx->frame[pos_GPS1] != ((pck_GPS1>>8) & 0xFF) ) { - gpx->crc |= crc_GPS1; - return -1; - } - - err = check_CRC(gpx, pos_GPS1, pck_GPS1); - if (err) gpx->crc |= crc_GPS1; - - err |= get_GPSweek(gpx); // no plausibility-check - err |= get_GPStime(gpx); // no plausibility-check - - return err; -} - -static int get_GPS2(gpx_t *gpx) { - int err=0; - - err = check_CRC(gpx, pos_GPS2, pck_GPS2); - if (err) gpx->crc |= crc_GPS2; - - return err; -} - -#define EARTH_a 6378137.0 -#define EARTH_b 6356752.31424518 -#define EARTH_a2_b2 (EARTH_a*EARTH_a - EARTH_b*EARTH_b) - -const -double a = EARTH_a, - b = EARTH_b, - a_b = EARTH_a2_b2, - e2 = EARTH_a2_b2 / (EARTH_a*EARTH_a), - ee2 = EARTH_a2_b2 / (EARTH_b*EARTH_b); - -static void ecef2elli(double X[], double *lat, double *lon, double *alt) { - double phi, lam, R, p, t; - - lam = atan2( X[1] , X[0] ); - - p = sqrt( X[0]*X[0] + X[1]*X[1] ); - t = atan2( X[2]*a , p*b ); - - phi = atan2( X[2] + ee2 * b * sin(t)*sin(t)*sin(t) , - p - e2 * a * cos(t)*cos(t)*cos(t) ); - - R = a / sqrt( 1 - e2*sin(phi)*sin(phi) ); - *alt = p / cos(phi) - R; - - *lat = phi*180/M_PI; - *lon = lam*180/M_PI; -} - -static int get_GPSkoord(gpx_t *gpx) { - int i, k; - unsigned byte; - ui8_t XYZ_bytes[4]; - int XYZ; // 32bit - double X[3], lat, lon, alt; - ui8_t gpsVel_bytes[2]; - short vel16; // 16bit - double V[3], phi, lam, dir; - - - for (k = 0; k < 3; k++) { - - for (i = 0; i < 4; i++) { - byte = gpx->frame[pos_GPSecefX + 4*k + i]; - XYZ_bytes[i] = byte; - } - memcpy(&XYZ, XYZ_bytes, 4); - X[k] = XYZ / 100.0; - - for (i = 0; i < 2; i++) { - byte = gpx->frame[pos_GPSecefV + 2*k + i]; - gpsVel_bytes[i] = byte; - } - vel16 = gpsVel_bytes[0] | gpsVel_bytes[1] << 8; - V[k] = vel16 / 100.0; - - } - - - // ECEF-Position - ecef2elli(X, &lat, &lon, &alt); - gpx->lat = lat; - gpx->lon = lon; - gpx->alt = alt; - if ((alt < -1000) || (alt > 80000)) return -3; // plausibility-check: altitude, if ecef=(0,0,0) - - - // ECEF-Velocities - // ECEF-Vel -> NorthEastUp - phi = lat*M_PI/180.0; - lam = lon*M_PI/180.0; - gpx->vN = -V[0]*sin(phi)*cos(lam) - V[1]*sin(phi)*sin(lam) + V[2]*cos(phi); - gpx->vE = -V[0]*sin(lam) + V[1]*cos(lam); - gpx->vU = V[0]*cos(phi)*cos(lam) + V[1]*cos(phi)*sin(lam) + V[2]*sin(phi); - - // NEU -> HorDirVer - gpx->vH = sqrt(gpx->vN*gpx->vN+gpx->vE*gpx->vE); -/* - double alpha; - alpha = atan2(gpx->vN, gpx->vE)*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(gpx->vE, gpx->vN) * 180 / M_PI; - if (dir < 0) dir += 360; - gpx->vD = dir; - - gpx->numSV = gpx->frame[pos_numSats]; - - return 0; -} - -static int get_GPS3(gpx_t *gpx) { - int err=0; - - // ((gpx->frame[pos_GPS3]<<8) | gpx->frame[pos_GPS3+1]) != pck_GPS3 ? - if ( gpx->frame[pos_GPS3] != ((pck_GPS3>>8) & 0xFF) ) { - gpx->crc |= crc_GPS3; - return -1; - } - - err = check_CRC(gpx, pos_GPS3, pck_GPS3); - if (err) gpx->crc |= crc_GPS3; - - err |= get_GPSkoord(gpx); // plausibility-check: altitude, if ecef=(0,0,0) - - return err; -} - -static int get_Aux(gpx_t *gpx) { -// -// "Ozone Sounding with Vaisala Radiosonde RS41" user's guide -// - int auxlen, auxcrc, count7E, pos7E; - int i, n; - - n = 0; - count7E = 0; - pos7E = pos_AUX; - gpx->xdata[0] = '\0'; - - if (frametype(gpx) <= 0) // pos7E == pos7611, 0x7E^0x76=0x08 ... - { - // 7Exx: xdata - while ( pos7E < FRAME_LEN && gpx->frame[pos7E] == 0x7E ) { - - auxlen = gpx->frame[pos7E+1]; - auxcrc = gpx->frame[pos7E+2+auxlen] | (gpx->frame[pos7E+2+auxlen+1]<<8); - - if ( auxcrc == crc16(gpx, pos7E+2, auxlen) ) { - if (count7E == 0) fprintf(stdout, "\n # xdata = "); - else { fprintf(stdout, " # "); gpx->xdata[n++] = '#'; } // aux separator - - //fprintf(stdout, " # %02x : ", gpx->frame[pos7E+2]); - for (i = 1; i < auxlen; i++) { - ui8_t c = gpx->frame[pos7E+2+i]; // (char) or better < 0x7F - if (c > 0x1E && c < 0x7F) { // ASCII-only - fprintf(stdout, "%c", c); - gpx->xdata[n++] = c; - } - } - count7E++; - pos7E += 2+auxlen+2; - } - else { - pos7E = FRAME_LEN; - gpx->crc |= crc_AUX; - } - } - } - gpx->xdata[n] = '\0'; - - i = check_CRC(gpx, pos7E, 0x7600); // 0x76xx: 00-padding block - if (i) gpx->crc |= crc_ZERO; - - return count7E; -} - -static int get_Calconf(gpx_t *gpx, int out) { - int i; - unsigned byte; - ui8_t calfr = 0; - ui16_t fw = 0; - int freq = 0, f0 = 0, f1 = 0; - char sondetyp[9]; - int err = 0; - - byte = gpx->frame[pos_CalData]; - calfr = byte; - err = check_CRC(gpx, pos_FRAME, pck_FRAME); - - if (gpx->option.vbs == 3) { - fprintf(stdout, "\n"); // fflush(stdout); - fprintf(stdout, "[%5d] ", gpx->frnr); - fprintf(stdout, " 0x%02x: ", calfr); - for (i = 0; i < 16; i++) { - byte = gpx->frame[pos_CalData+1+i]; - fprintf(stdout, "%02x ", byte); - } - if (err == 0) fprintf(stdout, "[OK]"); - else fprintf(stdout, "[NO]"); - fprintf(stdout, " "); - } - - if (err == 0) - { - if (calfr == 0x01) { - fw = gpx->frame[pos_CalData+6] | (gpx->frame[pos_CalData+7]<<8); - if (out && gpx->option.vbs) fprintf(stdout, ": fw 0x%04x ", fw); - gpx->conf_fw = fw; - } - - if (calfr == 0x02) { - ui8_t bk = gpx->frame[pos_Calburst]; // fw >= 0x4ef5, burst-killtimer in 0x31 relevant - ui16_t kt = gpx->frame[0x5A] + (gpx->frame[0x5B] << 8); // killtimer (short?) - if (out && gpx->option.vbs) fprintf(stdout, ": BK %02X ", bk); - if (out && gpx->option.vbs && kt != 0xFFFF ) fprintf(stdout, ": kt %.1fmin ", kt/60.0); - gpx->conf_bk = bk; - gpx->conf_kt = kt; - } - - if (calfr == 0x00) { - byte = gpx->frame[pos_Calfreq] & 0xC0; // erstmal nur oberste beiden bits - f0 = (byte * 10) / 64; // 0x80 -> 1/2, 0x40 -> 1/4 ; dann mal 40 - byte = gpx->frame[pos_Calfreq+1]; - f1 = 40 * byte; - freq = 400000 + f1+f0; // kHz; - if (out && gpx->option.vbs) fprintf(stdout, ": fq %d ", freq); - gpx->freq = freq; - } - - if (calfr == 0x31) { - ui16_t bt = gpx->frame[0x59] + (gpx->frame[0x5A] << 8); // burst timer (short?) - // fw >= 0x4ef5: default=[88 77]=0x7788sec=510min - if (out && gpx->option.vbs && bt != 0x0000 && gpx->conf_bk) fprintf(stdout, ": bt %.1fmin ", bt/60.0); - gpx->conf_bt = bt; - } - - if (calfr == 0x21) { // ... eventuell noch 2 bytes in 0x22 - for (i = 0; i < 9; i++) sondetyp[i] = 0; - for (i = 0; i < 8; i++) { - byte = gpx->frame[pos_CalRSTyp + i]; - if ((byte >= 0x20) && (byte < 0x7F)) sondetyp[i] = byte; - else if (byte == 0x00) sondetyp[i] = '\0'; - } - if (out && gpx->option.vbs) fprintf(stdout, ": %s ", sondetyp); - strcpy(gpx->rstyp, sondetyp); - } - } - - return 0; -} - -/* ------------------------------------------------------------------------------------ */ - -#define rs_N 255 -#define rs_R 24 -#define rs_K (rs_N-rs_R) - -static int rs41_ecc(gpx_t *gpx, int frmlen) { -// richtige framelen wichtig fuer 0-padding - - int i, leak, ret = 0; - int errors1, errors2; - ui8_t cw1[rs_N], cw2[rs_N]; - ui8_t err_pos1[rs_R], err_pos2[rs_R], - err_val1[rs_R], err_val2[rs_R]; - - memset(cw1, 0, rs_N); - memset(cw2, 0, rs_N); - - if (frmlen > FRAME_LEN) frmlen = FRAME_LEN; - //cfg_rs41.frmlen = frmlen; - //cfg_rs41.msglen = (frmlen-56)/2; // msgpos=56; - leak = frmlen % 2; - - for (i = frmlen; i < FRAME_LEN; i++) gpx->frame[i] = 0; // FRAME_LEN-HDR = 510 = 2*255 - - - for (i = 0; i < rs_R; i++) cw1[i] = gpx->frame[cfg_rs41.parpos+i ]; - for (i = 0; i < rs_R; i++) cw2[i] = gpx->frame[cfg_rs41.parpos+i+rs_R]; - for (i = 0; i < rs_K; i++) cw1[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i ]; - for (i = 0; i < rs_K; i++) cw2[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i+1]; - - errors1 = rs_decode(cw1, err_pos1, err_val1); - errors2 = rs_decode(cw2, err_pos2, err_val2); - - - if (gpx->option.ecc == 2 && (errors1 < 0 || errors2 < 0)) - { // 2nd pass - gpx->frame[pos_FRAME] = (pck_FRAME>>8)&0xFF; gpx->frame[pos_FRAME+1] = pck_FRAME&0xFF; - gpx->frame[pos_PTU] = (pck_PTU >>8)&0xFF; gpx->frame[pos_PTU +1] = pck_PTU &0xFF; - gpx->frame[pos_GPS1] = (pck_GPS1 >>8)&0xFF; gpx->frame[pos_GPS1 +1] = pck_GPS1 &0xFF; - gpx->frame[pos_GPS2] = (pck_GPS2 >>8)&0xFF; gpx->frame[pos_GPS2 +1] = pck_GPS2 &0xFF; - gpx->frame[pos_GPS3] = (pck_GPS3 >>8)&0xFF; gpx->frame[pos_GPS3 +1] = pck_GPS3 &0xFF; - // AUX-frames mit vielen Fehlern besser mit 00 auffuellen - // std-O3-AUX-frame: NDATA+7 - if (frametype(gpx) < -2) { // ft >= 0: NDATA_LEN , ft < 0: FRAME_LEN - for (i = NDATA_LEN + 7; i < FRAME_LEN-2; i++) gpx->frame[i] = 0; - } - else { // std-frm (len=320): std_ZERO-frame (7611 00..00 ECC7) - for (i = NDATA_LEN; i < FRAME_LEN; i++) gpx->frame[i] = 0; - gpx->frame[pos_ZEROstd ] = 0x76; // pck_ZEROstd - gpx->frame[pos_ZEROstd+1] = 0x11; // pck_ZEROstd - for (i = pos_ZEROstd+2; i < NDATA_LEN-2; i++) gpx->frame[i] = 0; - gpx->frame[NDATA_LEN-2] = 0xEC; // crc(pck_ZEROstd) - gpx->frame[NDATA_LEN-1] = 0xC7; // crc(pck_ZEROstd) - } - for (i = 0; i < rs_K; i++) cw1[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i ]; - for (i = 0; i < rs_K; i++) cw2[rs_R+i] = gpx->frame[cfg_rs41.msgpos+2*i+1]; - errors1 = rs_decode(cw1, err_pos1, err_val1); - errors2 = rs_decode(cw2, err_pos2, err_val2); - } - - - // Wenn Fehler im 00-padding korrigiert wurden, - // war entweder der frame zu kurz, oder - // Fehler wurden falsch korrigiert; - // allerdings ist bei t=12 die Wahrscheinlichkeit, - // dass falsch korrigiert wurde mit 1/t! sehr gering. - - // check CRC32 - // CRC32 OK: - //for (i = 0; i < cfg_rs41.hdrlen; i++) frame[i] = data[i]; - for (i = 0; i < rs_R; i++) { - gpx->frame[cfg_rs41.parpos+ i] = cw1[i]; - gpx->frame[cfg_rs41.parpos+rs_R+i] = cw2[i]; - } - for (i = 0; i < rs_K; i++) { // cfg_rs41.msglen <= rs_K - gpx->frame[cfg_rs41.msgpos+ 2*i] = cw1[rs_R+i]; - gpx->frame[cfg_rs41.msgpos+1+2*i] = cw2[rs_R+i]; - } - if (leak) { - gpx->frame[cfg_rs41.msgpos+2*i] = cw1[rs_R+i]; - } - - - ret = errors1 + errors2; - if (errors1 < 0 || errors2 < 0) { - ret = 0; - if (errors1 < 0) ret |= 0x1; - if (errors2 < 0) ret |= 0x2; - ret = -ret; - } - - return ret; -} - -/* ------------------------------------------------------------------------------------ */ - - -static int print_position(gpx_t *gpx, int ec) { - int i; - int err, err0, err1, err2, err3; - int output, out_mask; - - gpx->out = 0; - gpx->aux = 0; - - err = get_FrameConf(gpx); - - err1 = get_GPS1(gpx); - err2 = get_GPS2(gpx); - err3 = get_GPS3(gpx); - - err0 = get_PTU(gpx); - - out_mask = crc_FRAME|crc_GPS1|crc_GPS3; - output = ((gpx->crc & out_mask) != out_mask); // (!err || !err1 || !err3); - - if (output) { - - gpx->out = 1; // cf. gpx->crc - - if (!err) { - fprintf(stdout, "[%5d] ", gpx->frnr); - fprintf(stdout, "(%s) ", gpx->id); - } - if (!err1) { - Gps2Date(gpx); - fprintf(stdout, "%s ", weekday[gpx->wday]); - fprintf(stdout, "%04d-%02d-%02d %02d:%02d:%06.3f", - gpx->jahr, gpx->monat, gpx->tag, gpx->std, gpx->min, gpx->sek); - if (gpx->option.vbs == 3) fprintf(stdout, " (W %d)", gpx->week); - } - if (!err3) { - fprintf(stdout, " "); - fprintf(stdout, " lat: %.5f ", gpx->lat); - fprintf(stdout, " lon: %.5f ", gpx->lon); - fprintf(stdout, " alt: %.2f ", gpx->alt); - //if (gpx->option.vbs) - { - //fprintf(stdout, " (%.1f %.1f %.1f) ", gpx->vN, gpx->vE, gpx->vU); - fprintf(stdout," vH: %4.1f D: %5.1f° vV: %3.1f ", gpx->vH, gpx->vD, gpx->vU); - if (gpx->option.vbs == 3) fprintf(stdout," sats: %02d ", gpx->numSV); - } - } - if (gpx->option.ptu && !err0) { - if (gpx->T > -273.0) printf(" T=%.1fC ", gpx->T); - } - - - if (gpx->option.crc) { - fprintf(stdout, " # "); - if (gpx->option.ecc && ec >= 0 && (gpx->crc & 0x1F) != 0) { - int pos, blk, len, crc; // unexpected blocks - int flen = NDATA_LEN; - if (frametype(gpx) < 0) flen += XDATA_LEN; - pos = pos_FRAME; - while (pos < flen-1) { - blk = gpx->frame[pos]; // 0x80XX: encrypted block - len = gpx->frame[pos+1]; // 0x76XX: 00-padding block - crc = check_CRC(gpx, pos, blk<<8); - fprintf(stdout, " %02X%02X", gpx->frame[pos], gpx->frame[pos+1]); - fprintf(stdout, "[%d]", crc&1); - pos = pos+2+len+2; - } - } - else { - fprintf(stdout, "["); - for (i=0; i<5; i++) fprintf(stdout, "%d", (gpx->crc>>i)&1); - fprintf(stdout, "]"); - } - if (gpx->option.ecc == 2) { - if (ec > 0) fprintf(stdout, " (%d)", ec); - if (ec < 0) { - if (ec == -1) fprintf(stdout, " (-+)"); - else if (ec == -2) fprintf(stdout, " (+-)"); - else /*ec == -3*/ fprintf(stdout, " (--)"); - } - } - } - - get_Calconf(gpx, output); - - if (gpx->option.vbs > 1 || gpx->option.jsn) { - gpx->aux = get_Aux(gpx); - //if (gpx->aux) fprintf(stdout, "\n%d: %s", gpx->aux, gpx->xdata); - } - - fprintf(stdout, "\n"); // fflush(stdout); - - - if (gpx->option.jsn) { - // Print JSON output required by auto_rx. - if (!err && !err1 && !err3) { // frame-nb/id && gps-time && gps-position (crc-)ok; 3 CRCs, RS not needed - printf("{ \"frame\": %d, \"id\": \"%s\", \"datetime\": \"%04d-%02d-%02dT%02d:%02d:%06.3fZ\", \"lat\": %.5f, \"lon\": %.5f, \"alt\": %.5f, \"vel_h\": %.5f, \"heading\": %.5f, \"vel_v\": %.5f, \"sats\": %d", gpx->frnr, gpx->id, gpx->jahr, gpx->monat, gpx->tag, gpx->std, gpx->min, gpx->sek, gpx->lat, gpx->lon, gpx->alt, gpx->vH, gpx->vD, gpx->vU, gpx->numSV ); - if (gpx->option.ptu && !err0 && gpx->T > -273.0) { - printf(", \"temp\":%.1f", gpx->T ); - } - if (gpx->aux) { // <=> gpx->xdata[0]!='\0' - printf(", \"aux\":\"%s\"", gpx->xdata ); - } - printf(" }\n"); - printf("\n"); - } - } - - } - - err |= err1 | err3; - - return err; -} - -static void print_frame(gpx_t *gpx, int len) { - int i, ec = 0, ft; - - gpx->crc = 0; - - // len < NDATA_LEN: EOF - if (len < pos_GPS1) { // else: try prev.frame - for (i = len; i < FRAME_LEN; i++) gpx->frame[i] = 0; - } - - //frame[pos_FRAME-1] == 0x0F: len == NDATA_LEN(320) - //frame[pos_FRAME-1] == 0xF0: len == FRAME_LEN(518) - ft = frametype(gpx); - if (ft >= 0) len = NDATA_LEN; // ft >= 0: NDATA_LEN (default) - else len = FRAME_LEN; // ft < 0: FRAME_LEN (aux) - - if (gpx->option.ecc) { - ec = rs41_ecc(gpx, len); - } - - - if (gpx->option.raw) { - for (i = 0; i < len; i++) { - fprintf(stdout, "%02x", gpx->frame[i]); - } - if (gpx->option.ecc) { - if (ec >= 0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); - if (gpx->option.ecc == 2) { - if (ec > 0) fprintf(stdout, " (%d)", ec); - if (ec < 0) { - if (ec == -1) fprintf(stdout, " (-+)"); - else if (ec == -2) fprintf(stdout, " (+-)"); - else /*ec == -3*/ fprintf(stdout, " (--)"); - } - } - } - fprintf(stdout, "\n"); - } - else if (gpx->option.sat) { - get_SatData(gpx); - } - else { - print_position(gpx, ec); - } -} - - -int main(int argc, char *argv[]) { - - int option_inv = 0; // invertiert Signal - int option_iq = 0; - int option_ofs = 0; - int wavloaded = 0; - int sel_wavch = 0; // audio channel: left - int rawhex = 0, xorhex = 0; - - FILE *fp; - char *fpname = NULL; - char bitbuf[8]; - int bit_count = 0, - bitpos = 0, - byte_count = FRAMESTART, - header_found = 0; - int bit, byte; - int herrs; - int bitQ; - - int k, K; - float mv; - ui32_t mv_pos, mv0_pos; - int mp = 0; - - float thres = 0.7; - - int symlen = 1; - int bitofs = 2; - int shift = 0; - - pcm_t pcm = {0}; - dsp_t dsp = {0}; - - gpx_t gpx = {0}; - - -#ifdef CYGWIN - _setmode(fileno(stdin), _O_BINARY); // _fileno(stdin) -#endif - setbuf(stdout, NULL); - - - // init gpx - memcpy(gpx.frame, header_bytes, sizeof(header_bytes)); // 8 header bytes - - - 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, -vx, -vv (info, aux, info/conf)\n"); - fprintf(stderr, " -r, --raw\n"); - fprintf(stderr, " -i, --invert\n"); - fprintf(stderr, " --crc (check CRC)\n"); - fprintf(stderr, " --ecc2 (Reed-Solomon 2-pass)\n"); - fprintf(stderr, " --ths (peak threshold; default=%.1f)\n", thres); - fprintf(stderr, " --iq0,2,3 (IQ data)\n"); - return 0; - } - else if ( (strcmp(*argv, "-v") == 0) || (strcmp(*argv, "--verbose") == 0) ) { - gpx.option.vbs = 1; - } - else if (strcmp(*argv, "-vx") == 0) { gpx.option.vbs = 2; } - else if (strcmp(*argv, "-vv") == 0) { gpx.option.vbs = 3; } - else if (strcmp(*argv, "-vvv") == 0) { gpx.option.vbs = 4; } - else if (strcmp(*argv, "--crc") == 0) { gpx.option.crc = 1; } - else if ( (strcmp(*argv, "-r") == 0) || (strcmp(*argv, "--raw") == 0) ) { - gpx.option.raw = 1; - } - else if ( (strcmp(*argv, "-i") == 0) || (strcmp(*argv, "--invert") == 0) ) { - option_inv = 1; - } - else if (strcmp(*argv, "--ecc" ) == 0) { gpx.option.ecc = 1; } - else if (strcmp(*argv, "--ecc2") == 0) { gpx.option.ecc = 2; } - else if (strcmp(*argv, "--sat") == 0) { gpx.option.sat = 1; } - else if (strcmp(*argv, "--ptu") == 0) { gpx.option.ptu = 1; } - else if (strcmp(*argv, "--json") == 0) { - gpx.option.jsn = 1; - gpx.option.ecc = 2; - gpx.option.crc = 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, "--ofs") == 0) { option_ofs = 1; } - else if (strcmp(*argv, "--rawhex") == 0) { rawhex = 2; } // raw hex input - else if (strcmp(*argv, "--xorhex") == 0) { rawhex = 2; xorhex = 1; } // raw xor input - 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.ecc < 2) gpx.option.ecc = 1; - - if (gpx.option.ecc) { - rs_init_RS255(); // ... rs_init_RS255(&RS); - } - - - - if (!rawhex) { - - 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; - } - - // rs41: BT=0.5, h=0.8,1.0 ? - symlen = 1; - - // init dsp - // - //memset(&dsp, 0, sizeof(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._spb = dsp.sps*symlen; - dsp.hdr = header; - dsp.hdrlen = strlen(header); - dsp.BT = 0.5; // bw/time (ISI) // 0.3..0.5 - dsp.h = 1.0; // 0.8..0.9? modulation index - dsp.opt_iq = option_iq; - - if ( dsp.sps < 8 ) { - fprintf(stderr, "note: sample rate low (%.1f sps)\n", dsp.sps); - } - - - K = init_buffers(&dsp); // BT=0.5 (IQ-Int: BT > 0.5 ?) - if ( K < 0 ) { - fprintf(stderr, "error: init buffers\n"); - return -1; - }; - - //if (option_iq >= 2) bitofs += 1; - bitofs += shift; // +0 .. +3 // FM: +1 , IQ: +2 - - k = 0; - mv = 0.0; - mv_pos = 0; - - while ( f32buf_sample(&dsp, option_inv) != EOF ) { - - k += 1; - if (k >= dsp.K-4) { - mv0_pos = mv_pos; - mp = getCorrDFT(&dsp, 0, &mv, &mv_pos); - k = 0; - } - else { - mv = 0.0; - continue; - } - - if (mv > thres && mp > 0) { - if (mv_pos > mv0_pos) { - - header_found = 0; - herrs = headcmp(&dsp, symlen, mv_pos, mv<0, 0); // symlen=1 - if (herrs <= 3) header_found = 1; // herrs <= 3 bitfehler in header - - if (header_found) { - - if (/*preamble &&*/ option_ofs /*option_iq*/) - { - float freq = 0.0; - float snr = 0.0; - int er = get_fqofs_rs41(&dsp, mv_pos, &freq, &snr); - } - - byte_count = FRAMESTART; - bit_count = 0; // byte_count*8-HEADLEN - bitpos = 0; - - while ( byte_count < FRAME_LEN ) { - if (option_iq >= 2) { - bitQ = read_slbit(&dsp, symlen, &bit, option_inv, bitofs, bit_count, 1.0, 0); - } - else { - bitQ = read_slbit(&dsp, symlen, &bit, option_inv, bitofs, bit_count, -1, 0); - } - if ( bitQ == EOF) break; - bit_count += 1; - bitbuf[bitpos] = bit; - bitpos++; - if (bitpos == BITS) { - bitpos = 0; - byte = bits2byte(bitbuf); - gpx.frame[byte_count] = byte ^ mask[byte_count % MASK_LEN]; - byte_count++; - } - } - - print_frame(&gpx, byte_count); - byte_count = FRAMESTART; - header_found = 0; - } - } - } - - } - - free_buffers(&dsp); - } - else //if (rawhex) - { - char buffer_rawhex[2*FRAME_LEN+12]; - char *pbuf = NULL, *buf_sp = NULL; - ui8_t frmbyte; - int frameofs = 0, len, i; - - while (1 > 0) { - - pbuf = fgets(buffer_rawhex, 2*FRAME_LEN+12, fp); - if (pbuf == NULL) break; - buffer_rawhex[2*FRAME_LEN] = '\0'; - buf_sp = strchr(buffer_rawhex, ' '); - if (buf_sp != NULL && buf_sp-buffer_rawhex < 2*FRAME_LEN) { - buffer_rawhex[buf_sp-buffer_rawhex] = '\0'; - } - len = strlen(buffer_rawhex) / 2; - if (len > pos_SondeID+10) { - for (i = 0; i < len; i++) { //%2x SCNx8=%hhx(inttypes.h) - sscanf(buffer_rawhex+2*i, "%2hhx", &frmbyte); - // wenn ohne %hhx: sscanf(buffer_rawhex+rawhex*i, "%2x", &byte); frame[frameofs+i] = (ui8_t)byte; - if (xorhex) frmbyte ^= mask[(frameofs+i) % MASK_LEN]; - gpx.frame[frameofs+i] = frmbyte; - } - print_frame(&gpx, frameofs+len); - } - } - } - - - fclose(fp); - - return 0; -} - diff --git a/demod/rs92dm.c b/demod/rs92dm.c deleted file mode 100644 index 6dc1a6a..0000000 --- a/demod/rs92dm.c +++ /dev/null @@ -1,1456 +0,0 @@ - -/* - * rs92 - * sync header: correlation/matched filter - * files: rs92dm.c nav_gps_vel.c bch_ecc.c demod.h demod.c - * compile: - * gcc -c demod.c - * gcc rs92dm.c demod.o -lm -o rs92dm - * - * author: zilog80 - */ - -#include -#include -#include -#include - -#ifdef CYGWIN - #include // cygwin: _setmode() - #include -#endif - - -typedef unsigned char ui8_t; -typedef unsigned short ui16_t; -typedef unsigned int ui32_t; - -//#include "demod.c" -#include "demod.h" - -#include "bch_ecc.c" // RS/ecc/ - -#define rs_N 255 -#define rs_R 24 -#define rs_K (rs_N-rs_R) - -ui8_t cw[rs_N]; - -typedef struct { - int typ; - int msglen; - int msgpos; - int parpos; - int hdrlen; - int frmlen; -} rscfg_t; - -rscfg_t cfg_rs92 = { 92, 240-6-24, 6, 240-24, 6, 240}; - - -typedef struct { - int frnr; - char id[11]; - int week; int gpssec; - 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 vU; - int sats[4]; - double dop; - int freq; - unsigned short aux[4]; - double diter; -} gpx_t; - -gpx_t gpx; - -int option_verbose = 0, // ausfuehrliche Anzeige - option_raw = 0, // rohe Frames - option_inv = 0, // invertiert Signal - option_res = 0, // genauere Bitmessung - option_crc = 0, // check CRC - option_ecc = 0, // Reed-Solomon ECC - fileloaded = 0, - option_vergps = 0, - option_iter = 0, - option_vel = 0, // velocity - option_aux = 0, // Aux/Ozon - option_der = 0, // linErr - option_ths = 0, - rawin = 0; -double dop_limit = 9.9; -double d_err = 10000; - -int rollover = 0, - err_gps = 0; - -int almanac = 0, - ephem = 0; - -int exSat = -1; - -/* --- RS92-SGP: 8N1 manchester --- */ -#define BITS (1+8+1) // 10 -//#define HEADLEN 60 - -#define FRAMESTART 6 //((HEADLEN)/BITS) - -/* 2A 10*/ -char rawheader[] = //"10100110011001101001" - //"10100110011001101001" - "10100110011001101001" - "10100110011001101001" - "1010011001100110100110101010100110101001"; - -#define FRAME_LEN 240 -ui8_t frame[FRAME_LEN] = { 0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 0x10}; -/* --- RS92-SGP ------------------- */ - -#define MASK_LEN 64 -ui8_t mask[MASK_LEN] = { 0x96, 0x83, 0x3E, 0x51, 0xB1, 0x49, 0x08, 0x98, - 0x32, 0x05, 0x59, 0x0E, 0xF9, 0x44, 0xC6, 0x26, - 0x21, 0x60, 0xC2, 0xEA, 0x79, 0x5D, 0x6D, 0xA1, - 0x54, 0x69, 0x47, 0x0C, 0xDC, 0xE8, 0x5C, 0xF1, - 0xF7, 0x76, 0x82, 0x7F, 0x07, 0x99, 0xA2, 0x2C, - 0x93, 0x7C, 0x30, 0x63, 0xF5, 0x10, 0x2E, 0x61, - 0xD0, 0xBC, 0xB4, 0xB6, 0x06, 0xAA, 0xF4, 0x23, - 0x78, 0x6E, 0x3B, 0xAE, 0xBF, 0x7B, 0x4C, 0xC1}; -/* LFSR: ab i=8 (mod 64): - * m[16+i] = m[i] ^ m[i+2] ^ m[i+4] ^ m[i+6] - * ________________3205590EF944C6262160C2EA795D6DA15469470CDCE85CF1 - * F776827F0799A22C937C3063F5102E61D0BCB4B606AAF423786E3BAEBF7B4CC196833E51B1490898 - */ - -/* ------------------------------------------------------------------------------------ */ - -#define BAUD_RATE 4800 - -/* ------------------------------------------------------------------------------------ */ - -// manchester1 1->10,0->01: 1.bit -// manchester2 0->10,1->01: 2.bit -// RS92-SGP: 8N1 manchester2 -int bits2byte(char bits[]) { - int i, byteval=0, d=1; - - //if (bits[0] != 0) return 0x100; // erasure? - //if (bits[9] != 1) return 0x100; // erasure? - - for (i = 1; i <= 8; i++) { // little endian - /* for (i = 8; i > 1; i--) { // big endian */ - if (bits[i] == 1) byteval += d; - else if (bits[i] == 0) byteval += 0; - d <<= 1; - } - return byteval; -} - - -/* -ui8_t xorbyte(int pos) { - return xframe[pos] ^ mask[pos % MASK_LEN]; -} -*/ -ui8_t framebyte(int pos) { - return frame[pos]; -} - - -/* ------------------------------------------------------------------------------------ */ - -#define GPS_WEEK1024 1 -#define WEEKSEC 604800 - -/* - * 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; -} - -/* ------------------------------------------------------------------------------------ */ - -#define pos_FrameNb 0x08 // 2 byte -#define pos_SondeID 0x0C // 8 byte // oder: 0x0A, 10 byte? -#define pos_CalData 0x17 // 1 byte, counter 0x00..0x1f -#define pos_Calfreq 0x1A // 2 byte, calfr 0x00 - -#define posGPS_TOW 0x48 // 4 byte -#define posGPS_PRN 0x4E // 12*5 bit in 8 byte -#define posGPS_STATUS 0x56 // 12 byte -#define posGPS_DATA 0x62 // 12*8 byte - -#define pos_PTU 0x2C // 24 byte -#define pos_AuxData 0xC8 // 8 byte - - -#define BLOCK_CFG 0x6510 // frame[pos_FrameNb-2], frame[pos_FrameNb-1] -#define BLOCK_PTU 0x690C -#define BLOCK_GPS 0x673D // frame[posGPS_TOW-2], frame[posGPS_TOW-1] -#define BLOCK_AUX 0x6805 - -#define LEN_CFG (2*(BLOCK_CFG & 0xFF)) -#define LEN_GPS (2*(BLOCK_GPS & 0xFF)) -#define LEN_PTU (2*(BLOCK_PTU & 0xFF)) - - -int crc16(int start, int len) { - int crc16poly = 0x1021; - int rem = 0xFFFF, i, j; - int byte; - - if (start+len >= FRAME_LEN) return -1; - - for (i = 0; i < len; i++) { - byte = framebyte(start+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; -} - -int get_FrameNb() { - int i; - unsigned byte; - ui8_t frnr_bytes[2]; - int frnr; - - for (i = 0; i < 2; i++) { - byte = framebyte(pos_FrameNb + i); - frnr_bytes[i] = byte; - } - - frnr = frnr_bytes[0] + (frnr_bytes[1] << 8); - gpx.frnr = frnr; - - return 0; -} - -int get_SondeID() { - int i, ret=0; - unsigned byte; - ui8_t sondeid_bytes[10]; - int crc_frame, crc; - - // BLOCK_CFG == frame[pos_FrameNb-2 .. pos_FrameNb-1] ? - crc_frame = framebyte(pos_FrameNb+LEN_CFG) | (framebyte(pos_FrameNb+LEN_CFG+1) << 8); - crc = crc16(pos_FrameNb, LEN_CFG); -/* - if (option_crc) { - //fprintf(stdout, " (%04X:%02X%02X) ", BLOCK_CFG, frame[pos_FrameNb-2], frame[pos_FrameNb-1]); - fprintf(stdout, " [%04X:%04X] ", crc_frame, crc); - } -*/ - ret = 0; - if ( 0 && option_crc && crc != crc_frame) { - ret = -2; // erst wichtig, wenn Cal/Cfg-Data - } - - for (i = 0; i < 8; i++) { - byte = framebyte(pos_SondeID + i); - if ((byte < 0x20) || (byte > 0x7E)) return -1; - sondeid_bytes[i] = byte; - } - - for (i = 0; i < 8; i++) { - gpx.id[i] = sondeid_bytes[i]; - } - gpx.id[8] = '\0'; - - return ret; -} - -char weekday[7][3] = { "So", "Mo", "Di", "Mi", "Do", "Fr", "Sa"}; - -int get_GPStime() { - int i, ret=0; - unsigned byte; - ui8_t gpstime_bytes[4]; - int gpstime = 0, // 32bit - day; - int ms; - int crc_frame, crc; - - // BLOCK_GPS == frame[posGPS_TOW-2 .. posGPS_TOW-1] ? - crc_frame = framebyte(posGPS_TOW+LEN_GPS) | (framebyte(posGPS_TOW+LEN_GPS+1) << 8); - crc = crc16(posGPS_TOW, LEN_GPS); -/* - if (option_crc) { - //fprintf(stdout, " (%04X:%02X%02X) ", BLOCK_GPS, frame[posGPS_TOW-2], frame[posGPS_TOW-1]); - fprintf(stdout, " [%04X:%04X] ", crc_frame, crc); - } -*/ - ret = 0; - if (option_crc && crc != crc_frame) { - ret = -2; - } - - for (i = 0; i < 4; i++) { - byte = framebyte(posGPS_TOW + i); - gpstime_bytes[i] = byte; - } - - memcpy(&gpstime, gpstime_bytes, 4); - ms = gpstime % 1000; - gpstime /= 1000; - - gpx.gpssec = gpstime; - - day = (gpstime / (24 * 3600)) % 7; // besser CRC-check, da auch - //if ((day < 0) || (day > 6)) return -1; // gpssec=604800,604801 beobachtet - - gpstime %= (24*3600); - - gpx.wday = day; - gpx.std = gpstime / 3600; - gpx.min = (gpstime % 3600) / 60; - gpx.sek = gpstime % 60 + ms/1000.0; - - return ret; -} - -int get_Aux() { - int i; - unsigned short byte; - - for (i = 0; i < 4; i++) { - byte = framebyte(pos_AuxData+2*i)+(framebyte(pos_AuxData+2*i+1)<<8); - gpx.aux[i] = byte; - } - - return 0; -} - -int get_Cal() { - int i; - unsigned byte; - ui8_t calfr = 0; - //ui8_t burst = 0; - ui8_t bytes[2]; - int freq = 0; - unsigned int killtime = 0; - - byte = framebyte(pos_CalData); - calfr = byte; - - if (option_verbose == 4) { - fprintf(stdout, "\n"); - fprintf(stdout, "[%5d] ", gpx.frnr); - fprintf(stdout, " 0x%02x:", calfr); - } - for (i = 0; i < 16; i++) { - byte = framebyte(pos_CalData+1+i); - if (option_verbose == 4) { - fprintf(stdout, " %02x", byte); - } - } - if (option_aux) { - get_Aux(); - if (option_verbose == 4) { - fprintf(stdout, " # "); - for (i = 0; i < 8; i++) { - byte = framebyte(pos_AuxData+i); - fprintf(stdout, "%02x ", byte); - } - } - else { - if (gpx.aux[0] != 0 || gpx.aux[1] != 0 || gpx.aux[2] != 0 || gpx.aux[3] != 0) { - fprintf(stdout, " # %04x %04x %04x %04x", gpx.aux[0], gpx.aux[1], gpx.aux[2], gpx.aux[3]); - } - } - } - - if (calfr == 0x00) { - for (i = 0; i < 2; i++) { - bytes[i] = framebyte(pos_Calfreq + i); - } - byte = bytes[0] + (bytes[1] << 8); - //fprintf(stdout, ":%04x ", byte); - freq = 400000 + 10*byte; // kHz; - gpx.freq = freq; - fprintf(stdout, " : fq %d kHz", freq); - for (i = 0; i < 2; i++) { - bytes[i] = framebyte(pos_Calfreq + 2 + i); - } - killtime = bytes[0] + (bytes[1] << 8); - if (killtime < 0xFFFF && option_verbose == 4) { - fprintf(stdout, " ; KT:%ds", killtime); - } - } - - return 0; -} - - -/* ---------------------------------------------------------------------------------------------------- */ - - -#include "nav_gps_vel.c" - -EPHEM_t alm[33]; -//EPHEM_t eph[33][24]; -EPHEM_t *ephs = NULL; - -SAT_t sat[33], - sat1s[33]; - - -ui8_t prn_le[12*5+4]; -/* le - little endian */ -int prnbits_le(ui16_t byte16, ui8_t bits[64], int block) { - int i; /* letztes bit Ueberlauf, wenn 3. PRN = 32 */ - for (i = 0; i < 15; i++) { - bits[15*block+i] = byte16 & 1; - byte16 >>= 1; - } - bits[60+block] = byte16 & 1; - return byte16 & 1; -} -ui8_t prns[12], // PRNs in data - sat_status[12]; -int prn32toggle = 0x1, ind_prn32, prn32next; -void prn12(ui8_t *prn_le, ui8_t prns[12]) { - int i, j, d; - for (i = 0; i < 12; i++) { - prns[i] = 0; - d = 1; - for (j = 0; j < 5; j++) { - if (prn_le[5*i+j]) prns[i] += d; - d <<= 1; - } - } - ind_prn32 = 32; - for (i = 0; i < 12; i++) { - // PRN-32 overflow - if ( (prns[i] == 0) && (sat_status[i] & 0x0F) ) { // 5 bit: 0..31 - if ( ((i % 3 == 2) && (prn_le[60+i/3] & 1)) // Spalte 2 - || ((i % 3 != 2) && (prn_le[5*(i+1)] & 1)) ) { // Spalte 0,1 - prns[i] = 32; ind_prn32 = i; - } - } - else if ((sat_status[i] & 0x0F) == 0) { // erste beiden bits: 0x03 ? - prns[i] = 0; - } - } - - prn32next = 0; - if (ind_prn32 < 12) { - // PRN-32 overflow - if (ind_prn32 % 3 != 2) { // -> ind_prn32<11 // vorausgesetzt im Block folgt auf PRN-32 - if ((sat_status[ind_prn32+1] & 0x0F) && prns[ind_prn32+1] > 1) { // entweder PRN-1 oder PRN-gerade - // && prns[ind_prn32+1] != 3 ? - for (j = 0; j < ind_prn32; j++) { - if (prns[j] == (prns[ind_prn32+1]^prn32toggle) && (sat_status[j] & 0x0F)) break; - } - if (j < ind_prn32) { prn32toggle ^= 0x1; } - else { - for (j = ind_prn32+2; j < 12; j++) { - if (prns[j] == (prns[ind_prn32+1]^prn32toggle) && (sat_status[j] & 0x0F)) break; - } - if (j < 12) { prn32toggle ^= 0x1; } - } - prns[ind_prn32+1] ^= prn32toggle; - /* - // nochmal testen - for (j = 0; j < ind_prn32; j++) { if (prns[j] == prns[ind_prn32+1]) break; } - if (j < ind_prn32) prns[ind_prn32+1] = 0; - else { - for (j = ind_prn32+2; j < 12; j++) { if (prns[j] == prns[ind_prn32+1]) break; } - if (j < 12) prns[ind_prn32+1] = 0; - } - if (prns[ind_prn32+1] == 0) { prn32toggle ^= 0x1; } - */ - } - prn32next = prns[ind_prn32+1]; // -> ind_prn32<11 && ind_prn32 % 3 != 2 - } - } -} - - -int calc_satpos_alm(EPHEM_t alm[], double t, SAT_t *satp) { - double X, Y, Z, vX, vY, vZ; - int j; - int week; - double cl_corr, cl_drift; - - for (j = 1; j < 33; j++) { - if (alm[j].prn > 0 && alm[j].health == 0) { // prn==j - - // Woche hat 604800 sec - if (t-alm[j].toa > WEEKSEC/2) rollover = +1; - else if (t-alm[j].toa < -WEEKSEC/2) rollover = -1; - else rollover = 0; - week = alm[j].week - rollover; - /*if (j == 1)*/ gpx.week = week + GPS_WEEK1024*1024; - - if (option_vel >= 2) { - GPS_SatellitePositionVelocity_Ephem( - week, t, alm[j], - &cl_corr, &cl_drift, &X, &Y, &Z, &vX, &vY, &vZ - ); - satp[alm[j].prn].clock_drift = cl_drift; - satp[alm[j].prn].vX = vX; - satp[alm[j].prn].vY = vY; - satp[alm[j].prn].vZ = vZ; - } - else { - GPS_SatellitePosition_Ephem( - week, t, alm[j], - &cl_corr, &X, &Y, &Z - ); - } - - satp[alm[j].prn].X = X; - satp[alm[j].prn].Y = Y; - satp[alm[j].prn].Z = Z; - satp[alm[j].prn].clock_corr = cl_corr; - - } - } - - return 0; -} - -int calc_satpos_rnx(EPHEM_t eph[][24], double t, SAT_t *satp) { - double X, Y, Z, vX, vY, vZ; - int j, i, ti; - int week; - double cl_corr, cl_drift; - double tdiff, td; - - for (j = 1; j < 33; j++) { - - // Woche hat 604800 sec - tdiff = WEEKSEC; - ti = 0; - for (i = 0; i < 24; i++) { - if (eph[j][i].prn > 0) { - if (t-eph[j][i].toe > WEEKSEC/2) rollover = +1; - else if (t-eph[j][i].toe < -WEEKSEC/2) rollover = -1; - else rollover = 0; - td = t-eph[j][i].toe - rollover*WEEKSEC; - if (td < 0) td *= -1; - - if ( td < tdiff ) { - tdiff = td; - ti = i; - week = eph[j][ti].week - rollover; - gpx.week = eph[j][ti].gpsweek - rollover; - } - } - } - - if (option_vel >= 2) { - GPS_SatellitePositionVelocity_Ephem( - week, t, eph[j][ti], - &cl_corr, &cl_drift, &X, &Y, &Z, &vX, &vY, &vZ - ); - satp[eph[j][ti].prn].clock_drift = cl_drift; - satp[eph[j][ti].prn].vX = vX; - satp[eph[j][ti].prn].vY = vY; - satp[eph[j][ti].prn].vZ = vZ; - } - else { - GPS_SatellitePosition_Ephem( - week, t, eph[j][ti], - &cl_corr, &X, &Y, &Z - ); - } - - satp[eph[j][ti].prn].X = X; - satp[eph[j][ti].prn].Y = Y; - satp[eph[j][ti].prn].Z = Z; - satp[eph[j][ti].prn].clock_corr = cl_corr; - - } - - return 0; -} - -int calc_satpos_rnx2(EPHEM_t *eph, double t, SAT_t *satp) { - double X, Y, Z, vX, vY, vZ; - int j; - int week; - double cl_corr, cl_drift; - double tdiff, td; - int count, count0, satfound; - - for (j = 1; j < 33; j++) { - - count = count0 = 0; - satfound = 0; - - // Woche hat 604800 sec - tdiff = WEEKSEC; - - while (eph[count].prn > 0) { - - if (eph[count].prn == j && eph[count].health == 0) { - - satfound += 1; - - if (t - eph[count].toe > WEEKSEC/2) rollover = +1; - else if (t - eph[count].toe < -WEEKSEC/2) rollover = -1; - else rollover = 0; - td = fabs( t - eph[count].toe - rollover*WEEKSEC); - - if ( td < tdiff ) { - tdiff = td; - week = eph[count].week - rollover; - gpx.week = eph[count].gpsweek - rollover; - count0 = count; - } - } - count += 1; - } - - if ( satfound ) - { - if (option_vel >= 2) { - GPS_SatellitePositionVelocity_Ephem( - week, t, eph[count0], - &cl_corr, &cl_drift, &X, &Y, &Z, &vX, &vY, &vZ - ); - satp[j].clock_drift = cl_drift; - satp[j].vX = vX; - satp[j].vY = vY; - satp[j].vZ = vZ; - } - else { - GPS_SatellitePosition_Ephem( - week, t, eph[count0], - &cl_corr, &X, &Y, &Z - ); - } - - satp[j].X = X; - satp[j].Y = Y; - satp[j].Z = Z; - satp[j].clock_corr = cl_corr; - satp[j].ephtime = eph[count0].toe; - } - - } - - return 0; -} - - -typedef struct { - ui32_t tow; - ui8_t status; - int chips; - int deltachips; -} RANGE_t; -RANGE_t range[33]; - -int prn[12]; // valide PRN 0,..,k-1 - - -// pseudo.range = -df*pseudo.chips -// df = lightspeed/(chips/sec)/2^10 -const double df = 299792.458/1023.0/1024.0; //0.286183844 // c=299792458m/s, 1023000chips/s -// dl = L1/(chips/sec)/4 -const double dl = 1575.42/1.023/4.0; //385.0 // GPS L1 1575.42MHz=154*10.23MHz, dl=154*10/4 - - -int get_pseudorange() { - ui32_t gpstime; - ui8_t gpstime_bytes[4]; - ui8_t pseudobytes[4]; - unsigned chipbytes, deltabytes; - int i, j, k; - ui8_t bytes[4]; - ui16_t byte16; - double pr0, prj; - - // GPS-TOW in ms - for (i = 0; i < 4; i++) { - gpstime_bytes[i] = framebyte(posGPS_TOW + i); - } - memcpy(&gpstime, gpstime_bytes, 4); - - // Sat Status - for (i = 0; i < 12; i++) { - sat_status[i] = framebyte(posGPS_STATUS + i); - } - - // PRN-Nummern - for (i = 0; i < 4; i++) { - for (j = 0; j < 2; j++) { - bytes[j] = frame[posGPS_PRN+2*i+j]; - } - memcpy(&byte16, bytes, 2); - prnbits_le(byte16, prn_le, i); - } - prn12(prn_le, prns); - - - // GPS Sat Pos (& Vel) - if (almanac) calc_satpos_alm( alm, gpstime/1000.0, sat); - if (ephem) calc_satpos_rnx2(ephs, gpstime/1000.0, sat); - - // GPS Sat Pos t -= 1s - if (option_vel == 1) { - if (almanac) calc_satpos_alm( alm, gpstime/1000.0-1, sat1s); - if (ephem) calc_satpos_rnx2(ephs, gpstime/1000.0-1, sat1s); - } - - k = 0; - for (j = 0; j < 12; j++) { - - // Pseudorange/chips - for (i = 0; i < 4; i++) { - pseudobytes[i] = frame[posGPS_DATA+8*j+i]; - } - memcpy(&chipbytes, pseudobytes, 4); - - // delta_pseudochips / 385 - for (i = 0; i < 3; i++) { - pseudobytes[i] = frame[posGPS_DATA+8*j+4+i]; - } - deltabytes = 0; // bzw. pseudobytes[3]=0 (24 bit); deltabytes & (0xFF<<24) als - memcpy(&deltabytes, pseudobytes, 3); // gemeinsamer offset relevant in --vel1 ! - - //if ( (prns[j] == 0) && (sat_status[j] & 0x0F) ) prns[j] = 32; - range[prns[j]].tow = gpstime; - range[prns[j]].status = sat_status[j]; - - if ( chipbytes == 0x7FFFFFFF || chipbytes == 0x55555555 ) { - range[prns[j]].chips = 0; - continue; - } - if (option_vergps != 8) { - if ( chipbytes > 0x10000000 && chipbytes < 0xF0000000 ) { - range[prns[j]].chips = 0; - continue; - }} - - range[prns[j]].chips = chipbytes; - range[prns[j]].deltachips = deltabytes; - -/* - if (range[prns[j]].deltachips == 0x555555) { - range[prns[j]].deltachips = 0; - continue; - } -*/ - if ( (prns[j] > 0) && ((sat_status[j] & 0x0F) == 0xF) - && (dist(sat[prns[j]].X, sat[prns[j]].Y, sat[prns[j]].Z, 0, 0, 0) > 6700000) ) - { - for (i = 0; i < k; i++) { if (prn[i] == prns[j]) break; } - if (i == k && prns[j] != exSat) { - //if ( range[prns[j]].status & 0xF0 ) // Signalstaerke > 0 ? - { - prn[k] = prns[j]; - k++; - } - } - } - - } - - - for (j = 0; j < 12; j++) { // 0x013FB0A4 - sat[prns[j]].pseudorange = /*0x01400000*/ - range[prns[j]].chips * df; - sat1s[prns[j]].pseudorange = -(range[prns[j]].chips - range[prns[j]].deltachips/dl)*df; - //+ sat[prns[j]].clock_corr - sat1s[prns[j]].clock_corr - sat[prns[j]].pseudorate = - range[prns[j]].deltachips * df / dl; - - sat[prns[j]].prn = prns[j]; - sat1s[prns[j]].prn = prns[j]; - } - - - pr0 = (double)0x01400000; - for (j = 0; j < k; j++) { - prj = sat[prn[j]].pseudorange + sat[prn[j]].clock_corr; - if (prj < pr0) pr0 = prj; - } - for (j = 0; j < k; j++) sat[prn[j]].PR = sat[prn[j]].pseudorange + sat[prn[j]].clock_corr - pr0 + 20e6; - // es kann PRNs geben, die zeitweise stark abweichende PR liefern; - // eventuell Standardabweichung ermitteln und fehlerhafte Sats weglassen - for (j = 0; j < k; j++) { // sat/sat1s... PR-check - sat1s[prn[j]].PR = sat1s[prn[j]].pseudorange + sat[prn[j]].clock_corr - pr0 + 20e6; - } - - return k; -} - -int get_GPSvel(double lat, double lon, double vel_ecef[3], - double *vH, double *vD, double *vU) { - // ECEF-Velocities - // ECEF-Vel -> NorthEastUp - double phi = lat*M_PI/180.0; - double lam = lon*M_PI/180.0; - double vN = -vel_ecef[0]*sin(phi)*cos(lam) - vel_ecef[1]*sin(phi)*sin(lam) + vel_ecef[2]*cos(phi); - double vE = -vel_ecef[0]*sin(lam) + vel_ecef[1]*cos(lam); - *vU = vel_ecef[0]*cos(phi)*cos(lam) + vel_ecef[1]*cos(phi)*sin(lam) + vel_ecef[2]*sin(phi); - // NEU -> HorDirVer - *vH = sqrt(vN*vN+vE*vE); - *vD = atan2(vE, vN) * 180 / M_PI; - if (*vD < 0) *vD += 360; - - return 0; -} - -double DOP[4]; - -int get_GPSkoord(int N) { - double lat, lon, alt, rx_cl_bias; - double vH, vD, vU; - double lat1s, lon1s, alt1s, - lat0 , lon0 , alt0 , pos0_ecef[3]; - double pos_ecef[3], pos1s_ecef[3], dpos_ecef[3], - vel_ecef[3], dvel_ecef[3]; - double gdop, gdop0 = 1000.0; - //double hdop, vdop, pdop; - int i0, i1, i2, i3, j, k, n; - int nav_ret = 0; - int num = 0; - SAT_t Sat_A[4]; - SAT_t Sat_B[12]; // N <= 12 - SAT_t Sat_B1s[12]; - SAT_t Sat_C[12]; // 11 - double diter; - int exN = -1; - - if (option_vergps == 8) { - fprintf(stdout, " sats: "); - for (j = 0; j < N; j++) fprintf(stdout, "%02d ", prn[j]); - fprintf(stdout, "\n"); - } - - gpx.lat = gpx.lon = gpx.alt = 0; - - if (option_vergps != 2) { - for (i0=0;i0 0 && gdop < gdop0) { // wenn fehlerhafter Sat, diter wohl besserer Indikator - gpx.lat = lat; - gpx.lon = lon; - gpx.alt = alt; - gpx.dop = gdop; - gpx.diter = diter; - gpx.sats[0] = prn[i0]; gpx.sats[1] = prn[i1]; gpx.sats[2] = prn[i2]; gpx.sats[3] = prn[i3]; - gdop0 = gdop; - - if (option_vel == 4) { - gpx.vH = vH; - gpx.vD = vD; - gpx.vU = vU; - } - } - } - - }}}} - } - - if (option_vergps == 8 || option_vergps == 2) { - - for (j = 0; j < N; j++) Sat_B[j] = sat[prn[j]]; - for (j = 0; j < N; j++) Sat_B1s[j] = sat1s[prn[j]]; - - NAV_bancroft1(N, Sat_B, pos_ecef, &rx_cl_bias); - ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt); - gdop = -1; - if (calc_DOPn(N, Sat_B, pos_ecef, DOP) == 0) { - gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]); - } - - NAV_LinP(N, Sat_B, pos_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); - if (option_iter) { - for (j = 0; j < 3; j++) pos_ecef[j] += dpos_ecef[j]; - ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt); - } - gpx.diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1], dpos_ecef[2]); - - // Sat mit schlechten Daten suchen - if (gpx.diter > d_err) { - if (N > 5) { // N > 4 kann auch funktionieren - for (n = 0; n < N; n++) { - k = 0; - for (j = 0; j < N; j++) { - if (j != n) { - Sat_C[k] = Sat_B[j]; - k++; - } - } - for (j = 0; j < 3; j++) pos0_ecef[j] = 0; - NAV_bancroft1(N-1, Sat_C, pos0_ecef, &rx_cl_bias); - NAV_LinP(N-1, Sat_C, pos0_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); - diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1], dpos_ecef[2]); - ecef2elli(pos0_ecef[0], pos0_ecef[1], pos0_ecef[2], &lat0, &lon0, &alt0); - if (diter < gpx.diter) { - gpx.diter = diter; - for (j = 0; j < 3; j++) pos_ecef[j] = pos0_ecef[j]; - lat = lat0; - lon = lon0; - alt = alt0; - exN = n; - } - } - if (exN >= 0) { - if (prn[exN] == prn32next) prn32toggle ^= 0x1; - for (k = exN; k < N-1; k++) { - Sat_B[k] = Sat_B[k+1]; - prn[k] = prn[k+1]; - if (option_vel == 1) { - Sat_B1s[k] = Sat_B1s[k+1]; - } - } - N = N-1; - if (calc_DOPn(N, Sat_B, pos_ecef, DOP) == 0) { - gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]); - } - } - } -/* - if (exN < 0 && prn32next > 0) { - //prn32next used in pre-fix? prn32toggle ^= 0x1; - } -*/ - } - - if (option_vel == 1) { - NAV_bancroft1(N, Sat_B1s, pos1s_ecef, &rx_cl_bias); - if (option_iter) { - NAV_LinP(N, Sat_B1s, pos1s_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); - for (j = 0; j < 3; j++) pos1s_ecef[j] += dpos_ecef[j]; - } - for (j = 0; j < 3; j++) vel_ecef[j] = pos_ecef[j] - pos1s_ecef[j]; - get_GPSvel(lat, lon, vel_ecef, &vH, &vD, &vU); - ecef2elli(pos1s_ecef[0], pos1s_ecef[1], pos1s_ecef[2], &lat1s, &lon1s, &alt1s); - if (option_vergps == 8) { - fprintf(stdout, "\ndeltachips1s lat: %.6f , lon: %.6f , alt: %.2f ", lat1s, lon1s, alt1s); - fprintf(stdout, " vH: %4.1f D: %5.1f° vV: %3.1f ", vH, vD, vU); - fprintf(stdout, "\n"); - } - } - if (option_vel >= 2) { - //fprintf(stdout, "\nP(%.1f,%.1f,%.1f) \n", pos_ecef[0], pos_ecef[1], pos_ecef[2]); - vel_ecef[0] = vel_ecef[1] = vel_ecef[2] = 0; - NAV_LinV(N, Sat_B, pos_ecef, vel_ecef, 0.0, dvel_ecef, &rx_cl_bias); - for (j=0; j<3; j++) vel_ecef[j] += dvel_ecef[j]; - //fprintf(stdout, " V(%.1f,%.1f,%.1f) ", vel_ecef[0], vel_ecef[1], vel_ecef[2]); - //fprintf(stdout, " rx_vel_bias: %.1f \n", rx_cl_bias); - /* 2. Iteration: - NAV_LinV(N, Sat_B, pos_ecef, vel_ecef, rx_cl_bias, dvel_ecef, &rx_cl_bias); - for (j=0; j<3; j++) vel_ecef[j] += dvel_ecef[j]; - //fprintf(stdout, " V(%.1f,%.1f,%.1f) ", vel_ecef[0], vel_ecef[1], vel_ecef[2]); - //fprintf(stdout, " rx_vel_bias: %.1f \n", rx_cl_bias); - */ - get_GPSvel(lat, lon, vel_ecef, &vH, &vD, &vU); - } - - if (option_vergps == 8) { - fprintf(stdout, "bancroft[%2d] lat: %.6f , lon: %.6f , alt: %.2f ", N, lat, lon, alt); - fprintf(stdout, " (d:%.1f)", gpx.diter); - if (option_vel) { - fprintf(stdout, " vH: %4.1f D: %5.1f° vV: %3.1f ", vH, vD, vU); - } - fprintf(stdout, " DOP["); - for (j = 0; j < N; j++) { - fprintf(stdout, "%d", prn[j]); - if (j < N-1) fprintf(stdout, ","); else fprintf(stdout, "] %.1f ", gdop); - } - fprintf(stdout, "\n"); - } - - if (option_vergps == 2) { - gpx.lat = lat; - gpx.lon = lon; - gpx.alt = alt; - gpx.dop = gdop; - num = N; - - if (option_vel) { - gpx.vH = vH; - gpx.vD = vD; - gpx.vU = vU; - } - } - - } - - return num; -} - - -/* ------------------------------------------------------------------------------------ */ - -int rs92_ecc(int msglen) { - - int i, ret = 0; - int errors; - ui8_t err_pos[rs_R], err_val[rs_R]; - - if (msglen > FRAME_LEN) msglen = FRAME_LEN; - for (i = msglen; i < FRAME_LEN; i++) frame[i] = 0;//xFF; - - - for (i = 0; i < rs_R; i++) cw[i] = frame[cfg_rs92.parpos+i]; - for (i = 0; i < cfg_rs92.msglen; i++) cw[rs_R+i] = frame[cfg_rs92.msgpos+i]; - - errors = rs_decode(cw, err_pos, err_val); - - //for (i = 0; i < cfg_rs92.hdrlen; i++) frame[i] = data[i]; - for (i = 0; i < rs_R; i++) frame[cfg_rs92.parpos+i] = cw[i]; - for (i = 0; i < cfg_rs92.msglen; i++) frame[cfg_rs92.msgpos+i] = cw[rs_R+i]; - - ret = errors; - - return ret; -} - -/* ------------------------------------------------------------------------------------ */ - -int print_position() { // GPS-Hoehe ueber Ellipsoid - int j, k, n = 0; - int err1, err2; - - err1 = 0; - if (!option_verbose) err1 = err_gps; - err1 |= get_FrameNb(); - err1 |= get_SondeID(); - - err2 = err1 | err_gps; - //err2 |= get_GPSweek(); - err2 |= get_GPStime(); - - if (!err2 && (almanac || ephem)) { - k = get_pseudorange(); - if (k >= 4) { - n = get_GPSkoord(k); - } - } - - if (!err1) { - fprintf(stdout, "[%5d] ", gpx.frnr); - fprintf(stdout, "(%s) ", gpx.id); - } - - if (!err2) { - if (option_verbose) { - Gps2Date(gpx.week, gpx.gpssec, &gpx.jahr, &gpx.monat, &gpx.tag); - //fprintf(stdout, "(W %d) ", gpx.week); - fprintf(stdout, "(%04d-%02d-%02d) ", gpx.jahr, gpx.monat, gpx.tag); - } - fprintf(stdout, "%s ", weekday[gpx.wday]); // %04.1f: wenn sek >= 59.950, wird auf 60.0 gerundet - fprintf(stdout, "%02d:%02d:%06.3f", gpx.std, gpx.min, gpx.sek); - - if (n > 0) { - fprintf(stdout, " "); - - if (almanac) fprintf(stdout, " lat: %.4f lon: %.4f alt: %.1f ", gpx.lat, gpx.lon, gpx.alt); - else fprintf(stdout, " lat: %.5f lon: %.5f alt: %.1f ", gpx.lat, gpx.lon, gpx.alt); - - if (option_verbose && option_vergps != 8) { - fprintf(stdout, " (d:%.1f)", gpx.diter); - } - if (option_vel /*&& option_vergps >= 2*/) { - fprintf(stdout," vH: %4.1f D: %5.1f° vV: %3.1f ", gpx.vH, gpx.vD, gpx.vU); - } - if (option_verbose) { - if (option_vergps != 2) { - fprintf(stdout, " DOP[%02d,%02d,%02d,%02d] %.1f", - gpx.sats[0], gpx.sats[1], gpx.sats[2], gpx.sats[3], gpx.dop); - } - else { // wenn option_vergps=2, dann n=N=k(-1) - fprintf(stdout, " DOP["); - for (j = 0; j < n; j++) { - fprintf(stdout, "%d", prn[j]); - if (j < n-1) fprintf(stdout, ","); else fprintf(stdout, "] %.1f ", gpx.dop); - } - } - } - } - - get_Cal(); - - if (option_vergps == 8 /*|| option_vergps == 2*/) - { - fprintf(stdout, "\n"); - for (j = 0; j < 60; j++) { fprintf(stdout, "%d", prn_le[j]); if (j % 5 == 4) fprintf(stdout, " "); } - fprintf(stdout, ": "); - for (j = 0; j < 12; j++) fprintf(stdout, "%2d ", prns[j]); - fprintf(stdout, "\n"); - fprintf(stdout, " status: "); - for (j = 0; j < 12; j++) fprintf(stdout, "%02X ", sat_status[j]); //range[prns[j]].status - fprintf(stdout, "\n"); - } - - } - - if (!err1) { - fprintf(stdout, "\n"); - //if (option_vergps == 8) fprintf(stdout, "\n"); - } - - return err2; -} - -void print_frame(int len) { - int i, ret = 0; - ui8_t byte; - - if (option_ecc) { - ret = rs92_ecc(len); - } - - for (i = len; i < FRAME_LEN; i++) { - frame[i] = 0; - } - - if (option_raw) { -/* - for (i = 0; i < len; i++) { - byte = framebyte(i); - fprintf(stdout, "%02x", byte); - } - fprintf(stdout, "\n"); -*/ - for (i = 0; i < len; i++) { - //byte = frame[i]; - byte = framebyte(i); - fprintf(stdout, "%02x", byte); - } - if (option_ecc && option_verbose) { - fprintf(stdout, " "); - if (ret >= 0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); - fprintf(stdout, " errors: %d", ret); - } - fprintf(stdout, "\n"); -// fprintf(stdout, "\n"); - } - else print_position(); -} - - -int main(int argc, char *argv[]) { - - FILE *fp, *fp_alm = NULL, *fp_eph = NULL; - char *fpname = NULL; - float spb = 0.0; - char bitbuf[BITS]; - int bit_count = 0, - bitpos = 0, - byte_count = FRAMESTART, - header_found = 0; - int bit, byte; - int bitQ; - int herrs, herr1; - int headerlen = 0; - - float mv; - unsigned int mv_pos, mv0_pos; - - float thres = 0.7; - - int bitofs = 0, dif = 0; - int symlen = 2; - - -#ifdef CYGWIN - _setmode(_fileno(stdin), _O_BINARY); -#endif - setbuf(stdout, NULL); - - fpname = argv[0]; - ++argv; - while ((*argv) && (!fileloaded)) { - if ( (strcmp(*argv, "-h") == 0) || (strcmp(*argv, "--help") == 0) ) { - fprintf(stderr, "%s [options] \n", fpname); - fprintf(stderr, " file: audio.wav or raw_data\n"); - fprintf(stderr, " options:\n"); - fprintf(stderr, " --vel; --vel1, --vel2 (-g2)\n"); - fprintf(stderr, " -v, -vx, -vv\n"); - fprintf(stderr, " -r, --raw\n"); - fprintf(stderr, " -i, --invert\n"); - fprintf(stderr, " -a, --almanac \n"); - fprintf(stderr, " -e, --ephem \n"); - fprintf(stderr, " -g1 (verbose GPS: 4 sats)\n"); - fprintf(stderr, " -g2 (verbose GPS: all sats)\n"); - fprintf(stderr, " -gg (vverbose GPS)\n"); - fprintf(stderr, " --crc (CRC check GPS)\n"); - fprintf(stderr, " --ecc (Reed-Solomon)\n"); - fprintf(stderr, " --ths (peak threshold; default=%.1f)\n", thres); - return 0; - } - else if ( (strcmp(*argv, "--vel") == 0) ) { - option_vel = 4; - } - else if ( (strcmp(*argv, "--vel1") == 0) ) { - option_vel = 1; - if (option_vergps < 1) option_vergps = 2; - } - else if ( (strcmp(*argv, "--vel2") == 0) ) { - option_vel = 2; - if (option_vergps < 1) option_vergps = 2; - } - else if ( (strcmp(*argv, "--iter") == 0) ) { - option_iter = 1; - } - else if ( (strcmp(*argv, "-v") == 0) ) { - option_verbose = 1; - } - else if ( (strcmp(*argv, "-vv") == 0) ) { - option_verbose = 4; - } - else if ( (strcmp(*argv, "-vx") == 0) ) { - option_aux = 1; - } - else if (strcmp(*argv, "--crc") == 0) { option_crc = 1; } - 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; - } - else if ( (strcmp(*argv, "-a") == 0) || (strcmp(*argv, "--almanac") == 0) ) { - ++argv; - if (*argv) fp_alm = fopen(*argv, "r"); // txt-mode - else return -1; - if (fp_alm == NULL) fprintf(stderr, "[almanac] %s konnte nicht geoeffnet werden\n", *argv); - } - else if ( (strcmp(*argv, "-e") == 0) || (strncmp(*argv, "--ephem", 7) == 0) ) { - ++argv; - if (*argv) fp_eph = fopen(*argv, "rb"); // bin-mode - else return -1; - if (fp_eph == NULL) fprintf(stderr, "[rinex] %s konnte nicht geoeffnet werden\n", *argv); - } - else if ( (strcmp(*argv, "--dop") == 0) ) { - ++argv; - if (*argv) { - dop_limit = atof(*argv); - if (dop_limit <= 0 || dop_limit >= 100) dop_limit = 9.9; - } - else return -1; - } - else if ( (strcmp(*argv, "--der") == 0) ) { - ++argv; - if (*argv) { - d_err = atof(*argv); - if (d_err <= 0 || d_err >= 100000) d_err = 10000; - else option_der = 1; - } - else return -1; - } - else if ( (strcmp(*argv, "--exsat") == 0) ) { - ++argv; - if (*argv) { - exSat = atoi(*argv); - if (exSat < 1 || exSat > 32) exSat = -1; - } - else return -1; - } - else if (strcmp(*argv, "-g1") == 0) { option_vergps = 1; } // verbose1 GPS - else if (strcmp(*argv, "-g2") == 0) { option_vergps = 2; } // verbose2 GPS (bancroft) - else if (strcmp(*argv, "-gg") == 0) { option_vergps = 8; } // vverbose GPS - else if (strcmp(*argv, "--ecc") == 0) { option_ecc = 1; } - else if (strcmp(*argv, "--ths") == 0) { - ++argv; - if (*argv) { - thres = atof(*argv); - } - else return -1; - } - else { - if (!rawin) fp = fopen(*argv, "rb"); - else fp = fopen(*argv, "r"); - if (fp == NULL) { - fprintf(stderr, "%s konnte nicht geoeffnet werden\n", *argv); - return -1; - } - fileloaded = 1; - } - ++argv; - } - if (!fileloaded) fp = stdin; - - if (fp_alm) { - if (read_SEMalmanac(fp_alm, alm) == 0) { - almanac = 1; - } - fclose(fp_alm); - if (!option_der) d_err = 4000; - } - if (fp_eph) { - /* i = read_RNXephemeris(fp_eph, eph); - if (i == 0) { - ephem = 1; - almanac = 0; - } - fclose(fp_eph); */ - ephs = read_RNXpephs(fp_eph); - if (ephs) { - ephem = 1; - almanac = 0; - } - fclose(fp_eph); - if (!option_der) d_err = 1000; - } - - - 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"); - } - - - if (option_ecc) { - rs_init_RS255(); - } - - - symlen = 2; - headerlen = strlen(rawheader); - bitofs = 2; // +1 .. +2 - if (init_buffers(rawheader, headerlen, 2) < 0) { // shape=2 - 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 <= 2) header_found = 1; // herrs <= 2 bitfehler in header - - if (header_found) { - - byte_count = FRAMESTART; - bit_count = 0; - bitpos = 0; - - while ( byte_count < FRAME_LEN ) { - header_found = !(byte_count>=FRAME_LEN-2); - bitQ = read_sbit(fp, symlen, &bit, option_inv, bitofs, bit_count==0, !header_found); // symlen=2, return: zeroX/bit - if ( bitQ == EOF) break; - bit_count += 1; - bitbuf[bitpos] = bit; - bitpos++; - if (bitpos >= BITS) { - bitpos = 0; - byte = bits2byte(bitbuf); - frame[byte_count] = byte; - byte_count++; - } - } - header_found = 0; - print_frame(byte_count); - byte_count = FRAMESTART; - - } - } - } - - } - - - free_buffers(); - - free(ephs); - fclose(fp); - - return 0; -} - - diff --git a/demod/rs92dm_dft.c b/demod/rs92dm_dft.c deleted file mode 100644 index dbde296..0000000 --- a/demod/rs92dm_dft.c +++ /dev/null @@ -1,1497 +0,0 @@ - -/* - * rs92 - * sync header: correlation/matched filter - * files: rs92dm_dft.c nav_gps_vel.c bch_ecc.c demod_dft.h demod_dft.c - * compile: - * gcc -c demod_dft.c - * gcc rs92dm_dft.c demod_dft.o -lm -o rs92dm_dft - * - * author: zilog80 - */ - -#include -#include -#include -#include - -#ifdef CYGWIN - #include // cygwin: _setmode() - #include -#endif - - -typedef unsigned char ui8_t; -typedef unsigned short ui16_t; -typedef unsigned int ui32_t; - -//#include "demod_dft.c" -#include "demod_dft.h" - -#include "bch_ecc.c" // RS/ecc/ - -#define rs_N 255 -#define rs_R 24 -#define rs_K (rs_N-rs_R) - -ui8_t cw[rs_N]; - -typedef struct { - int typ; - int msglen; - int msgpos; - int parpos; - int hdrlen; - int frmlen; -} rscfg_t; - -rscfg_t cfg_rs92 = { 92, 240-6-24, 6, 240-24, 6, 240}; - - -typedef struct { - int frnr; - char id[11]; - int week; int gpssec; - 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 vU; - int sats[4]; - double dop; - int freq; - unsigned short aux[4]; - double diter; -} gpx_t; - -gpx_t gpx; - -int option_verbose = 0, // ausfuehrliche Anzeige - option_raw = 0, // rohe Frames - option_inv = 0, // invertiert Signal - option_res = 0, // genauere Bitmessung - option_crc = 0, // check CRC - option_ecc = 0, // Reed-Solomon ECC - fileloaded = 0, - option_vergps = 0, - option_iter = 0, - option_vel = 0, // velocity - option_aux = 0, // Aux/Ozon - option_der = 0, // linErr - option_ths = 0, - option_json = 0, // JSON output (auto_rx) - rawin = 0; -int wav_channel = 0; // audio channel: left -double dop_limit = 9.9; -double d_err = 10000; - -int rollover = 0, - err_gps = 0; - -int almanac = 0, - ephem = 0; - -int exSat = -1; - -/* --- RS92-SGP: 8N1 manchester --- */ -#define BITS (1+8+1) // 10 -//#define HEADLEN 60 - -#define FRAMESTART 6 //((HEADLEN)/BITS) - -/* 2A 10*/ -char rawheader[] = //"10100110011001101001" - //"10100110011001101001" - //"10100110011001101001" - "10100110011001101001" - "1010011001100110100110101010100110101001"; - -#define FRAME_LEN 240 -ui8_t frame[FRAME_LEN] = { 0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 0x10}; -/* --- RS92-SGP ------------------- */ - -#define MASK_LEN 64 -ui8_t mask[MASK_LEN] = { 0x96, 0x83, 0x3E, 0x51, 0xB1, 0x49, 0x08, 0x98, - 0x32, 0x05, 0x59, 0x0E, 0xF9, 0x44, 0xC6, 0x26, - 0x21, 0x60, 0xC2, 0xEA, 0x79, 0x5D, 0x6D, 0xA1, - 0x54, 0x69, 0x47, 0x0C, 0xDC, 0xE8, 0x5C, 0xF1, - 0xF7, 0x76, 0x82, 0x7F, 0x07, 0x99, 0xA2, 0x2C, - 0x93, 0x7C, 0x30, 0x63, 0xF5, 0x10, 0x2E, 0x61, - 0xD0, 0xBC, 0xB4, 0xB6, 0x06, 0xAA, 0xF4, 0x23, - 0x78, 0x6E, 0x3B, 0xAE, 0xBF, 0x7B, 0x4C, 0xC1}; -/* LFSR: ab i=8 (mod 64): - * m[16+i] = m[i] ^ m[i+2] ^ m[i+4] ^ m[i+6] - * ________________3205590EF944C6262160C2EA795D6DA15469470CDCE85CF1 - * F776827F0799A22C937C3063F5102E61D0BCB4B606AAF423786E3BAEBF7B4CC196833E51B1490898 - */ - -/* ------------------------------------------------------------------------------------ */ - -#define BAUD_RATE 4800 - -/* ------------------------------------------------------------------------------------ */ - -// manchester1 1->10,0->01: 1.bit -// manchester2 0->10,1->01: 2.bit -// RS92-SGP: 8N1 manchester2 -int bits2byte(char bits[]) { - int i, byteval=0, d=1; - - //if (bits[0] != 0) return 0x100; // erasure? - //if (bits[9] != 1) return 0x100; // erasure? - - for (i = 1; i <= 8; i++) { // little endian - /* for (i = 8; i > 1; i--) { // big endian */ - if (bits[i] == 1) byteval += d; - else if (bits[i] == 0) byteval += 0; - d <<= 1; - } - return byteval; -} - - -/* -ui8_t xorbyte(int pos) { - return xframe[pos] ^ mask[pos % MASK_LEN]; -} -*/ -ui8_t framebyte(int pos) { - return frame[pos]; -} - - -/* ------------------------------------------------------------------------------------ */ - -#define GPS_WEEK1024 1 -#define WEEKSEC 604800 - -/* - * 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; -} - -/* ------------------------------------------------------------------------------------ */ - -#define pos_FrameNb 0x08 // 2 byte -#define pos_SondeID 0x0C // 8 byte // oder: 0x0A, 10 byte? -#define pos_CalData 0x17 // 1 byte, counter 0x00..0x1f -#define pos_Calfreq 0x1A // 2 byte, calfr 0x00 - -#define posGPS_TOW 0x48 // 4 byte -#define posGPS_PRN 0x4E // 12*5 bit in 8 byte -#define posGPS_STATUS 0x56 // 12 byte -#define posGPS_DATA 0x62 // 12*8 byte - -#define pos_PTU 0x2C // 24 byte -#define pos_AuxData 0xC8 // 8 byte - - -#define BLOCK_CFG 0x6510 // frame[pos_FrameNb-2], frame[pos_FrameNb-1] -#define BLOCK_PTU 0x690C -#define BLOCK_GPS 0x673D // frame[posGPS_TOW-2], frame[posGPS_TOW-1] -#define BLOCK_AUX 0x6805 - -#define LEN_CFG (2*(BLOCK_CFG & 0xFF)) -#define LEN_GPS (2*(BLOCK_GPS & 0xFF)) -#define LEN_PTU (2*(BLOCK_PTU & 0xFF)) - - -int crc16(int start, int len) { - int crc16poly = 0x1021; - int rem = 0xFFFF, i, j; - int byte; - - if (start+len >= FRAME_LEN) return -1; - - for (i = 0; i < len; i++) { - byte = framebyte(start+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; -} - -int get_FrameNb() { - int i; - unsigned byte; - ui8_t frnr_bytes[2]; - int frnr; - - for (i = 0; i < 2; i++) { - byte = framebyte(pos_FrameNb + i); - frnr_bytes[i] = byte; - } - - frnr = frnr_bytes[0] + (frnr_bytes[1] << 8); - gpx.frnr = frnr; - - return 0; -} - -int get_SondeID() { - int i, ret=0; - unsigned byte; - ui8_t sondeid_bytes[10]; - int crc_frame, crc; - - // BLOCK_CFG == frame[pos_FrameNb-2 .. pos_FrameNb-1] ? - crc_frame = framebyte(pos_FrameNb+LEN_CFG) | (framebyte(pos_FrameNb+LEN_CFG+1) << 8); - crc = crc16(pos_FrameNb, LEN_CFG); -/* - if (option_crc) { - //fprintf(stdout, " (%04X:%02X%02X) ", BLOCK_CFG, frame[pos_FrameNb-2], frame[pos_FrameNb-1]); - fprintf(stdout, " [%04X:%04X] ", crc_frame, crc); - } -*/ - ret = 0; - if ( /*0 &&*/ option_crc && crc != crc_frame) { - ret = -2; // erst wichtig, wenn Cal/Cfg-Data - } - - for (i = 0; i < 8; i++) { - byte = framebyte(pos_SondeID + i); - if ((byte < 0x20) || (byte > 0x7E)) return -1; - sondeid_bytes[i] = byte; - } - - for (i = 0; i < 8; i++) { - gpx.id[i] = sondeid_bytes[i]; - } - gpx.id[8] = '\0'; - - return ret; -} - -char weekday[7][3] = { "So", "Mo", "Di", "Mi", "Do", "Fr", "Sa"}; - -int get_GPStime() { - int i, ret=0; - unsigned byte; - ui8_t gpstime_bytes[4]; - int gpstime = 0, // 32bit - day; - int ms; - int crc_frame, crc; - - // BLOCK_GPS == frame[posGPS_TOW-2 .. posGPS_TOW-1] ? - crc_frame = framebyte(posGPS_TOW+LEN_GPS) | (framebyte(posGPS_TOW+LEN_GPS+1) << 8); - crc = crc16(posGPS_TOW, LEN_GPS); -/* - if (option_crc) { - //fprintf(stdout, " (%04X:%02X%02X) ", BLOCK_GPS, frame[posGPS_TOW-2], frame[posGPS_TOW-1]); - fprintf(stdout, " [%04X:%04X] ", crc_frame, crc); - } -*/ - ret = 0; - if (option_crc && crc != crc_frame) { - ret = -2; - } - - for (i = 0; i < 4; i++) { - byte = framebyte(posGPS_TOW + i); - gpstime_bytes[i] = byte; - } - - memcpy(&gpstime, gpstime_bytes, 4); - ms = gpstime % 1000; - gpstime /= 1000; - - gpx.gpssec = gpstime; - - day = (gpstime / (24 * 3600)) % 7; // besser CRC-check, da auch - //if ((day < 0) || (day > 6)) return -1; // gpssec=604800,604801 beobachtet - - gpstime %= (24*3600); - - gpx.wday = day; - gpx.std = gpstime / 3600; - gpx.min = (gpstime % 3600) / 60; - gpx.sek = gpstime % 60 + ms/1000.0; - - return ret; -} - -int get_Aux() { - int i; - unsigned short byte; - - for (i = 0; i < 4; i++) { - byte = framebyte(pos_AuxData+2*i)+(framebyte(pos_AuxData+2*i+1)<<8); - gpx.aux[i] = byte; - } - - return 0; -} - -int get_Cal() { - int i; - unsigned byte; - ui8_t calfr = 0; - //ui8_t burst = 0; - ui8_t bytes[2]; - int freq = 0; - unsigned int killtime = 0; - - byte = framebyte(pos_CalData); - calfr = byte; - - if (option_verbose == 4) { - fprintf(stdout, "\n"); - fprintf(stdout, "[%5d] ", gpx.frnr); - fprintf(stdout, " 0x%02x:", calfr); - } - for (i = 0; i < 16; i++) { - byte = framebyte(pos_CalData+1+i); - if (option_verbose == 4) { - fprintf(stdout, " %02x", byte); - } - } - if (option_aux) { - get_Aux(); - if (option_verbose == 4) { - fprintf(stdout, " # "); - for (i = 0; i < 8; i++) { - byte = framebyte(pos_AuxData+i); - fprintf(stdout, "%02x ", byte); - } - } - else { - if (gpx.aux[0] != 0 || gpx.aux[1] != 0 || gpx.aux[2] != 0 || gpx.aux[3] != 0) { - fprintf(stdout, " # %04x %04x %04x %04x", gpx.aux[0], gpx.aux[1], gpx.aux[2], gpx.aux[3]); - } - } - } - - if (calfr == 0x00) { - for (i = 0; i < 2; i++) { - bytes[i] = framebyte(pos_Calfreq + i); - } - byte = bytes[0] + (bytes[1] << 8); - //fprintf(stdout, ":%04x ", byte); - freq = 400000 + 10*byte; // kHz; - gpx.freq = freq; - fprintf(stdout, " : fq %d kHz", freq); - for (i = 0; i < 2; i++) { - bytes[i] = framebyte(pos_Calfreq + 2 + i); - } - killtime = bytes[0] + (bytes[1] << 8); - if (killtime < 0xFFFF && option_verbose == 4) { - fprintf(stdout, " ; KT:%ds", killtime); - } - } - - return 0; -} - - -/* ---------------------------------------------------------------------------------------------------- */ - - -#include "nav_gps_vel.c" - -EPHEM_t alm[33]; -//EPHEM_t eph[33][24]; -EPHEM_t *ephs = NULL; - -SAT_t sat[33], - sat1s[33]; - - -ui8_t prn_le[12*5+4]; -/* le - little endian */ -int prnbits_le(ui16_t byte16, ui8_t bits[64], int block) { - int i; /* letztes bit Ueberlauf, wenn 3. PRN = 32 */ - for (i = 0; i < 15; i++) { - bits[15*block+i] = byte16 & 1; - byte16 >>= 1; - } - bits[60+block] = byte16 & 1; - return byte16 & 1; -} -ui8_t prns[12], // PRNs in data - sat_status[12]; -int prn32toggle = 0x1, ind_prn32, prn32next; -void prn12(ui8_t *prn_le, ui8_t prns[12]) { - int i, j, d; - for (i = 0; i < 12; i++) { - prns[i] = 0; - d = 1; - for (j = 0; j < 5; j++) { - if (prn_le[5*i+j]) prns[i] += d; - d <<= 1; - } - } - ind_prn32 = 32; - for (i = 0; i < 12; i++) { - // PRN-32 overflow - if ( (prns[i] == 0) && (sat_status[i] & 0x0F) ) { // 5 bit: 0..31 - if ( ((i % 3 == 2) && (prn_le[60+i/3] & 1)) // Spalte 2 - || ((i % 3 != 2) && (prn_le[5*(i+1)] & 1)) ) { // Spalte 0,1 - prns[i] = 32; ind_prn32 = i; - } - } - else if ((sat_status[i] & 0x0F) == 0) { // erste beiden bits: 0x03 ? - prns[i] = 0; - } - } - - prn32next = 0; - if (ind_prn32 < 12) { - // PRN-32 overflow - if (ind_prn32 % 3 != 2) { // -> ind_prn32<11 // vorausgesetzt im Block folgt auf PRN-32 - if ((sat_status[ind_prn32+1] & 0x0F) && prns[ind_prn32+1] > 1) { // entweder PRN-1 oder PRN-gerade - // && prns[ind_prn32+1] != 3 ? - for (j = 0; j < ind_prn32; j++) { - if (prns[j] == (prns[ind_prn32+1]^prn32toggle) && (sat_status[j] & 0x0F)) break; - } - if (j < ind_prn32) { prn32toggle ^= 0x1; } - else { - for (j = ind_prn32+2; j < 12; j++) { - if (prns[j] == (prns[ind_prn32+1]^prn32toggle) && (sat_status[j] & 0x0F)) break; - } - if (j < 12) { prn32toggle ^= 0x1; } - } - prns[ind_prn32+1] ^= prn32toggle; - /* - // nochmal testen - for (j = 0; j < ind_prn32; j++) { if (prns[j] == prns[ind_prn32+1]) break; } - if (j < ind_prn32) prns[ind_prn32+1] = 0; - else { - for (j = ind_prn32+2; j < 12; j++) { if (prns[j] == prns[ind_prn32+1]) break; } - if (j < 12) prns[ind_prn32+1] = 0; - } - if (prns[ind_prn32+1] == 0) { prn32toggle ^= 0x1; } - */ - } - prn32next = prns[ind_prn32+1]; // -> ind_prn32<11 && ind_prn32 % 3 != 2 - } - } -} - - -int calc_satpos_alm(EPHEM_t alm[], double t, SAT_t *satp) { - double X, Y, Z, vX, vY, vZ; - int j; - int week; - double cl_corr, cl_drift; - - for (j = 1; j < 33; j++) { - if (alm[j].prn > 0 && alm[j].health == 0) { // prn==j - - // Woche hat 604800 sec - if (t-alm[j].toa > WEEKSEC/2) rollover = +1; - else if (t-alm[j].toa < -WEEKSEC/2) rollover = -1; - else rollover = 0; - week = alm[j].week - rollover; - /*if (j == 1)*/ gpx.week = week + GPS_WEEK1024*1024; - - if (option_vel >= 2) { - GPS_SatellitePositionVelocity_Ephem( - week, t, alm[j], - &cl_corr, &cl_drift, &X, &Y, &Z, &vX, &vY, &vZ - ); - satp[alm[j].prn].clock_drift = cl_drift; - satp[alm[j].prn].vX = vX; - satp[alm[j].prn].vY = vY; - satp[alm[j].prn].vZ = vZ; - } - else { - GPS_SatellitePosition_Ephem( - week, t, alm[j], - &cl_corr, &X, &Y, &Z - ); - } - - satp[alm[j].prn].X = X; - satp[alm[j].prn].Y = Y; - satp[alm[j].prn].Z = Z; - satp[alm[j].prn].clock_corr = cl_corr; - - } - } - - return 0; -} - -int calc_satpos_rnx(EPHEM_t eph[][24], double t, SAT_t *satp) { - double X, Y, Z, vX, vY, vZ; - int j, i, ti; - int week; - double cl_corr, cl_drift; - double tdiff, td; - - for (j = 1; j < 33; j++) { - - // Woche hat 604800 sec - tdiff = WEEKSEC; - ti = 0; - for (i = 0; i < 24; i++) { - if (eph[j][i].prn > 0) { - if (t-eph[j][i].toe > WEEKSEC/2) rollover = +1; - else if (t-eph[j][i].toe < -WEEKSEC/2) rollover = -1; - else rollover = 0; - td = t-eph[j][i].toe - rollover*WEEKSEC; - if (td < 0) td *= -1; - - if ( td < tdiff ) { - tdiff = td; - ti = i; - week = eph[j][ti].week - rollover; - gpx.week = eph[j][ti].gpsweek - rollover; - } - } - } - - if (option_vel >= 2) { - GPS_SatellitePositionVelocity_Ephem( - week, t, eph[j][ti], - &cl_corr, &cl_drift, &X, &Y, &Z, &vX, &vY, &vZ - ); - satp[eph[j][ti].prn].clock_drift = cl_drift; - satp[eph[j][ti].prn].vX = vX; - satp[eph[j][ti].prn].vY = vY; - satp[eph[j][ti].prn].vZ = vZ; - } - else { - GPS_SatellitePosition_Ephem( - week, t, eph[j][ti], - &cl_corr, &X, &Y, &Z - ); - } - - satp[eph[j][ti].prn].X = X; - satp[eph[j][ti].prn].Y = Y; - satp[eph[j][ti].prn].Z = Z; - satp[eph[j][ti].prn].clock_corr = cl_corr; - - } - - return 0; -} - -int calc_satpos_rnx2(EPHEM_t *eph, double t, SAT_t *satp) { - double X, Y, Z, vX, vY, vZ; - int j; - int week; - double cl_corr, cl_drift; - double tdiff, td; - int count, count0, satfound; - - for (j = 1; j < 33; j++) { - - count = count0 = 0; - satfound = 0; - - // Woche hat 604800 sec - tdiff = WEEKSEC; - - while (eph[count].prn > 0) { - - if (eph[count].prn == j && eph[count].health == 0) { - - satfound += 1; - - if (t - eph[count].toe > WEEKSEC/2) rollover = +1; - else if (t - eph[count].toe < -WEEKSEC/2) rollover = -1; - else rollover = 0; - td = fabs( t - eph[count].toe - rollover*WEEKSEC); - - if ( td < tdiff ) { - tdiff = td; - week = eph[count].week - rollover; - gpx.week = eph[count].gpsweek - rollover; - count0 = count; - } - } - count += 1; - } - - if ( satfound ) - { - if (option_vel >= 2) { - GPS_SatellitePositionVelocity_Ephem( - week, t, eph[count0], - &cl_corr, &cl_drift, &X, &Y, &Z, &vX, &vY, &vZ - ); - satp[j].clock_drift = cl_drift; - satp[j].vX = vX; - satp[j].vY = vY; - satp[j].vZ = vZ; - } - else { - GPS_SatellitePosition_Ephem( - week, t, eph[count0], - &cl_corr, &X, &Y, &Z - ); - } - - satp[j].X = X; - satp[j].Y = Y; - satp[j].Z = Z; - satp[j].clock_corr = cl_corr; - satp[j].ephtime = eph[count0].toe; - } - - } - - return 0; -} - - -typedef struct { - ui32_t tow; - ui8_t status; - int chips; - int deltachips; -} RANGE_t; -RANGE_t range[33]; - -int prn[12]; // valide PRN 0,..,k-1 - - -// pseudo.range = -df*pseudo.chips -// df = lightspeed/(chips/sec)/2^10 -const double df = 299792.458/1023.0/1024.0; //0.286183844 // c=299792458m/s, 1023000chips/s -// dl = L1/(chips/sec)/4 -const double dl = 1575.42/1.023/4.0; //385.0 // GPS L1 1575.42MHz=154*10.23MHz, dl=154*10/4 - - -int get_pseudorange() { - ui32_t gpstime; - ui8_t gpstime_bytes[4]; - ui8_t pseudobytes[4]; - unsigned chipbytes, deltabytes; - int i, j, k; - ui8_t bytes[4]; - ui16_t byte16; - double pr0, prj; - - // GPS-TOW in ms - for (i = 0; i < 4; i++) { - gpstime_bytes[i] = framebyte(posGPS_TOW + i); - } - memcpy(&gpstime, gpstime_bytes, 4); - - // Sat Status - for (i = 0; i < 12; i++) { - sat_status[i] = framebyte(posGPS_STATUS + i); - } - - // PRN-Nummern - for (i = 0; i < 4; i++) { - for (j = 0; j < 2; j++) { - bytes[j] = frame[posGPS_PRN+2*i+j]; - } - memcpy(&byte16, bytes, 2); - prnbits_le(byte16, prn_le, i); - } - prn12(prn_le, prns); - - - // GPS Sat Pos (& Vel) - if (almanac) calc_satpos_alm( alm, gpstime/1000.0, sat); - if (ephem) calc_satpos_rnx2(ephs, gpstime/1000.0, sat); - - // GPS Sat Pos t -= 1s - if (option_vel == 1) { - if (almanac) calc_satpos_alm( alm, gpstime/1000.0-1, sat1s); - if (ephem) calc_satpos_rnx2(ephs, gpstime/1000.0-1, sat1s); - } - - k = 0; - for (j = 0; j < 12; j++) { - - // Pseudorange/chips - for (i = 0; i < 4; i++) { - pseudobytes[i] = frame[posGPS_DATA+8*j+i]; - } - memcpy(&chipbytes, pseudobytes, 4); - - // delta_pseudochips / 385 - for (i = 0; i < 3; i++) { - pseudobytes[i] = frame[posGPS_DATA+8*j+4+i]; - } - deltabytes = 0; // bzw. pseudobytes[3]=0 (24 bit); deltabytes & (0xFF<<24) als - memcpy(&deltabytes, pseudobytes, 3); // gemeinsamer offset relevant in --vel1 ! - - //if ( (prns[j] == 0) && (sat_status[j] & 0x0F) ) prns[j] = 32; - range[prns[j]].tow = gpstime; - range[prns[j]].status = sat_status[j]; - - if ( chipbytes == 0x7FFFFFFF || chipbytes == 0x55555555 ) { - range[prns[j]].chips = 0; - continue; - } - if (option_vergps != 8) { - if ( chipbytes > 0x10000000 && chipbytes < 0xF0000000 ) { - range[prns[j]].chips = 0; - continue; - }} - - range[prns[j]].chips = chipbytes; - range[prns[j]].deltachips = deltabytes; - -/* - if (range[prns[j]].deltachips == 0x555555) { - range[prns[j]].deltachips = 0; - continue; - } -*/ - if ( (prns[j] > 0) && ((sat_status[j] & 0x0F) == 0xF) - && (dist(sat[prns[j]].X, sat[prns[j]].Y, sat[prns[j]].Z, 0, 0, 0) > 6700000) ) - { - for (i = 0; i < k; i++) { if (prn[i] == prns[j]) break; } - if (i == k && prns[j] != exSat) { - //if ( range[prns[j]].status & 0xF0 ) // Signalstaerke > 0 ? - { - prn[k] = prns[j]; - k++; - } - } - } - - } - - - for (j = 0; j < 12; j++) { // 0x013FB0A4 - sat[prns[j]].pseudorange = /*0x01400000*/ - range[prns[j]].chips * df; - sat1s[prns[j]].pseudorange = -(range[prns[j]].chips - range[prns[j]].deltachips/dl)*df; - //+ sat[prns[j]].clock_corr - sat1s[prns[j]].clock_corr - sat[prns[j]].pseudorate = - range[prns[j]].deltachips * df / dl; - - sat[prns[j]].prn = prns[j]; - sat1s[prns[j]].prn = prns[j]; - } - - - pr0 = (double)0x01400000; - for (j = 0; j < k; j++) { - prj = sat[prn[j]].pseudorange + sat[prn[j]].clock_corr; - if (prj < pr0) pr0 = prj; - } - for (j = 0; j < k; j++) sat[prn[j]].PR = sat[prn[j]].pseudorange + sat[prn[j]].clock_corr - pr0 + 20e6; - // es kann PRNs geben, die zeitweise stark abweichende PR liefern; - // eventuell Standardabweichung ermitteln und fehlerhafte Sats weglassen - for (j = 0; j < k; j++) { // sat/sat1s... PR-check - sat1s[prn[j]].PR = sat1s[prn[j]].pseudorange + sat[prn[j]].clock_corr - pr0 + 20e6; - } - - return k; -} - -int get_GPSvel(double lat, double lon, double vel_ecef[3], - double *vH, double *vD, double *vU) { - // ECEF-Velocities - // ECEF-Vel -> NorthEastUp - double phi = lat*M_PI/180.0; - double lam = lon*M_PI/180.0; - double vN = -vel_ecef[0]*sin(phi)*cos(lam) - vel_ecef[1]*sin(phi)*sin(lam) + vel_ecef[2]*cos(phi); - double vE = -vel_ecef[0]*sin(lam) + vel_ecef[1]*cos(lam); - *vU = vel_ecef[0]*cos(phi)*cos(lam) + vel_ecef[1]*cos(phi)*sin(lam) + vel_ecef[2]*sin(phi); - // NEU -> HorDirVer - *vH = sqrt(vN*vN+vE*vE); - *vD = atan2(vE, vN) * 180 / M_PI; - if (*vD < 0) *vD += 360; - - return 0; -} - -double DOP[4]; - -int get_GPSkoord(int N) { - double lat, lon, alt, rx_cl_bias; - double vH, vD, vU; - double lat1s, lon1s, alt1s, - lat0 , lon0 , alt0 , pos0_ecef[3]; - double pos_ecef[3], pos1s_ecef[3], dpos_ecef[3], - vel_ecef[3], dvel_ecef[3]; - double gdop, gdop0 = 1000.0; - //double hdop, vdop, pdop; - int i0, i1, i2, i3, j, k, n; - int nav_ret = 0; - int num = 0; - SAT_t Sat_A[4]; - SAT_t Sat_B[12]; // N <= 12 - SAT_t Sat_B1s[12]; - SAT_t Sat_C[12]; // 11 - double diter; - int exN = -1; - - if (option_vergps == 8) { - fprintf(stdout, " sats: "); - for (j = 0; j < N; j++) fprintf(stdout, "%02d ", prn[j]); - fprintf(stdout, "\n"); - } - - gpx.lat = gpx.lon = gpx.alt = 0; - - if (option_vergps != 2) { - for (i0=0;i0 0 && gdop < gdop0) { // wenn fehlerhafter Sat, diter wohl besserer Indikator - gpx.lat = lat; - gpx.lon = lon; - gpx.alt = alt; - gpx.dop = gdop; - gpx.diter = diter; - gpx.sats[0] = prn[i0]; gpx.sats[1] = prn[i1]; gpx.sats[2] = prn[i2]; gpx.sats[3] = prn[i3]; - gdop0 = gdop; - - if (option_vel == 4) { - gpx.vH = vH; - gpx.vD = vD; - gpx.vU = vU; - } - } - } - - }}}} - } - - if (option_vergps == 8 || option_vergps == 2) { - - for (j = 0; j < N; j++) Sat_B[j] = sat[prn[j]]; - for (j = 0; j < N; j++) Sat_B1s[j] = sat1s[prn[j]]; - - NAV_bancroft1(N, Sat_B, pos_ecef, &rx_cl_bias); - ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt); - gdop = -1; - if (calc_DOPn(N, Sat_B, pos_ecef, DOP) == 0) { - gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]); - } - - NAV_LinP(N, Sat_B, pos_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); - if (option_iter) { - for (j = 0; j < 3; j++) pos_ecef[j] += dpos_ecef[j]; - ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt); - } - gpx.diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1], dpos_ecef[2]); - - // Sat mit schlechten Daten suchen - if (gpx.diter > d_err) { - if (N > 5) { // N > 4 kann auch funktionieren - for (n = 0; n < N; n++) { - k = 0; - for (j = 0; j < N; j++) { - if (j != n) { - Sat_C[k] = Sat_B[j]; - k++; - } - } - for (j = 0; j < 3; j++) pos0_ecef[j] = 0; - NAV_bancroft1(N-1, Sat_C, pos0_ecef, &rx_cl_bias); - NAV_LinP(N-1, Sat_C, pos0_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); - diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1], dpos_ecef[2]); - ecef2elli(pos0_ecef[0], pos0_ecef[1], pos0_ecef[2], &lat0, &lon0, &alt0); - if (diter < gpx.diter) { - gpx.diter = diter; - for (j = 0; j < 3; j++) pos_ecef[j] = pos0_ecef[j]; - lat = lat0; - lon = lon0; - alt = alt0; - exN = n; - } - } - if (exN >= 0) { - if (prn[exN] == prn32next) prn32toggle ^= 0x1; - for (k = exN; k < N-1; k++) { - Sat_B[k] = Sat_B[k+1]; - prn[k] = prn[k+1]; - if (option_vel == 1) { - Sat_B1s[k] = Sat_B1s[k+1]; - } - } - N = N-1; - if (calc_DOPn(N, Sat_B, pos_ecef, DOP) == 0) { - gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]); - } - } - } -/* - if (exN < 0 && prn32next > 0) { - //prn32next used in pre-fix? prn32toggle ^= 0x1; - } -*/ - } - - if (option_vel == 1) { - NAV_bancroft1(N, Sat_B1s, pos1s_ecef, &rx_cl_bias); - if (option_iter) { - NAV_LinP(N, Sat_B1s, pos1s_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); - for (j = 0; j < 3; j++) pos1s_ecef[j] += dpos_ecef[j]; - } - for (j = 0; j < 3; j++) vel_ecef[j] = pos_ecef[j] - pos1s_ecef[j]; - get_GPSvel(lat, lon, vel_ecef, &vH, &vD, &vU); - ecef2elli(pos1s_ecef[0], pos1s_ecef[1], pos1s_ecef[2], &lat1s, &lon1s, &alt1s); - if (option_vergps == 8) { - fprintf(stdout, "\ndeltachips1s lat: %.6f , lon: %.6f , alt: %.2f ", lat1s, lon1s, alt1s); - fprintf(stdout, " vH: %4.1f D: %5.1f° vV: %3.1f ", vH, vD, vU); - fprintf(stdout, "\n"); - } - } - if (option_vel >= 2) { - //fprintf(stdout, "\nP(%.1f,%.1f,%.1f) \n", pos_ecef[0], pos_ecef[1], pos_ecef[2]); - vel_ecef[0] = vel_ecef[1] = vel_ecef[2] = 0; - NAV_LinV(N, Sat_B, pos_ecef, vel_ecef, 0.0, dvel_ecef, &rx_cl_bias); - for (j=0; j<3; j++) vel_ecef[j] += dvel_ecef[j]; - //fprintf(stdout, " V(%.1f,%.1f,%.1f) ", vel_ecef[0], vel_ecef[1], vel_ecef[2]); - //fprintf(stdout, " rx_vel_bias: %.1f \n", rx_cl_bias); - /* 2. Iteration: - NAV_LinV(N, Sat_B, pos_ecef, vel_ecef, rx_cl_bias, dvel_ecef, &rx_cl_bias); - for (j=0; j<3; j++) vel_ecef[j] += dvel_ecef[j]; - //fprintf(stdout, " V(%.1f,%.1f,%.1f) ", vel_ecef[0], vel_ecef[1], vel_ecef[2]); - //fprintf(stdout, " rx_vel_bias: %.1f \n", rx_cl_bias); - */ - get_GPSvel(lat, lon, vel_ecef, &vH, &vD, &vU); - } - - if (option_vergps == 8) { - fprintf(stdout, "bancroft[%2d] lat: %.6f , lon: %.6f , alt: %.2f ", N, lat, lon, alt); - fprintf(stdout, " (d:%.1f)", gpx.diter); - if (option_vel) { - fprintf(stdout, " vH: %4.1f D: %5.1f° vV: %3.1f ", vH, vD, vU); - } - fprintf(stdout, " DOP["); - for (j = 0; j < N; j++) { - fprintf(stdout, "%d", prn[j]); - if (j < N-1) fprintf(stdout, ","); else fprintf(stdout, "] %.1f ", gdop); - } - fprintf(stdout, "\n"); - } - - if (option_vergps == 2) { - gpx.lat = lat; - gpx.lon = lon; - gpx.alt = alt; - gpx.dop = gdop; - num = N; - - if (option_vel) { - gpx.vH = vH; - gpx.vD = vD; - gpx.vU = vU; - } - } - - } - - return num; -} - - -/* ------------------------------------------------------------------------------------ */ - -int rs92_ecc(int msglen) { - - int i, ret = 0; - int errors; - ui8_t err_pos[rs_R], err_val[rs_R]; - - if (msglen > FRAME_LEN) msglen = FRAME_LEN; - for (i = msglen; i < FRAME_LEN; i++) frame[i] = 0;//xFF; - - - for (i = 0; i < rs_R; i++) cw[i] = frame[cfg_rs92.parpos+i]; - for (i = 0; i < cfg_rs92.msglen; i++) cw[rs_R+i] = frame[cfg_rs92.msgpos+i]; - - errors = rs_decode(cw, err_pos, err_val); - - //for (i = 0; i < cfg_rs92.hdrlen; i++) frame[i] = data[i]; - for (i = 0; i < rs_R; i++) frame[cfg_rs92.parpos+i] = cw[i]; - for (i = 0; i < cfg_rs92.msglen; i++) frame[cfg_rs92.msgpos+i] = cw[rs_R+i]; - - ret = errors; - - return ret; -} - -/* ------------------------------------------------------------------------------------ */ - -int print_position() { // GPS-Hoehe ueber Ellipsoid - int j, k, n = 0; - int err1, err2; - - err1 = 0; - if (!option_verbose) err1 = err_gps; - err1 |= get_FrameNb(); - err1 |= get_SondeID(); - - err2 = err1 | err_gps; - //err2 |= get_GPSweek(); - err2 |= get_GPStime(); - - if (!err2 && (almanac || ephem)) { - k = get_pseudorange(); - if (k >= 4) { - n = get_GPSkoord(k); - } - } - - if (!err1) { - fprintf(stdout, "[%5d] ", gpx.frnr); - fprintf(stdout, "(%s) ", gpx.id); - } - - if (!err2) { - //if (option_verbose) - { - Gps2Date(gpx.week, gpx.gpssec, &gpx.jahr, &gpx.monat, &gpx.tag); - //fprintf(stdout, "(W %d) ", gpx.week); - fprintf(stdout, "(%04d-%02d-%02d) ", gpx.jahr, gpx.monat, gpx.tag); - } - fprintf(stdout, "%s ", weekday[gpx.wday]); // %04.1f: wenn sek >= 59.950, wird auf 60.0 gerundet - fprintf(stdout, "%02d:%02d:%06.3f", gpx.std, gpx.min, gpx.sek); - - if (n > 0) { - fprintf(stdout, " "); - - if (almanac) fprintf(stdout, " lat: %.4f lon: %.4f alt: %.1f ", gpx.lat, gpx.lon, gpx.alt); - else fprintf(stdout, " lat: %.5f lon: %.5f alt: %.1f ", gpx.lat, gpx.lon, gpx.alt); - - if (option_verbose && option_vergps != 8) { - fprintf(stdout, " (d:%.1f)", gpx.diter); - } - if (option_vel /*&& option_vergps >= 2*/) { - fprintf(stdout," vH: %4.1f D: %5.1f° vV: %3.1f ", gpx.vH, gpx.vD, gpx.vU); - } - if (option_verbose) { - if (option_vergps != 2) { - fprintf(stdout, " DOP[%02d,%02d,%02d,%02d] %.1f", - gpx.sats[0], gpx.sats[1], gpx.sats[2], gpx.sats[3], gpx.dop); - } - else { // wenn option_vergps=2, dann n=N=k(-1) - fprintf(stdout, " DOP["); - for (j = 0; j < n; j++) { - fprintf(stdout, "%d", prn[j]); - if (j < n-1) fprintf(stdout, ","); else fprintf(stdout, "] %.1f ", gpx.dop); - } - } - } - } - - if (option_json) - { - // Print out telemetry data as JSON, even if we don't have a valid GPS lock. - if (!err1 && !err2){ - if (gpx.aux[0] != 0 || gpx.aux[1] != 0 || gpx.aux[2] != 0 || gpx.aux[3] != 0) { - printf("\n{ \"frame\": %d, \"id\": \"%s\", \"datetime\": \"%04d-%02d-%02dT%02d:%02d:%06.3fZ\", \"lat\": %.5f, \"lon\": %.5f, \"alt\": %.5f, \"vel_h\": %.5f, \"heading\": %.5f, \"vel_v\": %.5f, \"aux\": \"%04x%04x%04x%04x\"}\n", gpx.frnr, gpx.id, gpx.jahr, gpx.monat, gpx.tag, gpx.std, gpx.min, gpx.sek, gpx.lat, gpx.lon, gpx.alt, gpx.vH, gpx.vD, gpx.vU , gpx.aux[0], gpx.aux[1], gpx.aux[2], gpx.aux[3]); - } else { - printf("\n{ \"frame\": %d, \"id\": \"%s\", \"datetime\": \"%04d-%02d-%02dT%02d:%02d:%06.3fZ\", \"lat\": %.5f, \"lon\": %.5f, \"alt\": %.5f, \"vel_h\": %.5f, \"heading\": %.5f, \"vel_v\": %.5f }\n", gpx.frnr, gpx.id, gpx.jahr, gpx.monat, gpx.tag, gpx.std, gpx.min, gpx.sek, gpx.lat, gpx.lon, gpx.alt, gpx.vH, gpx.vD, gpx.vU ); - } - } - } - - get_Cal(); - - if (option_vergps == 8 /*|| option_vergps == 2*/) - { - fprintf(stdout, "\n"); - for (j = 0; j < 60; j++) { fprintf(stdout, "%d", prn_le[j]); if (j % 5 == 4) fprintf(stdout, " "); } - fprintf(stdout, ": "); - for (j = 0; j < 12; j++) fprintf(stdout, "%2d ", prns[j]); - fprintf(stdout, "\n"); - fprintf(stdout, " status: "); - for (j = 0; j < 12; j++) fprintf(stdout, "%02X ", sat_status[j]); //range[prns[j]].status - fprintf(stdout, "\n"); - } - - } - - if (!err1) { - fprintf(stdout, "\n"); - //if (option_vergps == 8) fprintf(stdout, "\n"); - } - - return err2; -} - -void print_frame(int len) { - int i, ret = 0; - ui8_t byte; - - if (option_ecc) { - ret = rs92_ecc(len); - } - - for (i = len; i < FRAME_LEN; i++) { - frame[i] = 0; - } - - if (option_raw) { -/* - for (i = 0; i < len; i++) { - byte = framebyte(i); - fprintf(stdout, "%02x", byte); - } - fprintf(stdout, "\n"); -*/ - for (i = 0; i < len; i++) { - //byte = frame[i]; - byte = framebyte(i); - fprintf(stdout, "%02x", byte); - } - if (option_ecc && option_verbose) { - fprintf(stdout, " "); - if (ret >= 0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); - fprintf(stdout, " errors: %d", ret); - } - fprintf(stdout, "\n"); -// fprintf(stdout, "\n"); - } - else print_position(); -} - - -int main(int argc, char *argv[]) { - - FILE *fp, *fp_alm = NULL, *fp_eph = NULL; - char *fpname = NULL; - float spb = 0.0; - char bitbuf[BITS]; - int bit_count = 0, - bitpos = 0, - byte_count = FRAMESTART, - header_found = 0; - int bit, byte; - int bitQ; - int herrs, herr1; - int headerlen = 0; - - int k,K; - float mv; - unsigned int mv_pos, mv0_pos; - int mp = 0; - - float thres = 0.7; - - int symlen = 2; - int bitofs = 2; // +0 .. +3 - int shift = 0; - - -#ifdef CYGWIN - _setmode(_fileno(stdin), _O_BINARY); -#endif - setbuf(stdout, NULL); - - fpname = argv[0]; - ++argv; - while ((*argv) && (!fileloaded)) { - if ( (strcmp(*argv, "-h") == 0) || (strcmp(*argv, "--help") == 0) ) { - fprintf(stderr, "%s [options] \n", fpname); - fprintf(stderr, " file: audio.wav or raw_data\n"); - fprintf(stderr, " options:\n"); - fprintf(stderr, " --vel; --vel1, --vel2 (-g2)\n"); - fprintf(stderr, " -v, -vx, -vv\n"); - fprintf(stderr, " -r, --raw\n"); - fprintf(stderr, " -i, --invert\n"); - fprintf(stderr, " -a, --almanac \n"); - fprintf(stderr, " -e, --ephem \n"); - fprintf(stderr, " -g1 (verbose GPS: 4 sats)\n"); - fprintf(stderr, " -g2 (verbose GPS: all sats)\n"); - fprintf(stderr, " -gg (vverbose GPS)\n"); - fprintf(stderr, " --crc (CRC check GPS)\n"); - fprintf(stderr, " --ecc (Reed-Solomon)\n"); - fprintf(stderr, " --ths (peak threshold; default=%.1f)\n", thres); - fprintf(stderr, " --json (JSON output)\n"); - return 0; - } - else if ( (strcmp(*argv, "--vel") == 0) ) { - option_vel = 4; - } - else if ( (strcmp(*argv, "--vel1") == 0) ) { - option_vel = 1; - if (option_vergps < 1) option_vergps = 2; - } - else if ( (strcmp(*argv, "--vel2") == 0) ) { - option_vel = 2; - if (option_vergps < 1) option_vergps = 2; - } - else if ( (strcmp(*argv, "--iter") == 0) ) { - option_iter = 1; - } - else if ( (strcmp(*argv, "-v") == 0) ) { - option_verbose = 1; - } - else if ( (strcmp(*argv, "-vv") == 0) ) { - option_verbose = 4; - } - else if ( (strcmp(*argv, "-vx") == 0) ) { - option_aux = 1; - } - else if (strcmp(*argv, "--crc") == 0) { option_crc = 1; } - 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; - } - else if ( (strcmp(*argv, "-a") == 0) || (strcmp(*argv, "--almanac") == 0) ) { - ++argv; - if (*argv) fp_alm = fopen(*argv, "r"); // txt-mode - else return -1; - if (fp_alm == NULL) fprintf(stderr, "[almanac] %s konnte nicht geoeffnet werden\n", *argv); - } - else if ( (strcmp(*argv, "-e") == 0) || (strncmp(*argv, "--ephem", 7) == 0) ) { - ++argv; - if (*argv) fp_eph = fopen(*argv, "rb"); // bin-mode - else return -1; - if (fp_eph == NULL) fprintf(stderr, "[rinex] %s konnte nicht geoeffnet werden\n", *argv); - } - else if ( (strcmp(*argv, "--dop") == 0) ) { - ++argv; - if (*argv) { - dop_limit = atof(*argv); - if (dop_limit <= 0 || dop_limit >= 100) dop_limit = 9.9; - } - else return -1; - } - else if ( (strcmp(*argv, "--der") == 0) ) { - ++argv; - if (*argv) { - d_err = atof(*argv); - if (d_err <= 0 || d_err >= 100000) d_err = 10000; - else option_der = 1; - } - else return -1; - } - else if ( (strcmp(*argv, "--exsat") == 0) ) { - ++argv; - if (*argv) { - exSat = atoi(*argv); - if (exSat < 1 || exSat > 32) exSat = -1; - } - else return -1; - } - else if (strcmp(*argv, "-g1") == 0) { option_vergps = 1; } // verbose1 GPS - else if (strcmp(*argv, "-g2") == 0) { option_vergps = 2; } // verbose2 GPS (bancroft) - else if (strcmp(*argv, "-gg") == 0) { option_vergps = 8; } // vverbose GPS - else if (strcmp(*argv, "--ecc") == 0) { option_ecc = 1; } - else if (strcmp(*argv, "--json") == 0) {option_json = 1; option_ecc = 1; option_crc = 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 { - if (!rawin) fp = fopen(*argv, "rb"); - else fp = fopen(*argv, "r"); - if (fp == NULL) { - fprintf(stderr, "%s konnte nicht geoeffnet werden\n", *argv); - return -1; - } - fileloaded = 1; - } - ++argv; - } - if (!fileloaded) fp = stdin; - - if (fp_alm) { - if (read_SEMalmanac(fp_alm, alm) == 0) { - almanac = 1; - } - fclose(fp_alm); - if (!option_der) d_err = 4000; - } - if (fp_eph) { - /* i = read_RNXephemeris(fp_eph, eph); - if (i == 0) { - ephem = 1; - almanac = 0; - } - fclose(fp_eph); */ - ephs = read_RNXpephs(fp_eph); - if (ephs) { - ephem = 1; - almanac = 0; - } - fclose(fp_eph); - if (!option_der) d_err = 1000; - } - - - 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"); - } - - - if (option_ecc) { - rs_init_RS255(); - } - - - symlen = 2; - bitofs += shift; - - headerlen = strlen(rawheader); - K = init_buffers(rawheader, headerlen, 2); // shape=2 - 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 (mv > thres && mp > 0) { - if (mv_pos > mv0_pos) { - - header_found = 0; - herrs = headcmp(symlen, rawheader, headerlen, mv_pos, mv<0, 0); // symlen=2 - herr1 = 0; - if (herrs <= 3 && herrs > 0) { - herr1 = headcmp(symlen, rawheader, headerlen, mv_pos+1, mv<0, 0); - if (herr1 < herrs) { - herrs = herr1; - herr1 = 1; - } - } - if (herrs <= 2) header_found = 1; // herrs <= 2 bitfehler in header - - if (header_found) { - - byte_count = FRAMESTART; - bit_count = 0; - bitpos = 0; - - while ( byte_count < FRAME_LEN ) { - bitQ = read_sbit(fp, symlen, &bit, option_inv, bitofs, bit_count==0); // symlen=2 - if ( bitQ == EOF) break; - bit_count += 1; - bitbuf[bitpos] = bit; - bitpos++; - if (bitpos >= BITS) { - bitpos = 0; - byte = bits2byte(bitbuf); - frame[byte_count] = byte; - byte_count++; - } - } - header_found = 0; - print_frame(byte_count); - byte_count = FRAMESTART; - - } - } - } - - } - - - free_buffers(); - - free(ephs); - fclose(fp); - - return 0; -} - -