From ebd678d3fee6642527d92da43f992776d366fc87 Mon Sep 17 00:00:00 2001 From: Mark Jessop Date: Mon, 8 Apr 2019 20:53:12 +1000 Subject: [PATCH] Switch to new 'mod' demodulators for RS41 and RS92 --- auto_rx/autorx/decode.py | 4 +- auto_rx/autorx/utils.py | 2 +- auto_rx/build.sh | 20 +- auto_rx/test/test_demod.py | 8 +- demod/mod/bch_ecc_mod.c | 1013 +++++++++++++++++++ demod/mod/bch_ecc_mod.h | 97 ++ demod/mod/demod_mod.c | 827 ++++++++++++++++ demod/mod/demod_mod.h | 119 +++ demod/mod/dfm09mod.c | 1118 +++++++++++++++++++++ demod/mod/lms6mod.c | 984 +++++++++++++++++++ demod/mod/m10mod.c | 1100 +++++++++++++++++++++ demod/mod/nav_gps_vel.c | 1892 ++++++++++++++++++++++++++++++++++++ demod/mod/rs41mod.c | 1620 ++++++++++++++++++++++++++++++ demod/mod/rs92mod.c | 1531 +++++++++++++++++++++++++++++ 14 files changed, 10327 insertions(+), 8 deletions(-) create mode 100644 demod/mod/bch_ecc_mod.c create mode 100644 demod/mod/bch_ecc_mod.h create mode 100644 demod/mod/demod_mod.c create mode 100644 demod/mod/demod_mod.h create mode 100644 demod/mod/dfm09mod.c create mode 100644 demod/mod/lms6mod.c create mode 100644 demod/mod/m10mod.c create mode 100644 demod/mod/nav_gps_vel.c create mode 100644 demod/mod/rs41mod.c create mode 100644 demod/mod/rs92mod.c diff --git a/auto_rx/autorx/decode.py b/auto_rx/autorx/decode.py index ce25bc6..2801e68 100644 --- a/auto_rx/autorx/decode.py +++ b/auto_rx/autorx/decode.py @@ -236,7 +236,7 @@ class SondeDecoder(object): if self.save_decode_audio: decode_cmd += " tee decode_%s.wav |" % str(self.device_idx) - decode_cmd += "./rs41ecc --crc --ecc --ptu --json 2>/dev/null" + decode_cmd += "./rs41mod --ptu --json 2>/dev/null" elif self.sonde_type == "RS92": # Decoding a RS92 requires either an ephemeris or an almanac file. @@ -281,7 +281,7 @@ class SondeDecoder(object): if self.save_decode_audio: decode_cmd += " tee decode_%s.wav |" % str(self.device_idx) - decode_cmd += "./rs92ecc -vx -v --crc --ecc --vel --json %s 2>/dev/null" % _rs92_gps_data + decode_cmd += "./rs92mod -vx -v --crc --ecc --vel --json %s 2>/dev/null" % _rs92_gps_data elif self.sonde_type == "DFM": # DFM06/DFM09 Sondes. diff --git a/auto_rx/autorx/utils.py b/auto_rx/autorx/utils.py index ca30e06..6773a1e 100644 --- a/auto_rx/autorx/utils.py +++ b/auto_rx/autorx/utils.py @@ -28,7 +28,7 @@ except ImportError: # List of binaries we check for on startup -REQUIRED_RS_UTILS = ['dft_detect', 'rs41ecc', 'rs92ecc', 'dfm09ecc', 'm10', 'imet1rs_dft'] +REQUIRED_RS_UTILS = ['dft_detect', 'dfm09ecc', 'm10', 'imet1rs_dft', 'rs41mod', 'rs92mod'] def check_rs_utils(): """ Check the required RS decoder binaries exist diff --git a/auto_rx/build.sh b/auto_rx/build.sh index d6fab48..bdfaf3a 100755 --- a/auto_rx/build.sh +++ b/auto_rx/build.sh @@ -15,9 +15,22 @@ gcc -c demod_dft.c gcc rs92dm_dft.c demod_dft.o -lm -o rs92ecc -I../ecc/ -I../rs92 gcc rs41dm_dft.c demod_dft.o -lm -o rs41ecc -I../ecc/ -I../rs41 -w gcc dfm09dm_dft.c demod_dft.o -lm -o dfm09ecc -I../ecc/ -I../dfm + +# New demodulators +cd ../demod/mod/ +gcc -c demod_mod.c -w +gcc -c bch_ecc_mod.c -w +gcc rs41mod.c demod_mod.o bch_ecc_mod.o -lm -o rs41mod -w +# Holding off on DFM decoder until the DFM17/15 ID issue is resolved. +#gcc dfm09mod.c demod_mod.o -lm -o dfm09mod -w +gcc rs92mod.c demod_mod.o bch_ecc_mod.o -lm -o rs92mod -w +#gcc lms6mod.c demod_mod.o bch_ecc_mod.o -lm -o lms6mod -w +#gcc m10mod.c demod_mod.o -lm -o m10mod -w + + # Build M10 decoder echo "Building M10 Demodulator." -cd ../m10/ +cd ../../m10/ g++ M10.cpp M10Decoder.cpp M10GeneralParser.cpp M10GtopParser.cpp M10TrimbleParser.cpp AudioFile.cpp -lm -o m10 -std=c++11 echo "Building iMet Demodulator." @@ -34,4 +47,9 @@ cp ../demod/dfm09ecc . cp ../m10/m10 . cp ../imet/imet1rs_dft . +cp ../demod/mod/rs41mod . +#cp ../demod/mod/dfm09mod . +cp ../demod/mod/rs92mod . +#cp ../demod/mod/lms6mod . + echo "Done!" diff --git a/auto_rx/test/test_demod.py b/auto_rx/test/test_demod.py index 0a7e2e1..8bdb555 100644 --- a/auto_rx/test/test_demod.py +++ b/auto_rx/test/test_demod.py @@ -194,7 +194,7 @@ processing_type['rs41_rtlfm'] = { # Currently using a timeout to kill rtl_fm as it doesnt notice the end of the incoming samples. 'demod': _demod_command, # Decode using rs41ecc - 'decode': "../rs41ecc --ptu --crc --ecc 2>/dev/null", + 'decode': "../rs41mod --ptu --crc --ecc2 2>/dev/null", # Count the number of telemetry lines. "post_process" : " | grep 00000 | wc -l", 'files' : "./generated/rs41*.bin" @@ -224,7 +224,7 @@ _demod_command += " sox -t raw -r %d -e s -b 16 -c 1 - -r 48000 -b 8 -t wav - hi processing_type['rs92_rtlfm'] = { 'demod': _demod_command, # Decode using rs92ecc - 'decode': "../rs92ecc -vx -v --crc --ecc --vel 2>/dev/null", + 'decode': "../rs92mod -vx -v --crc --ecc --vel 2>/dev/null", #'decode': "../rs92ecc -vx -v --crc --ecc -r --vel 2>/dev/null", # For measuring No-ECC performance # Count the number of telemetry lines. "post_process" : " | grep M2513116 | wc -l", @@ -256,7 +256,7 @@ _demod_command += " sox -t raw -r %d -e s -b 16 -c 1 - -r 48000 -b 8 -t wav - hi processing_type['rs92ngp_rtlfm'] = { 'demod': _demod_command, # Decode using rs92ecc - 'decode': "../rs92ecc -vx -v --crc --ecc --vel 2>/dev/null", + 'decode': "../rs92mod -vx -v --crc --ecc --vel 2>/dev/null", #'decode': "../rs92ecc -vx -v --crc --ecc -r --vel 2>/dev/null", # For measuring No-ECC performance # Count the number of telemetry lines. "post_process" : "| grep P3213708 | wc -l", @@ -285,7 +285,7 @@ _demod_command += " sox -t raw -r %d -e s -b 16 -c 1 - -r 48000 -b 8 -t wav - hi processing_type['dfm_rtlfm'] = { 'demod': _demod_command, - 'decode': "../dfm09ecc -vv --json --dist --auto 2>/dev/null", # ECC + 'decode': "../dfm09mod -vv --json --dist --auto 2>/dev/null", # ECC #'decode': "../dfm09ecc -vv --ecc -r --auto 2>/dev/null", # No-ECC # Count the number of telemetry lines. "post_process" : " | grep frame | wc -l", # ECC diff --git a/demod/mod/bch_ecc_mod.c b/demod/mod/bch_ecc_mod.c new file mode 100644 index 0000000..812fc9e --- /dev/null +++ b/demod/mod/bch_ecc_mod.c @@ -0,0 +1,1013 @@ + +/* + * BCH / Reed-Solomon + * encoder() + * decoder() (Euklid. Alg.) + * + * + * author: zilog80 + * + + Vaisala RS92, RS41: + RS(255, 231), t=12 + f=X^8+X^4+X^3+X^2+1, b=0 + g(X) = (X-alpha^0)...(X-alpha^(2t-1)) + + LMS6: + RS(255, 223), t=16 (CCSDS) + f=X^8+X^7+X^2+X+1, b=112 + g(X) = (X-(alpha^11)^112)...(X-(alpha^11)^(112+2t-1)) + + Meisei: + bin.BCH(63, 51), t=2 + g(X) = (X^6+X+1)(X^6+X^4+X^2+X+1) + g(a) = 0 fuer a = alpha^1,...,alpha^4 + Es koennen 2 Fehler korrigiert werden; diese koennen auch direkt mit + L(x) = 1 + L1 x + L2 x^2, L1=L1(S1), L2=L2(S1,S3) + gefunden werden. Problem: 3 Fehler und mehr erkennen. + Auch bei 3 Fehlern ist deg(Lambda)=2 und Lambda hat auch 2 Loesungen. + Meisei-Bloecke sind auf 46 bit gekuerzt und enthalten 2 parity bits. + -> Wenn decodierte Bloecke bits in Position 46-63 schalten oder + einer der parity-checks fehlschlaegt, dann Block nicht korrigierbar. + Es werden + - 54% der 3-Fehler-Bloecke erkannt + - 39% der 3-Fehler-Bloecke werden durch Position/Parity erkannt + - 7% der 3-Fehler-Bloecke werden falsch korrigiert + + * + */ + + +#include "bch_ecc_mod.h" + +/* +#define MAX_DEG 254 // max N-1 + + +typedef struct { + ui32_t f; + ui32_t ord; + ui8_t alpha; + ui8_t exp_a[256]; + ui8_t log_a[256]; +} GF_t; + +static GF_t GF256RS = { f : 0x11D, // RS-GF(2^8): X^8 + X^4 + X^3 + X^2 + 1 : 0x11D + ord : 256, // 2^8 + alpha: 0x02, // generator: alpha = X + exp_a: {0}, + log_a: {0} }; + +static GF_t GF256RSccsds = { f : 0x187, // RS-GF(2^8): X^8 + X^7 + X^2 + X + 1 : 0x187 + ord : 256, // 2^8 + alpha: 0x02, // generator: alpha = X + exp_a: {0}, + log_a: {0} }; + +static GF_t GF64BCH = { f : 0x43, // BCH-GF(2^6): X^6 + X + 1 : 0x43 + ord : 64, // 2^6 + alpha: 0x02, // generator: alpha = X + exp_a: {0}, + log_a: {0} }; + +static GF_t GF16RS = { f : 0x13, // RS-GF(2^4): X^4 + X + 1 : 0x13 + ord : 16, // 2^4 + alpha: 0x02, // generator: alpha = X + exp_a: {0}, + log_a: {0} }; + +static GF_t GF256AES = { f : 0x11B, // AES-GF(2^8): X^8 + X^4 + X^3 + X + 1 : 0x11B + ord : 256, // 2^8 + alpha: 0x03, // generator: alpha = X+1 + exp_a: {0}, + log_a: {0} }; + + +typedef struct { + ui8_t N; + ui8_t t; + ui8_t R; // RS: R=2t, BCH: R<=mt + ui8_t K; // K=N-R + ui8_t b; + ui8_t p; ui8_t ip; // p*ip = 1 mod N + ui8_t g[MAX_DEG+1]; // MAX_DEG+1 = 255 + GF_t GF; +} RS_t; + + +static RS_t RS256 = { 255, 12, 24, 231, 0, 1, 1, {0}, {0} }; +static RS_t RS256ccsds = { 255, 16, 32, 223, 112, 11, 116, {0}, {0} }; +static RS_t BCH64 = { 63, 2, 12, 51, 1, 1, 1, {0}, {0} }; + +// static RS_t RS16_0 = { 15, 3, 6, 9, 0, 1, 1, {0}, {0} }; +static RS_t RS16ccsds = { 15, 2, 4, 11, 6, 1, 1, {0}, {0} }; + +*/ + + +/* --------------------------------------------------------------------------------------------- */ + +static int GF_deg(ui32_t p) { + ui32_t d = 31; + if (p == 0) return -1; /* deg(0) = -infty */ + else { + while (d && !(p & (1<f)) + aa ^= gf->f; // a = a - GF.f + b >>= 1; + if (b & 1) ab ^= (ui8_t)aa; /* b_{i+1} > 0 ? */ + } + return ab; +} + +static int GF_genTab(GF_t *gf) { + int i, j; + ui8_t b; + +// GF.f = f; +// GF.ord = 1 << GF_deg(GF.f); + + // if (gf->exp_a[0] == 0) { + // /* mehrmals generieren kein Problem; + // ansonsten muesste geprueft werden, ob Tabelle fertig */ + // } + + b = 0x01; + for (i = 0; i < gf->ord; i++) { + gf->exp_a[i] = b; // b_i = a^i + b = GF2m_mul(gf, gf->alpha, b); + } + + gf->log_a[0] = -00; // log(0) = -inf + for (i = 1; i < gf->ord; i++) { + b = 0x01; j = 0; + while (b != i) { + b = GF2m_mul(gf, gf->alpha, b); + j++; + if (j > gf->ord-1) { + return -1; // a not primitive + } + } // j = log_a(i) + gf->log_a[i] = j; + } + + return 0; +} + +/* --------------------------------------------------------------------------------------------- */ +/* + +static ui32_t f256 = 0x11D; +static ui8_t f256FF = 0x1D; + +static int gf256_deg(ui8_t p) { + int d = 7; // sizeof(p)*8 - 1 = 7 fuer ui8_t + + if (p == 0) return -0xFF; // deg(0) = -infty + else { + while (d && !(p & (1<>= 1; + if (q & 1) h ^= p; // q_{i+1} > 0 ? + } + return h; +} + +static int gf256_divmod(ui8_t p, ui8_t q, ui8_t *s, ui8_t *r) { + int deg_p, deg_q = gf256_deg(q); // p = s*q + r + *s = 0; + + if (q == 0) { *s = -1; *r = -1; return -1;} // DIV_BY_ZERO + + if (q == 1) { *s = p; *r = 0; } + else { + deg_p = gf256_deg(p); + if (p == 0) { + p = f256FF; // (ui8_t) f256 = f256 & 0xFF = f256FF + deg_p = 8; // deg(f256) = 8 + } + while (deg_p >= deg_q) { + *s |= 1 << (deg_p-deg_q); + p ^= q << (deg_p-deg_q); + deg_p = gf256_deg(p); + } + *r = p; + } + return 0; +} + +static ui8_t gf256_inv(ui8_t a) { // 1 = x*a + y*f , ggT(a, f) = 1 + ui8_t rem, rem1, rem2, aux, aux1, aux2, quo; + + if (a == 0) return 0; // nicht definiert; DIV_BY_ZERO + if (a == 1) return 1; + + rem1 = a; + rem2 = 0; // = f256 + aux1 = 0x1; + aux2 = 0x0; + rem = rem1; + aux = aux1; + + while (rem > 0x1) { + gf256_divmod(rem2, rem1, &quo, &rem); + aux = gf256_mul(quo, aux1) ^ aux2; // aux = aux2 - quo*aux1 + rem2 = rem1; + rem1 = rem; + aux2 = aux1; + aux1 = aux; + } + return aux; +} +*/ + +/* --------------------------------------------------------------------------------------------- */ +/* + +// F2[X] mod X^8 + X^4 + X^3 + X + 1 + +static ui8_t exp_11B[256] = { // 0x11B: a^n , a = 0x03 = X+1 + 0x01, 0x03, 0x05, 0x0F, 0x11, 0x33, 0x55, 0xFF, 0x1A, 0x2E, 0x72, 0x96, 0xA1, 0xF8, 0x13, 0x35, + 0x5F, 0xE1, 0x38, 0x48, 0xD8, 0x73, 0x95, 0xA4, 0xF7, 0x02, 0x06, 0x0A, 0x1E, 0x22, 0x66, 0xAA, + 0xE5, 0x34, 0x5C, 0xE4, 0x37, 0x59, 0xEB, 0x26, 0x6A, 0xBE, 0xD9, 0x70, 0x90, 0xAB, 0xE6, 0x31, + 0x53, 0xF5, 0x04, 0x0C, 0x14, 0x3C, 0x44, 0xCC, 0x4F, 0xD1, 0x68, 0xB8, 0xD3, 0x6E, 0xB2, 0xCD, + 0x4C, 0xD4, 0x67, 0xA9, 0xE0, 0x3B, 0x4D, 0xD7, 0x62, 0xA6, 0xF1, 0x08, 0x18, 0x28, 0x78, 0x88, + 0x83, 0x9E, 0xB9, 0xD0, 0x6B, 0xBD, 0xDC, 0x7F, 0x81, 0x98, 0xB3, 0xCE, 0x49, 0xDB, 0x76, 0x9A, + 0xB5, 0xC4, 0x57, 0xF9, 0x10, 0x30, 0x50, 0xF0, 0x0B, 0x1D, 0x27, 0x69, 0xBB, 0xD6, 0x61, 0xA3, + 0xFE, 0x19, 0x2B, 0x7D, 0x87, 0x92, 0xAD, 0xEC, 0x2F, 0x71, 0x93, 0xAE, 0xE9, 0x20, 0x60, 0xA0, + 0xFB, 0x16, 0x3A, 0x4E, 0xD2, 0x6D, 0xB7, 0xC2, 0x5D, 0xE7, 0x32, 0x56, 0xFA, 0x15, 0x3F, 0x41, + 0xC3, 0x5E, 0xE2, 0x3D, 0x47, 0xC9, 0x40, 0xC0, 0x5B, 0xED, 0x2C, 0x74, 0x9C, 0xBF, 0xDA, 0x75, + 0x9F, 0xBA, 0xD5, 0x64, 0xAC, 0xEF, 0x2A, 0x7E, 0x82, 0x9D, 0xBC, 0xDF, 0x7A, 0x8E, 0x89, 0x80, + 0x9B, 0xB6, 0xC1, 0x58, 0xE8, 0x23, 0x65, 0xAF, 0xEA, 0x25, 0x6F, 0xB1, 0xC8, 0x43, 0xC5, 0x54, + 0xFC, 0x1F, 0x21, 0x63, 0xA5, 0xF4, 0x07, 0x09, 0x1B, 0x2D, 0x77, 0x99, 0xB0, 0xCB, 0x46, 0xCA, + 0x45, 0xCF, 0x4A, 0xDE, 0x79, 0x8B, 0x86, 0x91, 0xA8, 0xE3, 0x3E, 0x42, 0xC6, 0x51, 0xF3, 0x0E, + 0x12, 0x36, 0x5A, 0xEE, 0x29, 0x7B, 0x8D, 0x8C, 0x8F, 0x8A, 0x85, 0x94, 0xA7, 0xF2, 0x0D, 0x17, + 0x39, 0x4B, 0xDD, 0x7C, 0x84, 0x97, 0xA2, 0xFD, 0x1C, 0x24, 0x6C, 0xB4, 0xC7, 0x52, 0xF6, 0x01}; + +static ui8_t log_11B[256] = { // 0x11B: log_a , a = 0x03 = X+1 + -00 , 0x00, 0x19, 0x01, 0x32, 0x02, 0x1A, 0xC6, 0x4B, 0xC7, 0x1B, 0x68, 0x33, 0xEE, 0xDF, 0x03, + 0x64, 0x04, 0xE0, 0x0E, 0x34, 0x8D, 0x81, 0xEF, 0x4C, 0x71, 0x08, 0xC8, 0xF8, 0x69, 0x1C, 0xC1, + 0x7D, 0xC2, 0x1D, 0xB5, 0xF9, 0xB9, 0x27, 0x6A, 0x4D, 0xE4, 0xA6, 0x72, 0x9A, 0xC9, 0x09, 0x78, + 0x65, 0x2F, 0x8A, 0x05, 0x21, 0x0F, 0xE1, 0x24, 0x12, 0xF0, 0x82, 0x45, 0x35, 0x93, 0xDA, 0x8E, + 0x96, 0x8F, 0xDB, 0xBD, 0x36, 0xD0, 0xCE, 0x94, 0x13, 0x5C, 0xD2, 0xF1, 0x40, 0x46, 0x83, 0x38, + 0x66, 0xDD, 0xFD, 0x30, 0xBF, 0x06, 0x8B, 0x62, 0xB3, 0x25, 0xE2, 0x98, 0x22, 0x88, 0x91, 0x10, + 0x7E, 0x6E, 0x48, 0xC3, 0xA3, 0xB6, 0x1E, 0x42, 0x3A, 0x6B, 0x28, 0x54, 0xFA, 0x85, 0x3D, 0xBA, + 0x2B, 0x79, 0x0A, 0x15, 0x9B, 0x9F, 0x5E, 0xCA, 0x4E, 0xD4, 0xAC, 0xE5, 0xF3, 0x73, 0xA7, 0x57, + 0xAF, 0x58, 0xA8, 0x50, 0xF4, 0xEA, 0xD6, 0x74, 0x4F, 0xAE, 0xE9, 0xD5, 0xE7, 0xE6, 0xAD, 0xE8, + 0x2C, 0xD7, 0x75, 0x7A, 0xEB, 0x16, 0x0B, 0xF5, 0x59, 0xCB, 0x5F, 0xB0, 0x9C, 0xA9, 0x51, 0xA0, + 0x7F, 0x0C, 0xF6, 0x6F, 0x17, 0xC4, 0x49, 0xEC, 0xD8, 0x43, 0x1F, 0x2D, 0xA4, 0x76, 0x7B, 0xB7, + 0xCC, 0xBB, 0x3E, 0x5A, 0xFB, 0x60, 0xB1, 0x86, 0x3B, 0x52, 0xA1, 0x6C, 0xAA, 0x55, 0x29, 0x9D, + 0x97, 0xB2, 0x87, 0x90, 0x61, 0xBE, 0xDC, 0xFC, 0xBC, 0x95, 0xCF, 0xCD, 0x37, 0x3F, 0x5B, 0xD1, + 0x53, 0x39, 0x84, 0x3C, 0x41, 0xA2, 0x6D, 0x47, 0x14, 0x2A, 0x9E, 0x5D, 0x56, 0xF2, 0xD3, 0xAB, + 0x44, 0x11, 0x92, 0xD9, 0x23, 0x20, 0x2E, 0x89, 0xB4, 0x7C, 0xB8, 0x26, 0x77, 0x99, 0xE3, 0xA5, + 0x67, 0x4A, 0xED, 0xDE, 0xC5, 0x31, 0xFE, 0x18, 0x0D, 0x63, 0x8C, 0x80, 0xC0, 0xF7, 0x70, 0x07}; + +// ------------------------------------------------------------------------------------------------ + +// F2[X] mod X^8 + X^4 + X^3 + X^2 + 1 + +static ui8_t exp_11D[256] = { // 0x11D: a^n , a = 0x02 = X + 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1D, 0x3A, 0x74, 0xE8, 0xCD, 0x87, 0x13, 0x26, + 0x4C, 0x98, 0x2D, 0x5A, 0xB4, 0x75, 0xEA, 0xC9, 0x8F, 0x03, 0x06, 0x0C, 0x18, 0x30, 0x60, 0xC0, + 0x9D, 0x27, 0x4E, 0x9C, 0x25, 0x4A, 0x94, 0x35, 0x6A, 0xD4, 0xB5, 0x77, 0xEE, 0xC1, 0x9F, 0x23, + 0x46, 0x8C, 0x05, 0x0A, 0x14, 0x28, 0x50, 0xA0, 0x5D, 0xBA, 0x69, 0xD2, 0xB9, 0x6F, 0xDE, 0xA1, + 0x5F, 0xBE, 0x61, 0xC2, 0x99, 0x2F, 0x5E, 0xBC, 0x65, 0xCA, 0x89, 0x0F, 0x1E, 0x3C, 0x78, 0xF0, + 0xFD, 0xE7, 0xD3, 0xBB, 0x6B, 0xD6, 0xB1, 0x7F, 0xFE, 0xE1, 0xDF, 0xA3, 0x5B, 0xB6, 0x71, 0xE2, + 0xD9, 0xAF, 0x43, 0x86, 0x11, 0x22, 0x44, 0x88, 0x0D, 0x1A, 0x34, 0x68, 0xD0, 0xBD, 0x67, 0xCE, + 0x81, 0x1F, 0x3E, 0x7C, 0xF8, 0xED, 0xC7, 0x93, 0x3B, 0x76, 0xEC, 0xC5, 0x97, 0x33, 0x66, 0xCC, + 0x85, 0x17, 0x2E, 0x5C, 0xB8, 0x6D, 0xDA, 0xA9, 0x4F, 0x9E, 0x21, 0x42, 0x84, 0x15, 0x2A, 0x54, + 0xA8, 0x4D, 0x9A, 0x29, 0x52, 0xA4, 0x55, 0xAA, 0x49, 0x92, 0x39, 0x72, 0xE4, 0xD5, 0xB7, 0x73, + 0xE6, 0xD1, 0xBF, 0x63, 0xC6, 0x91, 0x3F, 0x7E, 0xFC, 0xE5, 0xD7, 0xB3, 0x7B, 0xF6, 0xF1, 0xFF, + 0xE3, 0xDB, 0xAB, 0x4B, 0x96, 0x31, 0x62, 0xC4, 0x95, 0x37, 0x6E, 0xDC, 0xA5, 0x57, 0xAE, 0x41, + 0x82, 0x19, 0x32, 0x64, 0xC8, 0x8D, 0x07, 0x0E, 0x1C, 0x38, 0x70, 0xE0, 0xDD, 0xA7, 0x53, 0xA6, + 0x51, 0xA2, 0x59, 0xB2, 0x79, 0xF2, 0xF9, 0xEF, 0xC3, 0x9B, 0x2B, 0x56, 0xAC, 0x45, 0x8A, 0x09, + 0x12, 0x24, 0x48, 0x90, 0x3D, 0x7A, 0xF4, 0xF5, 0xF7, 0xF3, 0xFB, 0xEB, 0xCB, 0x8B, 0x0B, 0x16, + 0x2C, 0x58, 0xB0, 0x7D, 0xFA, 0xE9, 0xCF, 0x83, 0x1B, 0x36, 0x6C, 0xD8, 0xAD, 0x47, 0x8E, 0x01}; + +static ui8_t log_11D[256] = { // 0x11D: log_a , a = 0x02 = X + -00 , 0x00, 0x01, 0x19, 0x02, 0x32, 0x1A, 0xC6, 0x03, 0xDF, 0x33, 0xEE, 0x1B, 0x68, 0xC7, 0x4B, + 0x04, 0x64, 0xE0, 0x0E, 0x34, 0x8D, 0xEF, 0x81, 0x1C, 0xC1, 0x69, 0xF8, 0xC8, 0x08, 0x4C, 0x71, + 0x05, 0x8A, 0x65, 0x2F, 0xE1, 0x24, 0x0F, 0x21, 0x35, 0x93, 0x8E, 0xDA, 0xF0, 0x12, 0x82, 0x45, + 0x1D, 0xB5, 0xC2, 0x7D, 0x6A, 0x27, 0xF9, 0xB9, 0xC9, 0x9A, 0x09, 0x78, 0x4D, 0xE4, 0x72, 0xA6, + 0x06, 0xBF, 0x8B, 0x62, 0x66, 0xDD, 0x30, 0xFD, 0xE2, 0x98, 0x25, 0xB3, 0x10, 0x91, 0x22, 0x88, + 0x36, 0xD0, 0x94, 0xCE, 0x8F, 0x96, 0xDB, 0xBD, 0xF1, 0xD2, 0x13, 0x5C, 0x83, 0x38, 0x46, 0x40, + 0x1E, 0x42, 0xB6, 0xA3, 0xC3, 0x48, 0x7E, 0x6E, 0x6B, 0x3A, 0x28, 0x54, 0xFA, 0x85, 0xBA, 0x3D, + 0xCA, 0x5E, 0x9B, 0x9F, 0x0A, 0x15, 0x79, 0x2B, 0x4E, 0xD4, 0xE5, 0xAC, 0x73, 0xF3, 0xA7, 0x57, + 0x07, 0x70, 0xC0, 0xF7, 0x8C, 0x80, 0x63, 0x0D, 0x67, 0x4A, 0xDE, 0xED, 0x31, 0xC5, 0xFE, 0x18, + 0xE3, 0xA5, 0x99, 0x77, 0x26, 0xB8, 0xB4, 0x7C, 0x11, 0x44, 0x92, 0xD9, 0x23, 0x20, 0x89, 0x2E, + 0x37, 0x3F, 0xD1, 0x5B, 0x95, 0xBC, 0xCF, 0xCD, 0x90, 0x87, 0x97, 0xB2, 0xDC, 0xFC, 0xBE, 0x61, + 0xF2, 0x56, 0xD3, 0xAB, 0x14, 0x2A, 0x5D, 0x9E, 0x84, 0x3C, 0x39, 0x53, 0x47, 0x6D, 0x41, 0xA2, + 0x1F, 0x2D, 0x43, 0xD8, 0xB7, 0x7B, 0xA4, 0x76, 0xC4, 0x17, 0x49, 0xEC, 0x7F, 0x0C, 0x6F, 0xF6, + 0x6C, 0xA1, 0x3B, 0x52, 0x29, 0x9D, 0x55, 0xAA, 0xFB, 0x60, 0x86, 0xB1, 0xBB, 0xCC, 0x3E, 0x5A, + 0xCB, 0x59, 0x5F, 0xB0, 0x9C, 0xA9, 0xA0, 0x51, 0x0B, 0xF5, 0x16, 0xEB, 0x7A, 0x75, 0x2C, 0xD7, + 0x4F, 0xAE, 0xD5, 0xE9, 0xE6, 0xE7, 0xAD, 0xE8, 0x74, 0xD6, 0xF4, 0xEA, 0xA8, 0x50, 0x58, 0xAF}; + +// ------------------------------------------------------------------------------------------------ + +// F2[X] mod X^6 + X + 1 : 0x43 + +static ui8_t exp64[64] = { // 0x43: a^n , a = 0x02 = X + 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x03, 0x06, 0x0C, 0x18, 0x30, 0x23, 0x05, 0x0A, 0x14, 0x28, + 0x13, 0x26, 0x0F, 0x1E, 0x3C, 0x3B, 0x35, 0x29, 0x11, 0x22, 0x07, 0x0E, 0x1C, 0x38, 0x33, 0x25, + 0x09, 0x12, 0x24, 0x0B, 0x16, 0x2C, 0x1B, 0x36, 0x2F, 0x1D, 0x3A, 0x37, 0x2D, 0x19, 0x32, 0x27, + 0x0D, 0x1A, 0x34, 0x2B, 0x15, 0x2A, 0x17, 0x2E, 0x1F, 0x3E, 0x3F, 0x3D, 0x39, 0x31, 0x21, 0x01}; + +static ui8_t log64[64] = { + -00 , 0x00, 0x01, 0x06, 0x02, 0x0C, 0x07, 0x1A, 0x03, 0x20, 0x0D, 0x23, 0x08, 0x30, 0x1B, 0x12, + 0x04, 0x18, 0x21, 0x10, 0x0E, 0x34, 0x24, 0x36, 0x09, 0x2D, 0x31, 0x26, 0x1C, 0x29, 0x13, 0x38, + 0x05, 0x3E, 0x19, 0x0B, 0x22, 0x1F, 0x11, 0x2F, 0x0F, 0x17, 0x35, 0x33, 0x25, 0x2C, 0x37, 0x28, + 0x0A, 0x3D, 0x2E, 0x1E, 0x32, 0x16, 0x27, 0x2B, 0x1D, 0x3C, 0x2A, 0x15, 0x14, 0x3B, 0x39, 0x3A}; + +// ------------------------------------------------------------------------------------------------ + +// F2[X] mod X^4 + X + 1 : 0x13 + +static ui8_t exp16[16] = { // 0x43: a^n , a = 0x02 = X + 0x1, 0x2, 0x4, 0x8, 0x3, 0x6, 0xC, 0xB, + 0x5, 0xA, 0x7, 0xE, 0xF, 0xD, 0x9, 0x1}; + +static ui8_t log16[16] = { + -00, 0x0, 0x1, 0x4, 0x2, 0x8, 0x5, 0xA, + 0x3, 0xE, 0x9, 0x7, 0x6, 0xD, 0xB, 0xC}; + +*/ +/* --------------------------------------------------------------------------------------------- */ + +static ui8_t GF_mul(GF_t *gf, ui8_t p, ui8_t q) { + ui32_t x; + if ((p == 0) || (q == 0)) return 0; + x = (ui32_t)gf->log_a[p] + gf->log_a[q]; + return gf->exp_a[x % (gf->ord-1)]; // a^(ord-1) = 1 +} + +static ui8_t GF_inv(GF_t *gf, ui8_t p) { + if (p == 0) return 0; // DIV_BY_ZERO + return gf->exp_a[gf->ord-1-gf->log_a[p]]; // a^(ord-1) = 1 +} + +/* --------------------------------------------------------------------------------------------- */ + +/* + * p(x) = p[0] + p[1]x + ... + p[N-1]x^(N-1) + */ + +static ui8_t poly_eval(GF_t *gf, ui8_t poly[], ui8_t x) { + int n; + ui8_t xn, y; + + y = poly[0]; + if (x != 0) { + for (n = 1; n < gf->ord-1; n++) { + xn = gf->exp_a[(n*gf->log_a[x]) % (gf->ord-1)]; + y ^= GF_mul(gf, poly[n], xn); + } + } + return y; +} + +static ui8_t poly_evalH(GF_t *gf, ui8_t poly[], ui8_t x) { + int n; + ui8_t y; + y = poly[gf->ord-1]; + for (n = gf->ord-2; n >= 0; n--) { + y = GF_mul(gf, y, x) ^ poly[n]; + } + return y; +} + + +static int poly_deg(ui8_t p[]) { + int n = MAX_DEG; + while (p[n] == 0 && n > 0) n--; + if (p[n] == 0) n--; // deg(0) = -inf + return n; +} + +static int poly_divmod(GF_t *gf, ui8_t p[], ui8_t q[], ui8_t *d, ui8_t *r) { + int deg_p, deg_q; // p(x) = q(x)d(x) + r(x) + int i; // deg(r) < deg(q) + ui8_t c; + + deg_p = poly_deg(p); + deg_q = poly_deg(q); + + if (deg_q < 0) return -1; // q=0: DIV_BY_ZERO + + for (i = 0; i <= MAX_DEG; i++) d[i] = 0; + for (i = 0; i <= MAX_DEG; i++) r[i] = 0; + + + c = GF_mul(gf, p[deg_p], GF_inv(gf, q[deg_q])); + + if (deg_q == 0) { + for (i = 0; i <= deg_p; i++) d[i] = GF_mul(gf, p[i], c); + for (i = 0; i <= MAX_DEG; i++) r[i] = 0; + } + else if (deg_p < 0) { // p=0 + for (i = 0; i <= MAX_DEG; i++) { + d[i] = 0; + r[i] = 0; + } + } + else if (deg_p < deg_q) { + for (i = 0; i <= MAX_DEG; i++) d[i] = 0; + for (i = 0; i <= deg_p; i++) r[i] = p[i]; // r(x)=p(x), deg(r)= deg_q) { + d[deg_p-deg_q] = c; + for (i = 0; i <= deg_q; i++) { + r[deg_p-i] ^= GF_mul(gf, q[deg_q-i], c); + } + while (r[deg_p] == 0 && deg_p > 0) deg_p--; + if (r[deg_p] == 0) deg_p--; + if (deg_p >= 0) c = GF_mul(gf, r[deg_p], GF_inv(gf, q[deg_q])); + } + } + return 0; +} + +static int poly_add(ui8_t a[], ui8_t b[], ui8_t *sum) { + int i; + ui8_t c[MAX_DEG+1]; + + for (i = 0; i <= MAX_DEG; i++) { + c[i] = a[i] ^ b[i]; + } + + for (i = 0; i <= MAX_DEG; i++) { sum[i] = c[i]; } + + return 0; +} + +static int poly_mul(GF_t *gf, ui8_t a[], ui8_t b[], ui8_t *ab) { + int i, j; + ui8_t c[MAX_DEG+1]; + + if (poly_deg(a)+poly_deg(b) > MAX_DEG) { + return -1; + } + + for (i = 0; i <= MAX_DEG; i++) { c[i] = 0; } + + for (i = 0; i <= poly_deg(a); i++) { + for (j = 0; j <= poly_deg(b); j++) { + c[i+j] ^= GF_mul(gf, a[i], b[j]); + } + } + + for (i = 0; i <= MAX_DEG; i++) { ab[i] = c[i]; } + + return 0; +} + + +static int polyGF_eggT(GF_t *gf, int deg, ui8_t a[], ui8_t b[], // in + ui8_t *u, ui8_t *v, ui8_t *ggt // out + ) { +// deg = 0: +// a(x)u(x)+b(x)v(x) = ggt(x) +// RS: +// deg=t, a(x)=S(x), b(x)=x^2t + + int i; + ui8_t r0[MAX_DEG+1], r1[MAX_DEG+1], r2[MAX_DEG+1], + s0[MAX_DEG+1], s1[MAX_DEG+1], s2[MAX_DEG+1], + t0[MAX_DEG+1], t1[MAX_DEG+1], t2[MAX_DEG+1], + quo[MAX_DEG+1]; + + for (i = 0; i <= MAX_DEG; i++) { u[i] = 0; } + for (i = 0; i <= MAX_DEG; i++) { v[i] = 0; } + for (i = 0; i <= MAX_DEG; i++) { ggt[i] = 0; } + + for (i = 0; i <= MAX_DEG; i++) { r0[i] = a[i]; } + for (i = 0; i <= MAX_DEG; i++) { r1[i] = b[i]; } + s0[0] = 1; for (i = 1; i <= MAX_DEG; i++) { s0[i] = 0; } // s0 = 1 + s1[0] = 0; for (i = 1; i <= MAX_DEG; i++) { s1[i] = 0; } // s1 = 0 + t0[0] = 0; for (i = 1; i <= MAX_DEG; i++) { t0[i] = 0; } // t0 = 0 + t1[0] = 1; for (i = 1; i <= MAX_DEG; i++) { t1[i] = 0; } // t1 = 1 + for (i = 0; i <= MAX_DEG; i++) { r2[i] = 0; } + for (i = 0; i <= MAX_DEG; i++) { s2[i] = 0; } + for (i = 0; i <= MAX_DEG; i++) { t2[i] = 0; } + + while ( poly_deg(r1) >= deg ) { + poly_divmod(gf, r0, r1, quo, r2); + for (i = 0; i <= MAX_DEG; i++) { r0[i] = r1[i]; } + for (i = 0; i <= MAX_DEG; i++) { r1[i] = r2[i]; } + poly_mul(gf, quo, s1, s2); + poly_add(s0, s2, s2); + for (i = 0; i <= MAX_DEG; i++) { s0[i] = s1[i]; } + for (i = 0; i <= MAX_DEG; i++) { s1[i] = s2[i]; } + poly_mul(gf, quo, t1, t2); + poly_add(t0, t2, t2); + for (i = 0; i <= MAX_DEG; i++) { t0[i] = t1[i]; } + for (i = 0; i <= MAX_DEG; i++) { t1[i] = t2[i]; } + } + + if (deg > 0) { + for (i = 0; i <= MAX_DEG; i++) { ggt[i] = r1[i]; } // deg=0: r0 + for (i = 0; i <= MAX_DEG; i++) { u[i] = s1[i]; } // deg=0: s0 + for (i = 0; i <= MAX_DEG; i++) { v[i] = t1[i]; } + } + else { + for (i = 0; i <= MAX_DEG; i++) { ggt[i] = r0[i]; } + for (i = 0; i <= MAX_DEG; i++) { u[i] = s0[i]; } + for (i = 0; i <= MAX_DEG; i++) { v[i] = t0[i]; } + } + + return 0; +} + +static int polyGF_lfsr(GF_t *gf, int deg, int x2t, ui8_t S[], + ui8_t *Lambda, ui8_t *Omega ) { +// BCH/RS/LFSR: deg=t+e/2, e=#erasures +// S(x)Lambda(x) = Omega(x) mod x^2t + int i; + ui8_t r0[MAX_DEG+1], r1[MAX_DEG+1], r2[MAX_DEG+1], + s0[MAX_DEG+1], s1[MAX_DEG+1], s2[MAX_DEG+1], + quo[MAX_DEG+1]; + + for (i = 0; i <= MAX_DEG; i++) { Lambda[i] = 0; } + for (i = 0; i <= MAX_DEG; i++) { Omega[i] = 0; } + + for (i = 0; i <= MAX_DEG; i++) { r0[i] = S[i]; } + for (i = 0; i <= MAX_DEG; i++) { r1[i] = 0; } r1[x2t] = 1; //x^2t + s0[0] = 1; for (i = 1; i <= MAX_DEG; i++) { s0[i] = 0; } + s1[0] = 0; for (i = 1; i <= MAX_DEG; i++) { s1[i] = 0; } + for (i = 0; i <= MAX_DEG; i++) { r2[i] = 0; } + for (i = 0; i <= MAX_DEG; i++) { s2[i] = 0; } + + while ( poly_deg(r1) >= deg ) { // deg=t+e/2 + poly_divmod(gf, r0, r1, quo, r2); + for (i = 0; i <= MAX_DEG; i++) { r0[i] = r1[i]; } + for (i = 0; i <= MAX_DEG; i++) { r1[i] = r2[i]; } + + poly_mul(gf, quo, s1, s2); + poly_add(s0, s2, s2); + for (i = 0; i <= MAX_DEG; i++) { s0[i] = s1[i]; } + for (i = 0; i <= MAX_DEG; i++) { s1[i] = s2[i]; } + } + +// deg > 0: + for (i = 0; i <= MAX_DEG; i++) { Omega[i] = r1[i]; } + for (i = 0; i <= MAX_DEG; i++) { Lambda[i] = s1[i]; } + + return 0; +} + +static int poly_D(ui8_t a[], ui8_t *Da) { + int i; + + for (i = 0; i <= MAX_DEG; i++) { Da[i] = 0; } // unten werden nicht immer + // alle Koeffizienten gesetzt + for (i = 1; i <= poly_deg(a); i++) { + if (i % 2) Da[i-1] = a[i]; // GF(2^n): b+b=0 + } + + return 0; +} + +static ui8_t forney(RS_t *RS, ui8_t x, ui8_t Omega[], ui8_t Lambda[]) { + GF_t *gf = &RS->GF; + ui8_t DLam[MAX_DEG+1]; + ui8_t w, z, Y; // x=X^(-1), Y = x^(b-1) * Omega(x)/Lambda'(x) + // Y = X^(1-b) * Omega(X^(-1))/Lambda'(X^(-1)) + poly_D(Lambda, DLam); + w = poly_eval(gf, Omega, x); + z = poly_eval(gf, DLam, x); if (z == 0) { return -00; } + Y = GF_mul(gf, w, GF_inv(gf, z)); + if (RS->b == 0) Y = GF_mul(gf, GF_inv(gf, x), Y); + else if (RS->b > 1) { + ui8_t xb1 = gf->exp_a[((RS->b-1)*gf->log_a[x]) % (gf->ord-1)]; + Y = GF_mul(gf, xb1, Y); + } + + return Y; +} + +static int era_sigma(RS_t *RS, int n, ui8_t era_pos[], ui8_t *sigma) { + GF_t *gf = &(RS->GF); + int i; + ui8_t Xa[MAX_DEG+1], sig[MAX_DEG+1]; + ui8_t a_i; + + for (i = 0; i <= MAX_DEG; i++) sig[i] = 0; + for (i = 0; i <= MAX_DEG; i++) Xa[i] = 0; + + // sigma(X)=(1 - alpha^j1 X)...(1 - alpha^jn X) + // j_{i+1} = era_pos[i] + sig[0] = 1; + Xa[0] = 1; + for (i = 0; i < n; i++) { // n <= 2*RS.t + a_i = gf->exp_a[(RS->p*era_pos[i]) % (gf->ord-1)]; + Xa[1] = a_i; // Xalp[0..1]: (1-alpha^(j_)X) + poly_mul(gf, sig, Xa, sig); + } + + for (i = 0; i <= MAX_DEG; i++) sigma[i] = sig[i]; + + return 0; +} + +static int syndromes(RS_t *RS, ui8_t cw[], ui8_t *S) { + GF_t *gf = &RS->GF; + int i, errors = 0; + ui8_t a_i; + + // syndromes: e_j=S((alpha^p)^(b+i)) (wie in g(X)) + for (i = 0; i < 2*RS->t; i++) { + a_i = gf->exp_a[(RS->p*(RS->b+i)) % (gf->ord-1)]; // (alpha^p)^(b+i) + S[i] = poly_eval(gf, cw, a_i); + if (S[i]) errors = 1; + } + return errors; +} + +/* +static int prn_GFpoly(ui32_t p) { + int i, s; + s = 0; + if (p == 0) printf("0"); + else { + if (p != (p & 0x1FF)) printf(" (& 0x1FF) "); + for (i=8; i>1; i--) { + if ( (p>>i) & 1 ) { + if (s) printf(" + "); + printf("X^%d", i); + s = 1; + } + } + if ( (p>>1) & 1 ) { + if (s) printf(" + "); + printf("X"); + s = 1; + } + if ( p & 1 ) { + if (s) printf(" + "); + printf("1"); + } + } + return 0; +} +static void prn_table(void) { + int i; + + printf("F2[X] mod "); + prn_GFpoly(GF.f); + printf(" : 0x%X\n", GF.f); + printf("\n"); + + printf("a^n[%d] = {\n", GF.ord); + printf(" "); + for (i=0; iGF; + int i, check_gen; + ui8_t Xalp[MAX_DEG+1]; + + *RS = RS256; // N=255, t=12, b=0, p=1 + *gf = GF256RS; + check_gen = GF_genTab(gf); + + for (i = 0; i <= MAX_DEG; i++) RS->g[i] = 0; + for (i = 0; i <= MAX_DEG; i++) Xalp[i] = 0; + + // g(X)=(X-alpha^b)...(X-alpha^(b+2t-1)), b=0 + RS->g[0] = 0x01; + Xalp[1] = 0x01; // X + for (i = 0; i < 2*RS->t; i++) { + Xalp[0] = gf->exp_a[(RS->b+i) % (gf->ord-1)]; // Xalp[0..1]: X - alpha^(b+i) + poly_mul(gf, RS->g, Xalp, RS->g); + } + + return check_gen; +} + +INCSTAT +int rs_init_RS255ccsds(RS_t *RS) { + GF_t *gf = &RS->GF; + int i, check_gen; + ui8_t Xalp[MAX_DEG+1]; + + *RS = RS256ccsds; // N=255, t=16, b=112, p=11 + *gf = GF256RSccsds; + check_gen = GF_genTab(gf); + + for (i = 0; i <= MAX_DEG; i++) RS->g[i] = 0; + for (i = 0; i <= MAX_DEG; i++) Xalp[i] = 0; + + // beta=alpha^p primitive root of g(X) + // beta^ip=alpha // N=255, p=11 -> ip=116 + for (i = 1; i < gf->ord-1; i++) { + if ( (RS->p * i) % (gf->ord-1) == 1 ) { + RS->ip = i; + break; + } + } + + // g(X)=(X-(alpha^p)^b)...(X-(alpha^p)^(b+2t-1)), b=112 + RS->g[0] = 0x01; + Xalp[1] = 0x01; // X + for (i = 0; i < 2*RS->t; i++) { + Xalp[0] = gf->exp_a[(RS->p*(RS->b+i)) % (gf->ord-1)]; // Xalp[0..1]: X - (alpha^p)^(b+i) + poly_mul(gf, RS->g, Xalp, RS->g); + } +/* + RS.g[ 0] = RS.g[32] = exp_a[0]; + RS.g[ 1] = RS.g[31] = exp_a[249]; + RS.g[ 2] = RS.g[30] = exp_a[59]; + RS.g[ 3] = RS.g[29] = exp_a[66]; + RS.g[ 4] = RS.g[28] = exp_a[4]; + RS.g[ 5] = RS.g[27] = exp_a[43]; + RS.g[ 6] = RS.g[26] = exp_a[126]; + RS.g[ 7] = RS.g[25] = exp_a[251]; + RS.g[ 8] = RS.g[24] = exp_a[97]; + RS.g[ 9] = RS.g[23] = exp_a[30]; + RS.g[10] = RS.g[22] = exp_a[3]; + RS.g[11] = RS.g[21] = exp_a[213]; + RS.g[12] = RS.g[20] = exp_a[50]; + RS.g[13] = RS.g[19] = exp_a[66]; + RS.g[14] = RS.g[18] = exp_a[170]; + RS.g[15] = RS.g[17] = exp_a[5]; + RS.g[16] = exp_a[24]; +*/ + return check_gen; +} + +INCSTAT +int rs_init_BCH64(RS_t *RS) { + GF_t *gf = &RS->GF; + int i, check_gen; + + *RS = BCH64; // N=63, t=2, b=1 + *gf = GF64BCH; + check_gen = GF_genTab(gf); + + for (i = 0; i <= MAX_DEG; i++) RS->g[i] = 0; + + // g(X)=X^12+X^10+X^8+X^5+X^4+X^3+1 + // =(X^6+X+1)(X^6+X^4+X^2+X+1) + RS->g[0] = RS->g[3] = RS->g[4] = RS->g[5] = RS->g[8] = RS->g[10] = RS->g[12] = 1; + + return check_gen; +} + +INCSTAT +int rs_init_RS15ccsds(RS_t *RS) { + GF_t *gf = &RS->GF; + int i, check_gen; + ui8_t Xalp[MAX_DEG+1]; + + //*RS = RS16_0; // N=15, t=3, b=0, p=1 + *RS = RS16ccsds; // N=15, t=2, b=6, p=1 + *gf = GF16RS; + check_gen = GF_genTab(gf); + + for (i = 0; i <= MAX_DEG; i++) RS->g[i] = 0; + for (i = 0; i <= MAX_DEG; i++) Xalp[i] = 0; + + // g(X)=(X-alpha^b)...(X-alpha^(b+2t-1)) + RS->g[0] = 0x01; + Xalp[1] = 0x01; // X + for (i = 0; i < 2*RS->t; i++) { + Xalp[0] = gf->exp_a[(RS->b+i) % (gf->ord-1)]; // Xalp[0..1]: X - alpha^(b+i) + poly_mul(gf, RS->g, Xalp, RS->g); + } + + return check_gen; +} + +INCSTAT +int rs_encode(RS_t *RS, ui8_t cw[]) { + GF_t *gf = &RS->GF; + int j; + ui8_t __cw[MAX_DEG+1], + parity[MAX_DEG+1], + d[MAX_DEG+1]; + for (j = 0; j <= MAX_DEG; j++) parity[j] = 0; + for (j = 0; j <= MAX_DEG; j++) __cw[j] = 0; + for (j = RS->R; j < RS->N; j++) __cw[j] = cw[j]; + poly_divmod(gf, __cw, RS->g, d, parity); + //if (poly_deg(parity) >= RS.R) return -1; + for (j = 0; j < RS->R; j++) cw[j] = parity[j]; + return 0; +} + +// 2*Errors + Erasure <= 2*t +INCSTAT +int rs_decode_ErrEra(RS_t *RS, ui8_t cw[], int nera, ui8_t era_pos[], + ui8_t *err_pos, ui8_t *err_val) { + GF_t *gf = &RS->GF; + ui8_t x, gamma; + ui8_t S[MAX_DEG+1], + Lambda[MAX_DEG+1], + Omega[MAX_DEG+1], + sigma[MAX_DEG+1], + sigLam[MAX_DEG+1]; + int deg_sigLam, deg_Lambda, deg_Omega; + int i, nerr, errera = 0; + + if (nera > 2*RS->t) { return -4; } + + for (i = 0; i < 2*RS->t; i++) { err_pos[i] = 0; } + for (i = 0; i < 2*RS->t; i++) { err_val[i] = 0; } + + // IF: erasures set 0 + // for (i = 0; i < nera; i++) cw[era_pos[i]] = 0x00; // erasures + // THEN: restore cw[era_pos[i]], if errera < 0 + + for (i = 0; i <= MAX_DEG; i++) { S[i] = 0; } + errera = syndromes(RS, cw, S); + // wenn S(x)=0 , dann poly_divmod(cw, RS.g, d, rem): rem=0 + + for (i = 0; i <= MAX_DEG; i++) { sigma[i] = 0; } + sigma[0] = 1; + + + if (nera > 0) { + era_sigma(RS, nera, era_pos, sigma); + poly_mul(gf, sigma, S, S); + for (i = 2*RS->t; i <= MAX_DEG; i++) S[i] = 0; // S = sig*S mod x^2t + } + + if (errera) + { + polyGF_lfsr(gf, RS->t+nera/2, 2*RS->t, S, Lambda, Omega); + + deg_Lambda = poly_deg(Lambda); + deg_Omega = poly_deg(Omega); + if (deg_Omega >= deg_Lambda + nera) { + errera = -3; + return errera; + } + gamma = Lambda[0]; + if (gamma) { + for (i = deg_Lambda; i >= 0; i--) Lambda[i] = GF_mul(gf, Lambda[i], GF_inv(gf, gamma)); + for (i = deg_Omega ; i >= 0; i--) Omega[i] = GF_mul(gf, Omega[i], GF_inv(gf, gamma)); + poly_mul(gf, sigma, Lambda, sigLam); + deg_sigLam = poly_deg(sigLam); + } + else { + errera = -2; + return errera; + } + + nerr = 0; // Errors + Erasures (erasure-pos bereits bekannt) + for (i = 1; i < gf->ord ; i++) { // Lambda(0)=1 + x = (ui8_t)i; // roll-over + if (poly_eval(gf, sigLam, x) == 0) { // Lambda(x)=0 fuer x in erasures[] moeglich + // error location index + ui8_t x1 = GF_inv(gf, x); + err_pos[nerr] = (gf->log_a[x1]*RS->ip) % (gf->ord-1); + // error value; bin-BCH: err_val=1 + err_val[nerr] = forney(RS, x, Omega, sigLam); + //err_val[nerr] == 0, wenn era_val[pos]=0, d.h. cw[pos] schon korrekt + nerr++; + } + if (nerr >= deg_sigLam) break; + } + + // 2*Errors + Erasure <= 2*t + if (nerr < deg_sigLam) errera = -1; // uncorrectable errors + else { + errera = nerr; + for (i = 0; i < errera; i++) cw[err_pos[i]] ^= err_val[i]; + } + } + + return errera; +} + +// Errors <= t +INCSTAT +int rs_decode(RS_t *RS, ui8_t cw[], ui8_t *err_pos, ui8_t *err_val) { + ui8_t tmp[1] = {0}; + return rs_decode_ErrEra(RS, cw, 0, tmp, err_pos, err_val); +} + +INCSTAT +int rs_decode_bch_gf2t2(RS_t *RS, ui8_t cw[], ui8_t *err_pos, ui8_t *err_val) { +// binary 2-error correcting BCH + GF_t *gf = &RS->GF; + ui8_t x, gamma, + S[MAX_DEG+1], + L[MAX_DEG+1], L2, + Lambda[MAX_DEG+1], + Omega[MAX_DEG+1]; + int i, n, errors = 0; + + + for (i = 0; i < RS->t; i++) { err_pos[i] = 0; } + for (i = 0; i < RS->t; i++) { err_val[i] = 0; } + + for (i = 0; i <= MAX_DEG; i++) { S[i] = 0; } + errors = syndromes(RS, cw, S); + // wenn S(x)=0 , dann poly_divmod(cw, RS.g, d, rem): rem=0 + + if (errors) { + polyGF_lfsr(gf, RS->t, 2*RS->t, S, Lambda, Omega); + gamma = Lambda[0]; + if (gamma) { + for (i = poly_deg(Lambda); i >= 0; i--) Lambda[i] = GF_mul(gf, Lambda[i], GF_inv(gf, gamma)); + for (i = poly_deg(Omega) ; i >= 0; i--) Omega[i] = GF_mul(gf, Omega[i], GF_inv(gf, gamma)); + } + else { + errors = -2; + return errors; + } + + // GF(2)-BCH, t=2: + // S1 = S[0] + // S1^2 = S2 , S2^2 = S4 + // L(x) = 1 + L1 x + L2 x^2 (1-2 errors) + // L1 = S1 , L2 = (S3 + S1^3)/S1 + if ( RS->t == 2 ) { + + for (i = 0; i <= MAX_DEG; i++) { L[i] = 0; } + L[0] = 1; + L[1] = S[0]; + L2 = GF_mul(gf, S[0], S[0]); L2 = GF_mul(gf, L2, S[0]); L2 ^= S[2]; + L2 = GF_mul(gf, L2, GF_inv(gf, S[0])); + L[2] = L2; + + if (S[1] != GF_mul(gf, S[0],S[0]) || S[3] != GF_mul(gf, S[1],S[1])) { + errors = -2; + return errors; + } + if (L[1] != Lambda[1] || L[2] != Lambda[2] ) { + errors = -2; + return errors; + } + } + + n = 0; + for (i = 1; i < gf->ord ; i++) { // Lambda(0)=1 + x = (ui8_t)i; // roll-over + if (poly_eval(gf, Lambda, x) == 0) { + // error location index + err_pos[n] = gf->log_a[GF_inv(gf, x)]; + // error value; bin-BCH: err_val=1 + err_val[n] = 1; // = forney(x, Omega, Lambda); + n++; + } + if (n >= poly_deg(Lambda)) break; + } + + if (n < poly_deg(Lambda)) errors = -1; // uncorrectable errors + else { + errors = n; + for (i = 0; i < errors; i++) cw[err_pos[i]] ^= err_val[i]; + } + } + + return errors; +} + diff --git a/demod/mod/bch_ecc_mod.h b/demod/mod/bch_ecc_mod.h new file mode 100644 index 0000000..d52a423 --- /dev/null +++ b/demod/mod/bch_ecc_mod.h @@ -0,0 +1,97 @@ + +/* + * BCH / Reed-Solomon + * encoder() + * decoder() (Euklid. Alg.) + * + * + * author: zilog80 + * + * + */ + +#ifdef INCLUDESTATIC + #define INCSTAT static +#else + typedef unsigned char ui8_t; + typedef unsigned int ui32_t; + #define INCSTAT +#endif + + +#define MAX_DEG 254 // max N-1 + + +typedef struct { + ui32_t f; + ui32_t ord; + ui8_t alpha; + ui8_t exp_a[256]; + ui8_t log_a[256]; +} GF_t; + +typedef struct { + ui8_t N; + ui8_t t; + ui8_t R; // RS: R=2t, BCH: R<=mt + ui8_t K; // K=N-R + ui8_t b; + ui8_t p; ui8_t ip; // p*ip = 1 mod N + ui8_t g[MAX_DEG+1]; // ohne g[] eventuell als init_return + GF_t GF; +} RS_t; + + +static GF_t GF256RS = { f : 0x11D, // RS-GF(2^8): X^8 + X^4 + X^3 + X^2 + 1 : 0x11D + ord : 256, // 2^8 + alpha: 0x02, // generator: alpha = X + exp_a: {0}, + log_a: {0} }; + +static GF_t GF256RSccsds = { f : 0x187, // RS-GF(2^8): X^8 + X^7 + X^2 + X + 1 : 0x187 + ord : 256, // 2^8 + alpha: 0x02, // generator: alpha = X + exp_a: {0}, + log_a: {0} }; + +static GF_t GF64BCH = { f : 0x43, // BCH-GF(2^6): X^6 + X + 1 : 0x43 + ord : 64, // 2^6 + alpha: 0x02, // generator: alpha = X + exp_a: {0}, + log_a: {0} }; + +static GF_t GF16RS = { f : 0x13, // RS-GF(2^4): X^4 + X + 1 : 0x13 + ord : 16, // 2^4 + alpha: 0x02, // generator: alpha = X + exp_a: {0}, + log_a: {0} }; + +static GF_t GF256AES = { f : 0x11B, // AES-GF(2^8): X^8 + X^4 + X^3 + X + 1 : 0x11B + ord : 256, // 2^8 + alpha: 0x03, // generator: alpha = X+1 + exp_a: {0}, + log_a: {0} }; + + +static RS_t RS256 = { 255, 12, 24, 231, 0, 1, 1, {0}, {0} }; +static RS_t RS256ccsds = { 255, 16, 32, 223, 112, 11, 116, {0}, {0} }; +static RS_t BCH64 = { 63, 2, 12, 51, 1, 1, 1, {0}, {0} }; + +// static RS_t RS16_0 = { 15, 3, 6, 9, 0, 1, 1, {0}, {0} }; +static RS_t RS16ccsds = { 15, 2, 4, 11, 6, 1, 1, {0}, {0} }; + + +#ifndef INCLUDESTATIC + +int rs_init_RS255(RS_t *RS); +int rs_init_RS255ccsds(RS_t *RS); +int rs_init_RS15ccsds(RS_t *RS); +int rs_init_BCH64(RS_t *RS); + +int rs_encode(RS_t *RS, ui8_t cw[]); +int rs_decode(RS_t *RS, ui8_t cw[], ui8_t *err_pos, ui8_t *err_val); +int rs_decode_ErrEra(RS_t *RS, ui8_t cw[], int nera, ui8_t era_pos[], ui8_t *err_pos, ui8_t *err_val); +int rs_decode_bch_gf2t2(RS_t *RS, ui8_t cw[], ui8_t *err_pos, ui8_t *err_val); + +#endif + diff --git a/demod/mod/demod_mod.c b/demod/mod/demod_mod.c new file mode 100644 index 0000000..d1383b1 --- /dev/null +++ b/demod/mod/demod_mod.c @@ -0,0 +1,827 @@ + +/* + * sync header: correlation/matched filter + * compile: + * gcc -c demod_mod.c + * + * author: zilog80 + */ + +/* ------------------------------------------------------------------------------------ */ + +#include +#include +#include + +#include "demod_mod.h" + +/* ------------------------------------------------------------------------------------ */ + + +static void raw_dft(dft_t *dft, float complex *Z) { + int s, l, l2, i, j, k; + float complex w1, w2, T; + + j = 1; + for (i = 1; i < dft->N; i++) { + if (i < j) { + T = Z[j-1]; + Z[j-1] = Z[i-1]; + Z[i-1] = T; + } + k = dft->N/2; + while (k < j) { + j = j - k; + k = k/2; + } + j = j + k; + } + + for (s = 0; s < dft->LOG2N; s++) { + l2 = 1 << s; + l = l2 << 1; + w1 = (float complex)1.0; + w2 = dft->ew[s]; // cexp(-I*M_PI/(float)l2) + for (j = 1; j <= l2; j++) { + for (i = j; i <= dft->N; i += l) { + k = i + l2; + T = Z[k-1] * w1; + Z[k-1] = Z[i-1] - T; + Z[i-1] = Z[i-1] + T; + } + w1 = w1 * w2; + } + } +} + +static void cdft(dft_t *dft, float complex *z, float complex *Z) { + int i; + for (i = 0; i < dft->N; i++) Z[i] = z[i]; + raw_dft(dft, Z); +} + +static void rdft(dft_t *dft, float *x, float complex *Z) { + int i; + for (i = 0; i < dft->N; i++) Z[i] = (float complex)x[i]; + raw_dft(dft, Z); +} + +static void Nidft(dft_t *dft, float complex *Z, float complex *z) { + int i; + for (i = 0; i < dft->N; i++) z[i] = conj(Z[i]); + raw_dft(dft, z); + // idft(): + // for (i = 0; i < dft->N; i++) z[i] = conj(z[i])/(float)dft->N; // hier: z reell +} + +static float bin2freq0(dft_t *dft, int k) { + float fq = dft->sr * k / /*(float)*/dft->N; + if (fq >= dft->sr/2.0) fq -= dft->sr; + return fq; +} +static float bin2freq(dft_t *dft, int k) { + float fq = k / (float)dft->N; + if ( fq >= 0.5) fq -= 1.0; + return fq*dft->sr; +} +static float bin2fq(dft_t *dft, int k) { + float fq = k / (float)dft->N; + if ( fq >= 0.5) fq -= 1.0; + return fq; +} + +static int max_bin(dft_t *dft, float complex *Z) { + int k, kmax; + double max; + + max = 0; kmax = 0; + for (k = 0; k < dft->N; k++) { + if (cabs(Z[k]) > max) { + max = cabs(Z[k]); + kmax = k; + } + } + + return kmax; +} + +static int dft_window(dft_t *dft, int w) { + int n; + + if (w < 0 || w > 3) return -1; + + for (n = 0; n < dft->N2; n++) { + switch (w) + { + case 0: // (boxcar) + dft->win[n] = 1.0; + break; + case 1: // Hann + dft->win[n] = 0.5 * ( 1.0 - cos(2*M_PI*n/(float)(dft->N2-1)) ); + break ; + case 2: // Hamming + dft->win[n] = 25/46.0 + (1.0 - 25/46.0)*cos(2*M_PI*n / (float)(dft->N2-1)); + break ; + case 3: // Blackmann + dft->win[n] = 7938/18608.0 + - 9240/18608.0*cos(2*M_PI*n / (float)(dft->N2-1)) + + 1430/18608.0*cos(4*M_PI*n / (float)(dft->N2-1)); + break ; + } + } + while (n < dft->N) dft->win[n++] = 0.0; + + return 0; +} + + +/* ------------------------------------------------------------------------------------ */ + +static int getCorrDFT(dsp_t *dsp) { + int i; + int mp = -1; + float mx = 0.0; + float mx2 = 0.0; + float re_cx = 0.0; + float xnorm = 1; + ui32_t mpos = 0; + + ui32_t pos = dsp->sample_out; + + dsp->mv = 0.0; + dsp->dc = 0.0; + + if (dsp->K + dsp->L > dsp->DFT.N) return -1; + if (dsp->sample_out < dsp->L) return -2; + + + dsp->dc = get_bufmu(dsp, pos - dsp->sample_out); //oder unten: dft_dc = creal(X[0])/(K+L); + // wenn richtige Stelle (Varianz pruefen, kein M10-carrier?), dann von bufs[] subtrahieren + + + for (i = 0; i < dsp->K + dsp->L; i++) (dsp->DFT).xn[i] = dsp->bufs[(pos+dsp->M -(dsp->K + dsp->L-1) + i) % dsp->M]; + while (i < dsp->DFT.N) (dsp->DFT).xn[i++] = 0.0; + + rdft(&dsp->DFT, dsp->DFT.xn, dsp->DFT.X); + + // dft_dc = creal(dsp->DFT.X[0])/dsp->DFT.N; + + for (i = 0; i < dsp->DFT.N; i++) dsp->DFT.Z[i] = dsp->DFT.X[i]*dsp->DFT.Fm[i]; + + Nidft(&dsp->DFT, dsp->DFT.Z, dsp->DFT.cx); + + + // relativ Peak - Normierung erst zum Schluss; + // dann jedoch nicht zwingend corr-Max wenn FM-Amplitude bzw. norm(x) nicht konstant + // (z.B. rs41 Signal-Pausen). Moeglicherweise wird dann wahres corr-Max in dem + // K-Fenster nicht erkannt, deshalb K nicht zu gross waehlen. + // + mx2 = 0.0; // t = L-1 + for (i = dsp->L-1; i < dsp->K + dsp->L; i++) { // i=t .. i=t+K < t+1+K + re_cx = creal(dsp->DFT.cx[i]); // imag(cx)=0 + if (re_cx*re_cx > mx2) { + mx = re_cx; + mx2 = mx*mx; + mp = i; + } + } + if (mp == dsp->L-1 || mp == dsp->K + dsp->L-1) return -4; // Randwert + // mp == t mp == K+t + + mpos = pos - (dsp->K + dsp->L-1) + mp; // t = L-1 + + //xnorm = sqrt(dsp->qs[(mpos + 2*dsp->M) % dsp->M]); // Nvar = L + xnorm = 0.0; + for (i = 0; i < dsp->L; i++) xnorm += dsp->bufs[(mpos-i + dsp->M) % dsp->M]*dsp->bufs[(mpos-i + dsp->M) % dsp->M]; + xnorm = sqrt(xnorm); + + mx /= xnorm*(dsp->DFT).N; + + dsp->mv = mx; + dsp->mv_pos = mpos; + + if (pos == dsp->sample_out) dsp->buffered = dsp->sample_out - mpos; + + return mp; +} + +/* ------------------------------------------------------------------------------------ */ + +static int findstr(char *buff, char *str, int pos) { + int i; + for (i = 0; i < 4; i++) { + if (buff[(pos+i)%4] != str[i]) break; + } + return i; +} + +float read_wav_header(pcm_t *pcm, FILE *fp) { + char txt[4+1] = "\0\0\0\0"; + unsigned char dat[4]; + int byte, p=0; + int sample_rate = 0, bits_sample = 0, channels = 0; + + if (fread(txt, 1, 4, fp) < 4) return -1; + if (strncmp(txt, "RIFF", 4)) return -1; + if (fread(txt, 1, 4, fp) < 4) return -1; + // pos_WAVE = 8L + if (fread(txt, 1, 4, fp) < 4) return -1; + if (strncmp(txt, "WAVE", 4)) return -1; + // pos_fmt = 12L + for ( ; ; ) { + if ( (byte=fgetc(fp)) == EOF ) return -1; + txt[p % 4] = byte; + p++; if (p==4) p=0; + if (findstr(txt, "fmt ", p) == 4) break; + } + if (fread(dat, 1, 4, fp) < 4) return -1; + if (fread(dat, 1, 2, fp) < 2) return -1; + + if (fread(dat, 1, 2, fp) < 2) return -1; + channels = dat[0] + (dat[1] << 8); + + if (fread(dat, 1, 4, fp) < 4) return -1; + memcpy(&sample_rate, dat, 4); //sample_rate = dat[0]|(dat[1]<<8)|(dat[2]<<16)|(dat[3]<<24); + + if (fread(dat, 1, 4, fp) < 4) return -1; + if (fread(dat, 1, 2, fp) < 2) return -1; + //byte = dat[0] + (dat[1] << 8); + + if (fread(dat, 1, 2, fp) < 2) return -1; + bits_sample = dat[0] + (dat[1] << 8); + + // pos_dat = 36L + info + for ( ; ; ) { + if ( (byte=fgetc(fp)) == EOF ) return -1; + txt[p % 4] = byte; + p++; if (p==4) p=0; + if (findstr(txt, "data", p) == 4) break; + } + if (fread(dat, 1, 4, fp) < 4) return -1; + + + fprintf(stderr, "sample_rate: %d\n", sample_rate); + fprintf(stderr, "bits : %d\n", bits_sample); + fprintf(stderr, "channels : %d\n", channels); + + if (pcm->sel_ch < 0 || pcm->sel_ch >= channels) pcm->sel_ch = 0; // default channel: 0 + fprintf(stderr, "channel-In : %d\n", pcm->sel_ch+1); + + if ((bits_sample != 8) && (bits_sample != 16)) return -1; + + + pcm->sr = sample_rate; + pcm->bps = bits_sample; + pcm->nch = channels; + + return 0; +} + +static int f32read_sample(dsp_t *dsp, float *s) { + int i; + short b = 0; + + for (i = 0; i < dsp->nch; i++) { + + if (fread( &b, dsp->bps/8, 1, dsp->fp) != 1) return EOF; + + if (i == dsp->ch) { // i = 0: links bzw. mono + //if (bits_sample == 8) sint = b-128; // 8bit: 00..FF, centerpoint 0x80=128 + //if (bits_sample == 16) sint = (short)b; + + if (dsp->bps == 8) { b -= 128; } + *s = b/128.0; + if (dsp->bps == 16) { *s /= 256.0; } + } + } + + return 0; +} + +static int f32read_csample(dsp_t *dsp, float complex *z) { + short x = 0, y = 0; + + if (fread( &x, dsp->bps/8, 1, dsp->fp) != 1) return EOF; + if (fread( &y, dsp->bps/8, 1, dsp->fp) != 1) return EOF; + + *z = x + I*y; + + if (dsp->bps == 8) { *z -= 128 + I*128; } + *z /= 128.0; + if (dsp->bps == 16) { *z /= 256.0; } + + return 0; +} + +float get_bufvar(dsp_t *dsp, int ofs) { + float mu = dsp->xs[(dsp->sample_out+dsp->M + ofs) % dsp->M]/dsp->Nvar; + float var = dsp->qs[(dsp->sample_out+dsp->M + ofs) % dsp->M]/dsp->Nvar - mu*mu; + return var; +} + +float get_bufmu(dsp_t *dsp, int ofs) { + float mu = dsp->xs[(dsp->sample_out+dsp->M + ofs) % dsp->M]/dsp->Nvar; + return mu; +} + +static int get_SNR(dsp_t *dsp) { + + if (dsp->opt_iq) + // if(dsp->rs_typ == RS41) + { + if (dsp->sample_posnoise > 0) // rs41 + { + if (dsp->sample_out >= dsp->sample_posframe && dsp->sample_out < dsp->sample_posframe+dsp->len_sq) { + if (dsp->sample_out == dsp->sample_posframe) dsp->V_signal = 0.0; + dsp->V_signal += cabs(dsp->rot_iqbuf[dsp->sample_out % dsp->N_IQBUF]); + } + if (dsp->sample_out == dsp->sample_posframe+dsp->len_sq) dsp->V_signal /= (double)dsp->len_sq; + + if (dsp->sample_out >= dsp->sample_posnoise && dsp->sample_out < dsp->sample_posnoise+dsp->len_sq) { + if (dsp->sample_out == dsp->sample_posnoise) dsp->V_noise = 0.0; + dsp->V_noise += cabs(dsp->rot_iqbuf[dsp->sample_out % dsp->N_IQBUF]); + } + if (dsp->sample_out == dsp->sample_posnoise+dsp->len_sq) { + dsp->V_noise /= (double)dsp->len_sq; + if (dsp->V_signal > 0 && dsp->V_noise > 0) { + // iq-samples/V [-1..1] + // dBw = 2*dBv, P=c*U*U + // dBw = 2*10*log10(V/V0) + dsp->SNRdB = 20.0 * log10(dsp->V_signal/dsp->V_noise+1e-20); + } + } + } + } + else dsp->SNRdB = 0; + + return 0; +} + +int f32buf_sample(dsp_t *dsp, int inv) { + float s = 0.0; + float xneu, xalt; + + float complex z, w, z0; + //static float complex z0; //= 1.0; + double gain = 0.8; + int n; + + double t = dsp->sample_in / (double)dsp->sr; + + if (dsp->opt_iq) { + + if ( f32read_csample(dsp, &z) == EOF ) return EOF; + dsp->raw_iqbuf[dsp->sample_in % dsp->N_IQBUF] = z; + + z *= cexp(-t*2*M_PI*dsp->df*I); + z0 = dsp->rot_iqbuf[(dsp->sample_in-1 + dsp->N_IQBUF) % dsp->N_IQBUF]; + w = z * conj(z0); + s = gain * carg(w)/M_PI; + //z0 = z; + dsp->rot_iqbuf[dsp->sample_in % dsp->N_IQBUF] = z; + + /* //if (rs_type==rs41) get_SNR(dsp); + // rs41, constant amplitude, avg/filter + s = 0.0; + for (n = 0; n < dsp->sps; n++) s += cabs(dsp->rot_iqbuf[(dsp->sample_in - n + dsp->N_IQBUF) % dsp->N_IQBUF]); + s /= (float)n; + */ + + if (dsp->opt_iq >= 2) + { + double xbit = 0.0; + //float complex xi = cexp(+I*M_PI*dsp->h/dsp->sps); + double f1 = -dsp->h*dsp->sr/(2*dsp->sps); + double f2 = -f1; + + float complex X1 = 0; + float complex X2 = 0; + + n = dsp->sps; + while (n > 0) { + n--; + t = -n / (double)dsp->sr; + z = dsp->rot_iqbuf[(dsp->sample_in - n + dsp->N_IQBUF) % dsp->N_IQBUF]; // +1 + X1 += z*cexp(-t*2*M_PI*f1*I); + X2 += z*cexp(-t*2*M_PI*f2*I); + } + + xbit = cabs(X2) - cabs(X1); + + s = xbit / dsp->sps; + } + } + else { + if (f32read_sample(dsp, &s) == EOF) return EOF; + } + + if (inv) s = -s; // swap IQ? + dsp->bufs[dsp->sample_in % dsp->M] = s - dsp->dc_ofs; + + xneu = dsp->bufs[(dsp->sample_in ) % dsp->M]; + xalt = dsp->bufs[(dsp->sample_in+dsp->M - dsp->Nvar) % dsp->M]; + dsp->xsum += xneu - xalt; // + xneu - xalt + dsp->qsum += (xneu - xalt)*(xneu + xalt); // + xneu*xneu - xalt*xalt + dsp->xs[dsp->sample_in % dsp->M] = dsp->xsum; + dsp->qs[dsp->sample_in % dsp->M] = dsp->qsum; + + + dsp->sample_out = dsp->sample_in - dsp->delay; + + dsp->sample_in += 1; + + return 0; +} + +static int read_bufbit(dsp_t *dsp, int symlen, char *bits, ui32_t mvp, int pos) { +// symlen==2: manchester2 0->10,1->01->1: 2.bit + + double rbitgrenze = pos*symlen*dsp->sps; + ui32_t rcount = ceil(rbitgrenze);//+0.99; // dfm? + + double sum = 0.0; + + + rbitgrenze += dsp->sps; + do { + sum += dsp->bufs[(rcount + mvp + dsp->M) % dsp->M]; + rcount++; + } while (rcount < rbitgrenze); // n < dsp->sps + + if (symlen == 2) { + rbitgrenze += dsp->sps; + do { + sum -= dsp->bufs[(rcount + mvp + dsp->M) % dsp->M]; + rcount++; + } while (rcount < rbitgrenze); // n < dsp->sps + } + + + if (symlen != 2) { + if (sum >= 0) *bits = '1'; + else *bits = '0'; + } + else { + if (sum >= 0) strncpy(bits, "10", 2); + else strncpy(bits, "01", 2); + } + + return 0; +} + +static int headcmp(dsp_t *dsp, int opt_dc) { + int errs = 0; + int pos; + int step = 1; + char sign = 0; + int len = dsp->hdrlen/dsp->symhd; + int inv = dsp->mv < 0; + + if (dsp->symhd != 1) step = 2; + if (inv) sign=1; + + for (pos = 0; pos < len; pos++) { // L = dsp->hdrlen * dsp->sps + 0.5; + //read_bufbit(dsp, dsp->symhd, dsp->rawbits+pos*step, mvp+1-(int)(len*dsp->sps), pos); + read_bufbit(dsp, dsp->symhd, dsp->rawbits+pos*step, dsp->mv_pos+1-dsp->L, pos); + } + dsp->rawbits[pos] = '\0'; + + while (len > 0) { + if ((dsp->rawbits[len-1]^sign) != dsp->hdr[len-1]) errs += 1; + len--; + } + + if (opt_dc && errs < 3) { + dsp->dc_ofs += dsp->dc; + } + + return errs; +} + +int get_fqofs_rs41(dsp_t *dsp, ui32_t mvp, float *freq, float *snr) { + int j; + int buf_start; + int presamples; + + // if(dsp->rs_typ == RS41_PREAMBLE) ... + if (dsp->opt_iq) + { + presamples = 256*dsp->sps; + + if (presamples > dsp->DFT.N2) presamples = dsp->DFT.N2; + + buf_start = mvp - dsp->hdrlen*dsp->sps - presamples; + + while (buf_start < 0) buf_start += dsp->N_IQBUF; + + for (j = 0; j < dsp->DFT.N2; j++) { + dsp->DFT.Z[j] = dsp->DFT.win[j]*dsp->raw_iqbuf[(buf_start+j) % dsp->N_IQBUF]; + } + while (j < dsp->DFT.N) dsp->DFT.Z[j++] = 0; + + raw_dft(&dsp->DFT, dsp->DFT.Z); + dsp->df = bin2freq(&dsp->DFT, max_bin(&dsp->DFT, dsp->DFT.Z)); + + // if |df|df) > 1000.0) dsp->df = 0.0; + + + dsp->sample_posframe = dsp->sample_in; // > sample_out //mvp - dsp->hdrlen*dsp->sps; + dsp->sample_posnoise = mvp + dsp->sr*7/8.0; // rs41 + + + *freq = dsp->df; + *snr = dsp->SNRdB; + } + else return -1; + + return 0; +} + +/* -------------------------------------------------------------------------- */ + +int read_slbit(dsp_t *dsp, int *bit, int inv, int ofs, int pos, float l, int spike) { +// symlen==2: manchester2 10->0,01->1: 2.bit + + float sample; + float avg; + float ths = 0.5, scale = 0.27; + + double sum = 0.0; + double mid; + //double l = 1.0; + + double bg = pos*dsp->symlen*dsp->sps; + + if (pos == 0) { + bg = 0; + dsp->sc = 0; + } + + + if (dsp->symlen == 2) { + mid = bg + (dsp->sps-1)/2.0; + bg += dsp->sps; + do { + if (dsp->buffered > 0) dsp->buffered -= 1; + else if (f32buf_sample(dsp, inv) == EOF) return EOF; + + sample = dsp->bufs[(dsp->sample_out-dsp->buffered + ofs + dsp->M) % dsp->M]; + if (spike && fabs(sample - avg) > ths) { + avg = 0.5*(dsp->bufs[(dsp->sample_out-dsp->buffered-1 + ofs + dsp->M) % dsp->M] + +dsp->bufs[(dsp->sample_out-dsp->buffered+1 + ofs + dsp->M) % dsp->M]); + sample = avg + scale*(sample - avg); // spikes + } + + if ( l < 0 || (mid-l < dsp->sc && dsp->sc < mid+l)) sum -= sample; + + dsp->sc++; + } while (dsp->sc < bg); // n < dsp->sps + } + + mid = bg + (dsp->sps-1)/2.0; + bg += dsp->sps; + do { + if (dsp->buffered > 0) dsp->buffered -= 1; + else if (f32buf_sample(dsp, inv) == EOF) return EOF; + + sample = dsp->bufs[(dsp->sample_out-dsp->buffered + ofs + dsp->M) % dsp->M]; + if (spike && fabs(sample - avg) > ths) { + avg = 0.5*(dsp->bufs[(dsp->sample_out-dsp->buffered-1 + ofs + dsp->M) % dsp->M] + +dsp->bufs[(dsp->sample_out-dsp->buffered+1 + ofs + dsp->M) % dsp->M]); + sample = avg + scale*(sample - avg); // spikes + } + + if ( l < 0 || (mid-l < dsp->sc && dsp->sc < mid+l)) sum += sample; + + dsp->sc++; + } while (dsp->sc < bg); // n < dsp->sps + + + if (sum >= 0) *bit = 1; + else *bit = 0; + + return 0; +} + +/* -------------------------------------------------------------------------- */ + + +#define SQRT2 1.4142135624 // sqrt(2) +// sigma = sqrt(log(2)) / (2*PI*BT): +//#define SIGMA 0.2650103635 // BT=0.5: 0.2650103635 , BT=0.3: 0.4416839392 + +// Gaussian FM-pulse +static double Q(double x) { + return 0.5 - 0.5*erf(x/SQRT2); +} +static double pulse(double t, double sigma) { + return Q((t-0.5)/sigma) - Q((t+0.5)/sigma); +} + + +static double norm2_vect(float *vect, int n) { + int i; + double x, y = 0.0; + for (i = 0; i < n; i++) { + x = vect[i]; + y += x*x; + } + return y; +} + +int init_buffers(dsp_t *dsp) { + + int i, pos; + float b0, b1, b2, b, t; + float normMatch; + double sigma = sqrt(log(2)) / (2*M_PI*dsp->BT); + + int p2 = 1; + int K, L, M; + int n, k; + float *m = NULL; + + + L = dsp->hdrlen * dsp->sps + 0.5; + M = 3*L; + //if (dsp->sps < 6) M = 6*L; + + dsp->delay = L/16; + dsp->sample_in = 0; + + p2 = 1; + while (p2 < M) p2 <<= 1; + while (p2 < 0x2000) p2 <<= 1; // or 0x4000, if sample not too short + M = p2; + dsp->DFT.N = p2; + dsp->DFT.LOG2N = log(dsp->DFT.N)/log(2)+0.1; // 32bit cpu ... intermediate floating-point precision + //while ((1 << dsp->DFT.LOG2N) < dsp->DFT.N) dsp->DFT.LOG2N++; // better N = (1 << LOG2N) ... + + K = M-L - dsp->delay; // L+K < M + + dsp->DFT.sr = dsp->sr; + + dsp->K = K; + dsp->L = L; + dsp->M = M; + + dsp->Nvar = L; // wenn Nvar fuer xnorm, dann Nvar=rshd.L + + + dsp->bufs = (float *)calloc( M+1, sizeof(float)); if (dsp->bufs == NULL) return -100; + dsp->match = (float *)calloc( L+1, sizeof(float)); if (dsp->match == NULL) return -100; + + dsp->xs = (float *)calloc( M+1, sizeof(float)); if (dsp->xs == NULL) return -100; + dsp->qs = (float *)calloc( M+1, sizeof(float)); if (dsp->qs == NULL) return -100; + + dsp->rawbits = (char *)calloc( 2*dsp->hdrlen+1, sizeof(char)); if (dsp->rawbits == NULL) return -100; + + + for (i = 0; i < M; i++) dsp->bufs[i] = 0.0; + + + for (i = 0; i < L; i++) { + pos = i/dsp->sps; + t = (i - pos*dsp->sps)/dsp->sps - 0.5; + + b1 = ((dsp->hdr[pos] & 0x1) - 0.5)*2.0; + b = b1*pulse(t, sigma); + + if (pos > 0) { + b0 = ((dsp->hdr[pos-1] & 0x1) - 0.5)*2.0; + b += b0*pulse(t+1, sigma); + } + + if (pos < dsp->hdrlen-1) { + b2 = ((dsp->hdr[pos+1] & 0x1) - 0.5)*2.0; + b += b2*pulse(t-1, sigma); + } + + dsp->match[i] = b; + } + + normMatch = sqrt( norm2_vect(dsp->match, L) ); + for (i = 0; i < L; i++) { + dsp->match[i] /= normMatch; + } + + + dsp->DFT.xn = calloc(dsp->DFT.N+1, sizeof(float)); if (dsp->DFT.xn == NULL) return -1; + + dsp->DFT.Fm = calloc(dsp->DFT.N+1, sizeof(float complex)); if (dsp->DFT.Fm == NULL) return -1; + dsp->DFT.X = calloc(dsp->DFT.N+1, sizeof(float complex)); if (dsp->DFT.X == NULL) return -1; + dsp->DFT.Z = calloc(dsp->DFT.N+1, sizeof(float complex)); if (dsp->DFT.Z == NULL) return -1; + dsp->DFT.cx = calloc(dsp->DFT.N+1, sizeof(float complex)); if (dsp->DFT.cx == NULL) return -1; + + dsp->DFT.ew = calloc(dsp->DFT.LOG2N+1, sizeof(float complex)); if (dsp->DFT.ew == NULL) return -1; + + // FFT window + // a) N2 = N + // b) N2 < N (interpolation) + dsp->DFT.win = calloc(dsp->DFT.N+1, sizeof(float complex)); if (dsp->DFT.win == NULL) return -1; // float real + dsp->DFT.N2 = dsp->DFT.N; + //dsp->DFT.N2 = dsp->DFT.N/2 - 1; // N=2^log2N + dft_window(&dsp->DFT, 1); + + for (n = 0; n < dsp->DFT.LOG2N; n++) { + k = 1 << n; + dsp->DFT.ew[n] = cexp(-I*M_PI/(float)k); + } + + m = calloc(dsp->DFT.N+1, sizeof(float)); if (m == NULL) return -1; + for (i = 0; i < L; i++) m[L-1 - i] = dsp->match[i]; // t = L-1 + while (i < dsp->DFT.N) m[i++] = 0.0; + rdft(&dsp->DFT, m, dsp->DFT.Fm); + + free(m); m = NULL; + + + if (dsp->opt_iq) + { + if (dsp->nch < 2) return -1; + + dsp->N_IQBUF = dsp->DFT.N; + dsp->raw_iqbuf = calloc(dsp->N_IQBUF+1, sizeof(float complex)); if (dsp->raw_iqbuf == NULL) return -1; + dsp->rot_iqbuf = calloc(dsp->N_IQBUF+1, sizeof(float complex)); if (dsp->rot_iqbuf == NULL) return -1; + + dsp->len_sq = dsp->sps*8; + } + + + return K; +} + +int free_buffers(dsp_t *dsp) { + + if (dsp->match) { free(dsp->match); dsp->match = NULL; } + if (dsp->bufs) { free(dsp->bufs); dsp->bufs = NULL; } + if (dsp->xs) { free(dsp->xs); dsp->xs = NULL; } + if (dsp->qs) { free(dsp->qs); dsp->qs = NULL; } + if (dsp->rawbits) { free(dsp->rawbits); dsp->rawbits = NULL; } + + if (dsp->DFT.xn) { free(dsp->DFT.xn); dsp->DFT.xn = NULL; } + if (dsp->DFT.ew) { free(dsp->DFT.ew); dsp->DFT.ew = NULL; } + if (dsp->DFT.Fm) { free(dsp->DFT.Fm); dsp->DFT.Fm = NULL; } + if (dsp->DFT.X) { free(dsp->DFT.X); dsp->DFT.X = NULL; } + if (dsp->DFT.Z) { free(dsp->DFT.Z); dsp->DFT.Z = NULL; } + if (dsp->DFT.cx) { free(dsp->DFT.cx); dsp->DFT.cx = NULL; } + + if (dsp->DFT.win) { free(dsp->DFT.win); dsp->DFT.win = NULL; } + + if (dsp->opt_iq) + { + if (dsp->raw_iqbuf) { free(dsp->raw_iqbuf); dsp->raw_iqbuf = NULL; } + if (dsp->rot_iqbuf) { free(dsp->rot_iqbuf); dsp->rot_iqbuf = NULL; } + } + + return 0; +} + +/* ------------------------------------------------------------------------------------ */ + +ui32_t get_sample(dsp_t *dsp) { + return dsp->sample_out; +} + +/* ------------------------------------------------------------------------------------ */ + + +int find_header(dsp_t *dsp, float thres, int hdmax, int bitofs, int opt_dc) { + ui32_t k = 0; + ui32_t mvpos0 = 0; + int mp; + int header_found = 0; + int herrs; + + while ( f32buf_sample(dsp, 0) != EOF ) { + + k += 1; + if (k >= dsp->K-4) { + mvpos0 = dsp->mv_pos; + mp = getCorrDFT(dsp); // correlation score -> dsp->mv + //if (option_auto == 0 && dsp->mv < 0) mv = 0; + k = 0; + } + else { + dsp->mv = 0.0; + continue; + } + + if (dsp->mv > thres || dsp->mv < -thres) { + + if (dsp->mv_pos > mvpos0) { + + header_found = 0; + herrs = headcmp(dsp, opt_dc); + if (herrs <= hdmax) header_found = 1; // max bitfehler in header + + if (header_found) return 1; + } + } + + } + + return EOF; +} + diff --git a/demod/mod/demod_mod.h b/demod/mod/demod_mod.h new file mode 100644 index 0000000..90f7647 --- /dev/null +++ b/demod/mod/demod_mod.h @@ -0,0 +1,119 @@ + + +#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; + int symhd; + float sps; // samples per symbol + float _spb; // samples per bit + float br; // baud rate + // + ui32_t sample_in; + ui32_t sample_out; + ui32_t delay; + ui32_t sc; + int buffered; + int L; + int M; + int K; + float *match; + float *bufs; + float dc_ofs; + float dc; + float mv; + ui32_t mv_pos; + // + int N_norm; + int Nvar; + float xsum; + float qsum; + float *xs; + float *qs; + + // IQ-data + int opt_iq; + int N_IQBUF; + float complex *raw_iqbuf; + float complex *rot_iqbuf; + + // + char *rawbits; + char *hdr; + int hdrlen; + + // + float BT; // bw/time (ISI) + float h; // modulation index + + // DFT + dft_t DFT; + + double df; + int len_sq; + + ui32_t sample_posframe; + ui32_t sample_posnoise; + + double V_noise; + double V_signal; + double SNRdB; + +} dsp_t; + + +typedef struct { + int sr; // sample_rate + int bps; // bits_sample bits/sample + int nch; // channels + int sel_ch; // select wav channel +} pcm_t; + + + +float read_wav_header(pcm_t *, FILE *); +int f32buf_sample(dsp_t *, int); +int read_slbit(dsp_t *, int*, int, int, int, float, int); + +int get_fqofs_rs41(dsp_t *, ui32_t, float *, float *); +float get_bufvar(dsp_t *, int); +float get_bufmu(dsp_t *, int); + +int init_buffers(dsp_t *); +int free_buffers(dsp_t *); + +ui32_t get_sample(dsp_t *); + +int find_header(dsp_t *, float, int, int, int); + diff --git a/demod/mod/dfm09mod.c b/demod/mod/dfm09mod.c new file mode 100644 index 0000000..4c7bebe --- /dev/null +++ b/demod/mod/dfm09mod.c @@ -0,0 +1,1118 @@ + +/* + * dfm09 (dfm06) + * sync header: correlation/matched filter + * files: dfm09mod.c demod_mod.h demod_mod.c + * compile: + * gcc -c demod_mod.c + * gcc dfm09mod.c demod_mod.o -lm -o dfm09mod + * + * author: zilog80 + */ + +#include +#include +#include +#include + +#ifdef CYGWIN + #include // cygwin: _setmode() + #include +#endif + + +#include "demod_mod.h" + + +typedef struct { + i8_t vbs; // verbose output + i8_t raw; // raw frames + i8_t crc; // CRC check output + i8_t ecc; // Reed-Solomon ECC + i8_t sat; // GPS sat data + i8_t ptu; // PTU: temperature + i8_t inv; + i8_t aut; + i8_t jsn; // JSON output (auto_rx) + i8_t dst; // continuous pcks 0..8 + i8_t dbg; +} option_t; + +typedef struct { + int ec; + float ts; +} pcksts_t; + +typedef struct { + ui8_t max_ch; + ui8_t nul_ch; + ui8_t chXbit; + ui32_t SN_X; + ui32_t chX[2]; +} sn_t; + +#define BITFRAME_LEN 280 + +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+2]; + float status[2]; + float _frmcnt; + char sonde_id[16]; // "ID__:xxxxxxxx\0\0" + char frame_bits[BITFRAME_LEN+4]; + char dat_str[9][13+1]; + sn_t snc; + pcksts_t pck[9]; + option_t option; + int ptu_out; +} gpx_t; + + +//#define HEADLEN 32 +// DFM09: Manchester2: 01->1,10->0 +static char dfm_rawheader[] = "10011010100110010101101001010101"; //->"0100010111001111"; // 0x45CF (big endian) +static char dfm_header[] = "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 + +static 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}}; +static ui8_t He[8] = { 0x7, 0xB, 0xD, 0xE, 0x8, 0x4, 0x2, 0x1}; // Spalten von H: + // 1-bit-error-Syndrome + +static 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; +} + +static 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 + } + } + } +} + +static 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; +} + +static int hamming(int opt_ecc, 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 (opt_ecc) { + ecc = check(ham+B*i); + if (ecc > 0) ret |= (1<>i)&1 ) ecn++; + } + return ecn; +} + +static int dat_out(gpx_t *gpx, ui8_t *dat_bits, int ec) { + int i, ret = 0; + 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); + gpx->dat_str[fr_id][i] = nib2chr(nib); + } + gpx->dat_str[fr_id][13] = '\0'; + + gpx->pck[fr_id].ts = gpx->_frmcnt; // time_stamp,frame_count,... + if (gpx->option.ecc) { + gpx->pck[fr_id].ec = ec; // option_ecc laesst -1 garnicht durch + if (ec > 0) { + ui8_t ecn = cnt_biterr(ec); + gpx->pck[fr_id].ec = ecn; + if ((gpx->option.dst || gpx->option.jsn) && ecn > 4) gpx->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) +static 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) +static float fl24(int d) { // float24 + int val, p; + float f; + p = (d>>20) & 0xF; + val = d & 0xFFFFF; + f = val/(float)(1<meas24[0], + f1 = gpx->meas24[3], + f2 = gpx->meas24[4]; + if (gpx->ptu_out >= 0xC) { + f = gpx->meas24[0+1]; + f1 = gpx->meas24[3+2]; + f2 = gpx->meas24[4+2]; + } + //float *meas = gpx->meas24; + float B0 = 3260.0; // B/Kelvin, fit -55C..+40C + float T0 = 25 + 273.15; // t0=25C + float R0 = 5.0e3; // R0=R25=5k + float Rf = 220e3; // Rf = 220k + float g = f2/Rf; + float R = (f-f1) / g; // meas[0,3,4] > 0 ? + float T = 0; // T/Kelvin + if (f*f1*f2 == 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] +} +static float get_Temp2(gpx_t *gpx) { // 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 = gpx->meas24[0], + f1 = gpx->meas24[3], + f2 = gpx->meas24[4]; + if (gpx->ptu_out >= 0xC) { + f = gpx->meas24[0+1]; + f1 = gpx->meas24[3+2]; + f2 = gpx->meas24[4+2]; + } + 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 + + // ptu_out=0xD: Rs_o=13.6, Rf2=? + 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 (gpx->option.ptu && gpx->ptu_out && gpx->option.dbg) { + printf(" (Rso: %.1f , Rb: %.1f)", Rs_o/1e3, Rb/1e3); + } + + return T - 273.15; +// DFM-06: meas20 * 16 = meas24 +} +static float get_Temp4(gpx_t *gpx) { // 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 f = gpx->meas24[0], + f1 = gpx->meas24[3], + f2 = gpx->meas24[4]; + if (gpx->ptu_out >= 0xC) { + f = gpx->meas24[0+1]; + f1 = gpx->meas24[3+2]; + f2 = gpx->meas24[4+2]; + } + //float *meas = gpx->meas24; + float Rf = 220e3; // Rf = 220k + float g = f2/Rf; + float R = (f-f1) / g; // f,f1,f2 > 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 +static int conf_out(gpx_t *gpx, ui8_t *conf_bits, int ec) { + int ret = 0; + int val; + ui8_t conf_id; + ui8_t hl; + ui32_t SN6, SN; + ui8_t dfm6typ; + ui8_t sn2_ch, sn_ch; + + + conf_id = bits2val(conf_bits, 4); + + if (conf_id > 4 && bits2val(conf_bits+8, 4*5) == 0) gpx->snc.nul_ch = bits2val(conf_bits, 8); + + dfm6typ = ((gpx->snc.nul_ch & 0xF0)==0x50) && (gpx->snc.nul_ch & 0x0F); + if (dfm6typ) gpx->ptu_out = 6; + if (dfm6typ && (gpx->sonde_typ & 0xF) > 6) + { // reset if 0x5A, 0x5B (DFM-06) + gpx->sonde_typ = 0; + gpx->snc.max_ch = conf_id; + } + + if (conf_id > 4 && conf_id > gpx->snc.max_ch) gpx->snc.max_ch = conf_id; // mind. 5 Kanaele // reset? lower 0xsCaaaab? + + if (conf_id > 4 && conf_id == (gpx->snc.nul_ch>>4)+1) + { + sn2_ch = bits2val(conf_bits, 8); + sn_ch = ((sn2_ch>>4) & 0xF); + if (conf_id == sn_ch) + { + if ( (gpx->snc.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; + gpx->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); + gpx->snc.chX[hl] = (val >> 4) & 0xFFFF; + gpx->snc.chXbit |= 1 << hl; + if (gpx->snc.chXbit == 3) { + SN = (gpx->snc.chX[0] << 16) | gpx->snc.chX[1]; + if ( SN == gpx->snc.SN_X || gpx->snc.SN_X == 0 ) { + + gpx->sonde_typ = SNbit | sn_ch; + gpx->SN = SN; + + if (sn_ch == 0xA /*&& (sn2_ch & 0xF) == 0xC*/) gpx->ptu_out = sn_ch; else gpx->ptu_out = 0; + if (sn_ch == 0xC) gpx->ptu_out = sn_ch;// DFM-09P, DFM-17 ? + if (sn_ch == 0xD && gpx->option.dbg) gpx->ptu_out = sn_ch;// DFM-17 (P?)? test 0xD ...? + // PS-15 ? (sn2_ch & 0xF) == 0x0 : gpx->ptu_out = 0 + + 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'; + } + gpx->snc.SN_X = SN; + gpx->snc.chXbit = 0; + } + } + ret = (gpx->sonde_typ & 0xF); + } + } + + + 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 + } + if (gpx->ptu_out >= 0xC) { // DFM>=09(P) + if (conf_id >= 5 && conf_id <= 6) { + val = bits2val(conf_bits+4, 4*6); + gpx->meas24[conf_id] = fl24(val); + } + } + + // STM32-status: Bat, MCU-Temp + if (gpx->ptu_out >= 0xA) { // DFM>=09(P) (STM32) + ui8_t ofs = 0; + if (gpx->ptu_out >= 0xC) ofs = 2; + if (conf_id == 0x5+ofs) { // voltage + val = bits2val(conf_bits+8, 4*4); + gpx->status[0] = val/1000.0; + } + if (conf_id == 0x6+ofs) { // T-intern (STM32) + val = bits2val(conf_bits+8, 4*4); + gpx->status[1] = val/100.0; + } + } + + return ret; +} + +static void print_gpx(gpx_t *gpx) { + int i, j; + int contgps = 0; + int output = 0; + int jsonout = 0; + int start = 0; + + if (gpx->frnr > 0) start = 0x1000; + + output |= start; + + + for (i = 0; i < 9/*8*/; i++) { // trigger: pck8 + if ( !( (gpx->option.dst || gpx->option.jsn) && gpx->pck[i].ec < 0) ) + { + if (gpx->pck[8].ts - gpx->pck[i].ts < 6.0) { output |= (1<option.dst && gpx->pck[i].ec < 0) { output &= ~(1<option.dst && !contgps) { + output = 0; + } + if (gpx->option.jsn && !contgps) { + jsonout = 0; + } + + if (output & 0xF000) { + + if (gpx->option.raw == 2) { + for (i = 0; i < 9; i++) { + printf(" %s", gpx->dat_str[i]); + if (gpx->option.ecc) printf(" (%1X) ", gpx->pck[i].ec&0xF); + } + for (i = 0; i < 9; i++) { + for (j = 0; j < 13; j++) gpx->dat_str[i][j] = ' '; + } + } + else { + if (gpx->option.aut && gpx->option.vbs >= 2) printf("<%c> ", gpx->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 (gpx->option.vbs >= 2 && gpx->option.ecc) printf("(%1X,%1X,%1X) ", gpx->pck[0].ec&0xF, gpx->pck[8].ec&0xF, gpx->pck[1].ec&0xF); + printf(" "); + printf(" lat: %.5f ", gpx->lat); if (gpx->option.vbs >= 2 && gpx->option.ecc) printf("(%1X) ", gpx->pck[2].ec&0xF); + printf(" lon: %.5f ", gpx->lon); if (gpx->option.vbs >= 2 && gpx->option.ecc) printf("(%1X) ", gpx->pck[3].ec&0xF); + printf(" alt: %.1f ", gpx->alt); if (gpx->option.vbs >= 2 && gpx->option.ecc) printf("(%1X) ", gpx->pck[4].ec&0xF); + printf(" vH: %5.2f ", gpx->horiV); + printf(" D: %5.1f ", gpx->dir); + printf(" vV: %5.2f ", gpx->vertV); + if (gpx->option.ptu && gpx->ptu_out) { + float t = get_Temp(gpx); + if (t > -270.0) printf(" T=%.1fC ", t); + if (gpx->option.dbg) { + float t2 = get_Temp2(gpx); + float t4 = get_Temp4(gpx); + if (t2 > -270.0) printf(" T2=%.1fC ", t2); + if (t4 > -270.0) printf(" T4=%.1fC ", t4); + printf(" f0: %.2f ", gpx->meas24[0]); + printf(" f1: %.2f ", gpx->meas24[1]); + printf(" f2: %.2f ", gpx->meas24[2]); + printf(" f3: %.2f ", gpx->meas24[3]); + printf(" f4: %.2f ", gpx->meas24[4]); + if (gpx->ptu_out >= 0xC) { + printf(" f5: %.2f ", gpx->meas24[5]); + printf(" f6: %.2f ", gpx->meas24[6]); + } + + } + } + if (gpx->option.vbs == 3 && (gpx->ptu_out == 0xA || gpx->ptu_out >= 0xC)) { + printf(" U: %.2fV ", gpx->status[0]); + printf(" Ti: %.1fK ", gpx->status[1]); + } + if (gpx->option.vbs) + { + if (gpx->sonde_typ & SNbit) { + printf(" (%s) ", gpx->sonde_id); + gpx->sonde_typ ^= SNbit; + } + } + } + printf("\n"); + + if (gpx->option.jsn && jsonout) + { + // JSON Buffer to store sonde ID + char json_sonde_id[] = "DFMxx-xxxxxxxx\0\0"; + switch (gpx->sonde_typ & 0xF) { + case 0: sprintf(json_sonde_id, "DFMxx-xxxxxxxx"); break; //json_sonde_id[0] = '\0'; + case 6: sprintf(json_sonde_id, "DFM06-%6X", gpx->SN6); break; + case 0xA: sprintf(json_sonde_id, "DFM09-%6u", gpx->SN); break; + // 0x7: PS-15?, 0xC: DFM-17? (0xD: DFM-17?p) + default : sprintf(json_sonde_id, "DFMx%1X-%6u", gpx->sonde_typ & 0xF,gpx->SN); + } + + // 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 (gpx->ptu_out) { // get temperature + float t = get_Temp(gpx); // ecc-valid temperature? + if (t > -270.0) printf(", \"temp\": %.1f", t); + } + printf(" }\n"); + printf("\n"); + } + + } + + for (i = 0; i < 9; i++) gpx->pck[i].ec = -1; +} + +static int print_frame(gpx_t *gpx) { + int i; + int nib = 0; + int frid = -1; + int ret0, ret1, ret2; + int ret = 0; + + 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]; + + deinterleave(gpx->frame_bits+CONF, 7, hamming_conf); + deinterleave(gpx->frame_bits+DAT1, 13, hamming_dat1); + deinterleave(gpx->frame_bits+DAT2, 13, hamming_dat2); + + ret0 = hamming(gpx->option.ecc, hamming_conf, 7, block_conf); + ret1 = hamming(gpx->option.ecc, hamming_dat1, 13, block_dat1); + ret2 = hamming(gpx->option.ecc, hamming_dat2, 13, block_dat2); + ret = ret0 | ret1 | ret2; + + if (gpx->option.raw == 1) { + + for (i = 0; i < 7; i++) { + nib = bits2val(block_conf+S*i, S); + printf("%01X", nib & 0xFF); + } + if (gpx->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 (gpx->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 (gpx->option.ecc) { + if (ret2 == 0) printf(" [OK] "); + else if (ret2 > 0) printf(" [KO] "); + else printf(" [NO] "); + } + + if (gpx->option.ecc && gpx->option.vbs) { + if (gpx->option.vbs > 1) printf(" (%1X,%1X,%1X) ", cnt_biterr(ret0), cnt_biterr(ret1), cnt_biterr(ret2)); + printf(" (%d) ", cnt_biterr(ret0)+cnt_biterr(ret1)+cnt_biterr(ret2)); + } + + printf("\n"); + + } + else if (gpx->option.ecc) { + + if (ret0 == 0 || ret0 > 0) { + conf_out(gpx, block_conf, ret0); + } + if (ret1 == 0 || ret1 > 0) { + frid = dat_out(gpx, block_dat1, ret1); + if (frid == 8) print_gpx(gpx); + } + if (ret2 == 0 || ret2 > 0) { + frid = dat_out(gpx, block_dat2, ret2); + if (frid == 8) print_gpx(gpx); + } + + } + else { + + conf_out(gpx, block_conf, ret0); + frid = dat_out(gpx, block_dat1, ret1); + if (frid == 8) print_gpx(gpx); + frid = dat_out(gpx, block_dat2, ret2); + if (frid == 8) print_gpx(gpx); + + } + + return ret; +} + +/* -------------------------------------------------------------------------- */ + +// header bit buffer +typedef struct { + char *hdr; + char *buf; + char len; + int bufpos; + float ths; +} hdb_t; + +static float cmp_hdb(hdb_t *hdb) { // bit-errors? + int i, j; + int headlen = hdb->len; + int berrs1 = 0, berrs2 = 0; + + i = 0; + j = hdb->bufpos; + while (i < headlen) { + if (j < 0) j = headlen-1; + if (hdb->buf[j] != hdb->hdr[headlen-1-i]) berrs1 += 1; + j--; + i++; + } + + i = 0; + j = hdb->bufpos; + while (i < headlen) { + if (j < 0) j = headlen-1; + if ((hdb->buf[j]^0x01) != hdb->hdr[headlen-1-i]) berrs2 += 1; + j--; + i++; + } + if (berrs2 < berrs1) return (-headlen+berrs2)/(float)headlen; + else return ( headlen-berrs1)/(float)headlen; + + return 0; +} + +static int find_binhead(FILE *fp, hdb_t *hdb, float *score) { + int bit; + int headlen = hdb->len; + float mv; + + //*score = 0.0; + + while ( (bit = fgetc(fp)) != EOF ) + { + bit &= 1; + + hdb->bufpos = (hdb->bufpos+1) % headlen; + hdb->buf[hdb->bufpos] = 0x30 | bit; // Ascii + + mv = cmp_hdb(hdb); + if ( fabs(mv) > hdb->ths ) { + *score = mv; + return 1; + } + } + + return EOF; +} + + +int main(int argc, char **argv) { + + int option_verbose = 0; // ausfuehrliche Anzeige + int option_raw = 0; // rohe Frames + int option_inv = 0; // invertiert Signal + int option_ecc = 0; + int option_ptu = 0; + int option_dist = 0; // continuous pcks 0..8 + int option_auto = 0; + int option_iq = 0; + int option_bin = 0; + int option_json = 0; // JSON blob output (for auto_rx) + int wavloaded = 0; + int sel_wavch = 0; // audio channel: left + int spike = 0; + + FILE *fp = NULL; + char *fpname = NULL; + + int ret = 0; + int k; + + int bit; + int bitpos = 0; + int bitQ; + int pos; + int frm = 0, nfrms = 8; // nfrms=1,2,4,8 + + int headerlen = 0; + + int header_found = 0; + + float thres = 0.65; + float _mv = 0.0; + + int symlen = 2; + int bitofs = 2; // +1 .. +2 + int shift = 0; + + pcm_t pcm = {0}; + dsp_t dsp = {0}; + + gpx_t gpx = {0}; + + hdb_t hdb = {0}; + ui32_t hdrcnt = 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; + //gpx.ptu_out = 1; // force ptu (non PS-15) + } + else if ( (strcmp(*argv, "--spike") == 0) ) { + spike = 1; + } + else if ( (strcmp(*argv, "--auto") == 0) ) { option_auto = 1; } + else if (strcmp(*argv, "--bin") == 0) { option_bin = 1; } // bit/byte binary input + 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) ) { 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, "--dbg") == 0) { gpx.option.dbg = 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; + + + // init gpx + strcpy(gpx.frame_bits, dfm_header); //, sizeof(dfm_header); + for (k = 0; k < 9; k++) gpx.pck[k].ec = -1; // init ecc-status + + gpx.option.inv = option_inv; + gpx.option.vbs = option_verbose; + gpx.option.raw = option_raw; + gpx.option.ptu = option_ptu; + gpx.option.ecc = option_ecc; + gpx.option.aut = option_auto; + gpx.option.dst = option_dist; + gpx.option.jsn = option_json; + + + headerlen = strlen(dfm_rawheader); + + + if (!option_bin) { + //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; + } + + // dfm: BT=1?, h=2.4? + symlen = 2; + + // init dsp + // + dsp.fp = fp; + dsp.sr = pcm.sr; + dsp.bps = pcm.bps; + dsp.nch = pcm.nch; + dsp.ch = pcm.sel_ch; + dsp.br = (float)BAUD_RATE; + dsp.sps = (float)dsp.sr/dsp.br; + dsp.symlen = symlen; + dsp.symhd = symlen; + dsp._spb = dsp.sps*symlen; + dsp.hdr = dfm_rawheader; + dsp.hdrlen = strlen(dfm_rawheader); + dsp.BT = 0.5; // bw/time (ISI) // 0.3..0.5 + dsp.h = 1.8; // 2.4 modulation index abzgl. BT + dsp.opt_iq = option_iq; + + if ( dsp.sps < 8 ) { + fprintf(stderr, "note: sample rate low\n"); + } + } + else { + // init circular header bit buffer + hdb.hdr = dfm_rawheader; + hdb.len = strlen(dfm_rawheader); + hdb.ths = 1.0 - 2.1/(float)hdb.len; // 1.0-max_bit_errors/hdrlen // max 1.1 !! + hdb.bufpos = -1; + hdb.buf = calloc(hdb.len, sizeof(char)); + if (hdb.buf == NULL) { + fprintf(stderr, "error: malloc\n"); + return -1; + } + } + + + k = init_buffers(&dsp); + if ( k < 0 ) { + fprintf(stderr, "error: init buffers\n"); + return -1; + }; + + + bitofs += shift; + + + while ( 1 ) + { + if (option_bin) { // aka find_binrawhead() + header_found = find_binhead(fp, &hdb, &_mv); // symbols or bits? + hdrcnt += nfrms; + } + else { + header_found = find_header(&dsp, thres, 2, bitofs, 0); + _mv = dsp.mv; + } + if (header_found == EOF) break; + + // mv == correlation score + if (_mv *(0.5-gpx.option.inv) < 0) { + if (gpx.option.aut == 0) header_found = 0; + else gpx.option.inv ^= 0x1; + } + + 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 + if (option_bin) { + gpx._frmcnt = hdrcnt + frm; + } + else { + gpx._frmcnt = dsp.mv_pos/(2.0*dsp.sps*BITFRAME_LEN) + frm; + } + while ( pos < BITFRAME_LEN ) + { + if (option_bin) { + // symbols or bits? + // manchester1 1->10,0->01: 1.bit (DFM-06) + // manchester2 0->10,1->01: 2.bit (DFM-09) + bitQ = fgetc(fp); + if (bitQ != EOF) { + bit = bitQ & 0x1; + bitQ = fgetc(fp); // check: rbit0^rbit1=1 (Manchester) + if (bitQ != EOF) bit = bitQ & 0x1; // 2.bit (DFM-09) + } + } + else { + if (option_iq >= 2) { + float bl = -1; + if (option_iq > 2) bl = 4.0; + bitQ = read_slbit(&dsp, &bit, 0/*gpx.option.inv*/, bitofs, bitpos, bl, 0); + } + else { + bitQ = read_slbit(&dsp, &bit, 0/*gpx.option.inv*/, bitofs, bitpos, -1, spike); + } + } + if ( bitQ == EOF ) { frm = nfrms; break; } // liest 2x EOF + + if (gpx.option.inv) bit ^= 1; + + gpx.frame_bits[pos] = 0x30 + bit; + pos++; + bitpos += 1; + } + gpx.frame_bits[pos] = '\0'; + + ret = print_frame(&gpx); + if (pos < BITFRAME_LEN) break; + pos = 0; + frm += 1; + //if (ret < 0) frms += 1; + } + } + + header_found = 0; + pos = headerlen; + } + + if (!option_bin) free_buffers(&dsp); + else { + if (hdb.buf) { free(hdb.buf); hdb.buf = NULL; } + } + + + fclose(fp); + + return 0; +} + diff --git a/demod/mod/lms6mod.c b/demod/mod/lms6mod.c new file mode 100644 index 0000000..1201cf7 --- /dev/null +++ b/demod/mod/lms6mod.c @@ -0,0 +1,984 @@ + +/* + * LMS6 + * (403 MHz) + * + * sync header: correlation/matched filter + * files: lms6mod.c demod_mod.c demod_mod.h bch_ecc_mod.c bch_ecc_mod.h + * compile, either (a) or (b): + * (a) + * gcc -c demod_mod.c + * gcc -DINCLUDESTATIC lms6mod.c demod_mod.o -lm -o lms6mod + * (b) + * gcc -c demod_mod.c + * gcc -c bch_ecc_mod.c + * gcc lms6mod.c demod_mod.o bch_ecc_mod.o -lm -o lms6mod + * + * usage: + * ./lms6mod --vit --ecc + * ( --vit recommended) + * 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_mod.h" + +//#define INCLUDESTATIC 1 +#ifdef INCLUDESTATIC + #include "bch_ecc_mod.c" +#else + #include "bch_ecc_mod.h" +#endif + + +typedef struct { + i8_t vbs; // verbose output + i8_t raw; // raw frames + i8_t crc; // CRC check output + i8_t ecc; // Reed-Solomon ECC + i8_t sat; // GPS sat data + i8_t ptu; // PTU: temperature + i8_t inv; + i8_t vit; + i8_t jsn; // JSON output (auto_rx) +} option_t; + + +/* -------------------------------------------------------------------------- */ + +#define BAUD_RATE 4800 + +#define BITS 8 +#define HEADOFS 0 //16 +#define HEADLEN ((4*16)-HEADOFS) + +#define SYNC_LEN 5 +#define FRM_LEN (223) +#define PAR_LEN (32) +#define FRMBUF_LEN (3*FRM_LEN) +#define BLOCKSTART (SYNC_LEN*BITS*2) +#define BLOCK_LEN (FRM_LEN+PAR_LEN+SYNC_LEN) // 255+5 = 260 +#define RAWBITBLOCK_LEN ((BLOCK_LEN+1)*BITS*2) // (+1 tail) + +#define FRAME_LEN (300) // 4800baud, 16bits/byte +#define BITFRAME_LEN (FRAME_LEN*BITS) +#define RAWBITFRAME_LEN (BITFRAME_LEN*2) +#define OVERLAP 64 +#define OFS 4 + + +static char rawheader[] = "0101011000001000""0001110010010111""0001101010100111""0011110100111110"; // (c0,inv(c1)) +// (00) 58 f3 3f b8 +// char header[] = "0000001101011101""0100100111000010""0100111111110010""0110100001101011"; // (c0,c1) +static ui8_t rs_sync[] = { 0x00, 0x58, 0xf3, 0x3f, 0xb8}; +// 0x58f33fb8 little-endian <-> 0x1ACFFC1D big-endian bytes + +// (00) 58 f3 3f b8 +static char blk_syncbits[] = "0000000000000000""0000001101011101""0100100111000010""0100111111110010""0110100001101011"; + +static ui8_t frm_sync[] = { 0x24, 0x54, 0x00, 0x00}; + + +#define L 7 // d_f=10 +static char polyA[] = "1001111"; // 0x4f: x^6+x^3+x^2+x+1 +static char polyB[] = "1101101"; // 0x6d: x^6+x^5+x^3+x^2+1 +/* +// d_f=6 +qA[] = "1110011"; // 0x73: x^6+x^5+x^4+x+1 +qB[] = "0011110"; // 0x1e: x^4+x^3+x^2+x +pA[] = "10010101"; // 0x95: x^7+x^4+x^2+1 = (x+1)(x^6+x^5+x^4+x+1) = (x+1)qA +pB[] = "00100010"; // 0x22: x^5+x = (x+1)(x^4+x^3+x^2+x)=x(x+1)^3 = (x+1)qB +polyA = qA + x*qB +polyB = qA + qB +*/ + +#define N (1 << L) +#define M (1 << (L-1)) + +typedef struct { + ui8_t bIn; + ui8_t codeIn; + ui8_t prevState; // 0..M=64 + int w; // > 255 : if (w>250): w=250 ? + //float sw; +} states_t; + +typedef struct { + char rawbits[RAWBITFRAME_LEN+OVERLAP*BITS*2 +8]; + states_t state[RAWBITFRAME_LEN+OVERLAP +8][M]; + states_t d[N]; +} VIT_t; + +typedef struct { + int frnr; + int sn; + int week; int gpstow; + int jahr; int monat; int tag; + int wday; + int std; int min; float sek; + double lat; double lon; double alt; + double vH; double vD; double vV; + double vE; double vN; double vU; + char blk_rawbits[RAWBITBLOCK_LEN+SYNC_LEN*BITS*2 +8]; + ui8_t frame[FRM_LEN]; // = { 0x24, 0x54, 0x00, 0x00}; // dataheader + int frm_pos; // ecc_blk <-> frm_blk + int sf; + option_t option; + RS_t RS; + VIT_t *vit; +} gpx_t; + + +// ------------------------------------------------------------------------ + +static ui8_t vit_code[N]; +static vitCodes_init = 0; + +static int vit_initCodes(gpx_t *gpx) { + int cA, cB; + int i, bits; + + VIT_t *pv = calloc(1, sizeof(VIT_t)); + if (pv == NULL) return -1; + gpx->vit = pv; + + if ( vitCodes_init == 0 ) { + for (bits = 0; bits < N; bits++) { + cA = 0; + cB = 0; + for (i = 0; i < L; i++) { + cA ^= (polyA[L-1-i]&1) & ((bits >> i)&1); + cB ^= (polyB[L-1-i]&1) & ((bits >> i)&1); + } + vit_code[bits] = (cA<<1) | cB; + } + vitCodes_init = 1; + } + + return 0; +} + +static int vit_dist(int c, char *rc) { + return (((c>>1)^rc[0])&1) + ((c^rc[1])&1); +} + +static int vit_start(VIT_t *vit, char *rc) { + int t, m, j, c, d; + + t = L-1; + m = M; + while ( t > 0 ) { // t=0..L-2: nextStatestate[t][j].prevState = j/2; + } + t--; + m /= 2; + } + + m = 2; + for (t = 1; t < L; t++) { + for (j = 0; j < m; j++) { + c = vit_code[j]; + vit->state[t][j].bIn = j % 2; + vit->state[t][j].codeIn = c; + d = vit_dist( c, rc+2*(t-1) ); + vit->state[t][j].w = vit->state[t-1][vit->state[t][j].prevState].w + d; + } + m *= 2; + } + + return t; +} + +static int vit_next(VIT_t *vit, int t, char *rc) { + int b, nstate; + int j, index; + + for (j = 0; j < M; j++) { + for (b = 0; b < 2; b++) { + nstate = j*2 + b; + vit->d[nstate].bIn = b; + vit->d[nstate].codeIn = vit_code[nstate]; + vit->d[nstate].prevState = j; + vit->d[nstate].w = vit->state[t][j].w + vit_dist( vit->d[nstate].codeIn, rc ); + } + } + + for (j = 0; j < M; j++) { + + if ( vit->d[j].w <= vit->d[j+M].w ) index = j; else index = j+M; + + vit->state[t+1][j] = vit->d[index]; + } + + return 0; +} + +static int vit_path(VIT_t *vit, int j, int t) { + int c; + + vit->rawbits[2*t] = '\0'; + while (t > 0) { + c = vit->state[t][j].codeIn; + vit->rawbits[2*t -2] = 0x30 + ((c>>1) & 1); + vit->rawbits[2*t -1] = 0x30 + (c & 1); + j = vit->state[t][j].prevState; + t--; + } + + return 0; +} + +static int viterbi(VIT_t *vit, char *rc) { + int t, tmax; + int j, j_min, w_min; + + vit_start(vit, rc); + + tmax = strlen(rc)/2; + + for (t = L-1; t < tmax; t++) + { + vit_next(vit, t, rc+2*t); + } + + w_min = -1; + for (j = 0; j < M; j++) { + if (w_min < 0) { + w_min = vit->state[tmax][j].w; + j_min = j; + } + if (vit->state[tmax][j].w < w_min) { + w_min = vit->state[tmax][j].w; + j_min = j; + } + } + vit_path(vit, j_min, tmax); + + return 0; +} + +// ------------------------------------------------------------------------ + +static int deconv(char* rawbits, char *bits) { + + int j, n, bitA, bitB; + char *p; + int len; + int errors = 0; + int m = L-1; + + len = strlen(rawbits); + for (j = 0; j < m; j++) bits[j] = '0'; + n = 0; + while ( 2*(m+n) < len ) { + p = rawbits+2*(m+n); + bitA = bitB = 0; + for (j = 0; j < m; j++) { + bitA ^= (bits[n+j]&1) & (polyA[j]&1); + bitB ^= (bits[n+j]&1) & (polyB[j]&1); + } + if ( (bitA^(p[0]&1))==(polyA[m]&1) && (bitB^(p[1]&1))==(polyB[m]&1) ) bits[n+m] = '1'; + else if ( (bitA^(p[0]&1))==0 && (bitB^(p[1]&1))==0 ) bits[n+m] = '0'; + else { + if ( (bitA^(p[0]&1))!=(polyA[m]&1) && (bitB^(p[1]&1))==(polyB[m]&1) ) bits[n+m] = 0x39; + else bits[n+m] = 0x38; + errors = n; + break; + } + n += 1; + } + bits[n+m] = '\0'; + + return errors; +} + +// ------------------------------------------------------------------------ + +static int crc16_0(ui8_t frame[], int len) { + int crc16poly = 0x1021; + int rem = 0x0, i, j; + int byte; + + for (i = 0; i < len; i++) { + byte = frame[i]; + rem = rem ^ (byte << 8); + for (j = 0; j < 8; j++) { + if (rem & 0x8000) { + rem = (rem << 1) ^ crc16poly; + } + else { + rem = (rem << 1); + } + rem &= 0xFFFF; + } + } + return rem; +} + +static int check_CRC(ui8_t frame[]) { + ui32_t crclen = 0, + crcdat = 0; + + crclen = 221; + crcdat = (frame[crclen]<<8) | frame[crclen+1]; + if ( crcdat != crc16_0(frame, crclen) ) { + return 1; // CRC NO + } + else return 0; // CRC OK +} + +// ------------------------------------------------------------------------ + +static int bits2bytes(char *bitstr, ui8_t *bytes) { + int i, bit, d, byteval; + int len = strlen(bitstr)/8; + int bitpos, bytepos; + + bitpos = 0; + bytepos = 0; + + while (bytepos < len) { + + byteval = 0; + d = 1; + for (i = 0; i < BITS; i++) { + bit=*(bitstr+bitpos+i); /* little endian */ + //bit=*(bitstr+bitpos+7-i); /* big endian */ + if ((bit == '1') || (bit == '9')) byteval += d; + else /*if ((bit == '0') || (bit == '8'))*/ byteval += 0; + d <<= 1; + } + bitpos += BITS; + bytes[bytepos++] = byteval & 0xFF; + } + + //while (bytepos < FRAME_LEN+OVERLAP) bytes[bytepos++] = 0; + + return bytepos; +} + +/* -------------------------------------------------------------------------- */ + + +#define pos_SondeSN (OFS+0x00) // ?4 byte 00 7A.... +#define pos_FrameNb (OFS+0x04) // 2 byte +//GPS Position +#define pos_GPSTOW (OFS+0x06) // 4 byte +#define pos_GPSlat (OFS+0x0E) // 4 byte +#define pos_GPSlon (OFS+0x12) // 4 byte +#define pos_GPSalt (OFS+0x16) // 4 byte +//GPS Velocity East-North-Up (ENU) +#define pos_GPSvO (OFS+0x1A) // 3 byte +#define pos_GPSvN (OFS+0x1D) // 3 byte +#define pos_GPSvV (OFS+0x20) // 3 byte + + +static int get_SondeSN(gpx_t *gpx) { + unsigned byte; + + byte = (gpx->frame[pos_SondeSN]<<24) | (gpx->frame[pos_SondeSN+1]<<16) + | (gpx->frame[pos_SondeSN+2]<<8) | gpx->frame[pos_SondeSN+3]; + gpx->sn = byte & 0xFFFFFF; + + return 0; +} + +static int get_FrameNb(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t frnr_bytes[2]; + int frnr; + + for (i = 0; i < 2; i++) { + byte = gpx->frame[pos_FrameNb + i]; + frnr_bytes[i] = byte; + } + + frnr = (frnr_bytes[0] << 8) + frnr_bytes[1] ; + gpx->frnr = frnr; + + return 0; +} + + +//char weekday[7][3] = { "So", "Mo", "Di", "Mi", "Do", "Fr", "Sa"}; +static char weekday[7][4] = { "Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat"}; + +static int get_GPStime(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t gpstime_bytes[4]; + int gpstime = 0, // 32bit + day; + float ms; + + for (i = 0; i < 4; i++) { + byte = gpx->frame[pos_GPSTOW + i]; + gpstime_bytes[i] = byte; + } + gpstime = 0; + for (i = 0; i < 4; i++) { + gpstime |= gpstime_bytes[i] << (8*(3-i)); + } + + gpx->gpstow = gpstime; + + ms = gpstime % 1000; + gpstime /= 1000; + + day = gpstime / (24 * 3600); + gpstime %= (24*3600); + + if ((day < 0) || (day > 6)) return -1; + + gpx->wday = day; + gpx->std = gpstime / 3600; + gpx->min = (gpstime % 3600) / 60; + gpx->sek = gpstime % 60 + ms/1000.0; + + return 0; +} + +static double B60B60 = (1<<30)/90.0; // 2^32/360 = 2^30/90 = 0xB60B60.711x + +static int get_GPSlat(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t gpslat_bytes[4]; + int gpslat; + double lat; + + for (i = 0; i < 4; i++) { + byte = gpx->frame[pos_GPSlat + i]; + gpslat_bytes[i] = byte; + } + + gpslat = 0; + for (i = 0; i < 4; i++) { + gpslat |= gpslat_bytes[i] << (8*(3-i)); + } + lat = gpslat / B60B60; + gpx->lat = lat; + + return 0; +} + +static int get_GPSlon(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t gpslon_bytes[4]; + int gpslon; + double lon; + + for (i = 0; i < 4; i++) { + byte = gpx->frame[pos_GPSlon + i]; + gpslon_bytes[i] = byte; + } + + gpslon = 0; + for (i = 0; i < 4; i++) { + gpslon |= gpslon_bytes[i] << (8*(3-i)); + } + lon = gpslon / B60B60; + gpx->lon = lon; + + return 0; +} + +static int get_GPSalt(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t gpsheight_bytes[4]; + int gpsheight; + double height; + + for (i = 0; i < 4; i++) { + byte = gpx->frame[pos_GPSalt + i]; + gpsheight_bytes[i] = byte; + } + + gpsheight = 0; + for (i = 0; i < 4; i++) { + gpsheight |= gpsheight_bytes[i] << (8*(3-i)); + } + height = gpsheight / 1000.0; + gpx->alt = height; + + if (height < -200 || height > 60000) return -1; + return 0; +} + +static int get_GPSvel24(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t gpsVel_bytes[3]; + int vel24; + double vx, vy, vz, dir; //, alpha; + + for (i = 0; i < 3; i++) { + byte = gpx->frame[pos_GPSvO + i]; + gpsVel_bytes[i] = byte; + } + vel24 = gpsVel_bytes[0] << 16 | gpsVel_bytes[1] << 8 | gpsVel_bytes[2]; + if (vel24 > (0x7FFFFF)) vel24 -= 0x1000000; + vx = vel24 / 1e3; // ost + + for (i = 0; i < 3; i++) { + byte = gpx->frame[pos_GPSvN + i]; + gpsVel_bytes[i] = byte; + } + vel24 = gpsVel_bytes[0] << 16 | gpsVel_bytes[1] << 8 | gpsVel_bytes[2]; + if (vel24 > (0x7FFFFF)) vel24 -= 0x1000000; + vy= vel24 / 1e3; // nord + + for (i = 0; i < 3; i++) { + byte = gpx->frame[pos_GPSvV + i]; + gpsVel_bytes[i] = byte; + } + vel24 = gpsVel_bytes[0] << 16 | gpsVel_bytes[1] << 8 | gpsVel_bytes[2]; + if (vel24 > (0x7FFFFF)) vel24 -= 0x1000000; + vz = vel24 / 1e3; // hoch + + gpx->vE = vx; + gpx->vN = vy; + gpx->vU = vz; + + + gpx->vH = sqrt(vx*vx+vy*vy); +/* + alpha = atan2(vy, vx)*180/M_PI; // ComplexPlane (von x-Achse nach links) - GeoMeteo (von y-Achse nach rechts) + dir = 90-alpha; // z=x+iy= -> i*conj(z)=y+ix=re(i(pi/2-t)), Achsen und Drehsinn vertauscht + if (dir < 0) dir += 360; // atan2(y,x)=atan(y/x)=pi/2-atan(x/y) , atan(1/t) = pi/2 - atan(t) + gpx->vD2 = dir; +*/ + dir = atan2(vx, vy) * 180 / M_PI; + if (dir < 0) dir += 360; + gpx->vD = dir; + + gpx->vV = vz; + + return 0; +} + + +// RS(255,223)-CCSDS +#define rs_N 255 +#define rs_K 223 +#define rs_R (rs_N-rs_K) // 32 + +static int lms6_ecc(gpx_t *gpx, ui8_t *cw) { + int errors; + ui8_t err_pos[rs_R], + err_val[rs_R]; + + errors = rs_decode(&gpx->RS, cw, err_pos, err_val); + + return errors; +} + +static void print_frame(gpx_t *gpx, int crc_err, int len) { + int err=0; + + if (gpx->frame[0] != 0) + { + //if ((gpx->frame[pos_SondeSN+1] & 0xF0) == 0x70) // ? beginnen alle SNs mit 0x7A.... bzw 80..... ? + if ( gpx->frame[pos_SondeSN+1] ) + { + get_SondeSN(gpx); + get_FrameNb(gpx); + printf(" (%7d) ", gpx->sn); + printf(" [%5d] ", gpx->frnr); + err = get_GPStime(gpx); + if (!err) printf("%s ", weekday[gpx->wday]); + printf("%02d:%02d:%06.3f ", gpx->std, gpx->min, gpx->sek); // falls Rundung auf 60s: Ueberlauf + + get_GPSlat(gpx); + get_GPSlon(gpx); + err = get_GPSalt(gpx); + if (!err) { + printf(" lat: %.5f ", gpx->lat); + printf(" lon: %.5f ", gpx->lon); + printf(" alt: %.2fm ", gpx->alt); + get_GPSvel24(gpx); + //if (gpx->option.vbs == 2) printf(" (%.1f ,%.1f,%.1f) ", gpx->vE, gpx->vN, gpx->vU); + printf(" vH: %.1fm/s D: %.1f vV: %.1fm/s ", gpx->vH, gpx->vD, gpx->vV); + } + + if (crc_err==0) printf(" [OK]"); else printf(" [NO]"); + + printf("\n"); + + + if (gpx->option.jsn) { + // Print JSON output required by auto_rx. + if (crc_err==0) { // CRC-OK + // UTC oder GPS? + printf("{ \"frame\": %d, \"id\": \"%d\", \"time\": \"%02d:%02d:%06.3fZ\", \"lat\": %.5f, \"lon\": %.5f, \"alt\": %.5f, \"vel_h\": %.5f, \"heading\": %.5f, \"vel_v\": %.5f }\n", + gpx->frnr, gpx->sn, gpx->std, gpx->min, gpx->sek, gpx->lat, gpx->lon, gpx->alt, gpx->vH, gpx->vD, gpx->vV ); + printf("\n"); + } + } + + } + } +} + + +static void proc_frame(gpx_t *gpx, int len) { + int blk_pos = SYNC_LEN; + ui8_t block_bytes[BLOCK_LEN+8]; + ui8_t rs_cw[rs_N]; + char frame_bits[BITFRAME_LEN+OVERLAP*BITS +8]; // init L-1 bits mit 0 + char *rawbits = NULL; + int i, j; + int err = 0; + int errs = 0; + int crc_err = 0; + int flen, blen; + + + if ((len % 8) > 4) { + while (len % 8) gpx->blk_rawbits[len++] = '0'; + } + gpx->blk_rawbits[len] = '\0'; + + flen = len / (2*BITS); + + if (gpx->option.vit == 1) { + viterbi(gpx->vit, gpx->blk_rawbits); + rawbits = gpx->vit->rawbits; + } + else rawbits = gpx->blk_rawbits; + + err = deconv(rawbits, frame_bits); + + if (err) { for (i=err; i < RAWBITBLOCK_LEN/2; i++) frame_bits[i] = 0; } + + + blen = bits2bytes(frame_bits, block_bytes); + for (j = blen; j < BLOCK_LEN+8; j++) block_bytes[j] = 0; + + + if (gpx->option.ecc) { + for (j = 0; j < rs_N; j++) rs_cw[rs_N-1-j] = block_bytes[SYNC_LEN+j]; + errs = lms6_ecc(gpx, rs_cw); + for (j = 0; j < rs_N; j++) block_bytes[SYNC_LEN+j] = rs_cw[rs_N-1-j]; + } + + if (gpx->option.raw == 2) { + for (i = 0; i < flen; i++) printf("%02x ", block_bytes[i]); + if (gpx->option.ecc) printf("(%d)", errs); + printf("\n"); + } + else if (gpx->option.raw == 4 && gpx->option.ecc) { + for (i = 0; i < rs_N; i++) printf("%02x", block_bytes[SYNC_LEN+i]); + printf(" (%d)", errs); + printf("\n"); + } + else if (gpx->option.raw == 8) { + if (gpx->option.vit == 1) { + for (i = 0; i < len; i++) printf("%c", gpx->vit->rawbits[i]); printf("\n"); + } + else { + for (i = 0; i < len; i++) printf("%c", gpx->blk_rawbits[i]); printf("\n"); + } + } + + blk_pos = SYNC_LEN; + + while ( blk_pos-SYNC_LEN < FRM_LEN ) { + + if (gpx->sf == 0) { + while ( blk_pos-SYNC_LEN < FRM_LEN ) { + gpx->sf = 0; + for (j = 0; j < 4; j++) gpx->sf += (block_bytes[blk_pos+j] == frm_sync[j]); + if (gpx->sf == 4) { + gpx->frm_pos = 0; + break; + } + blk_pos++; + } + } + + if ( gpx->sf && gpx->frm_pos < FRM_LEN ) { + gpx->frame[gpx->frm_pos] = block_bytes[blk_pos]; + gpx->frm_pos++; + blk_pos++; + } + + if (gpx->frm_pos == FRM_LEN) { + + crc_err = check_CRC(gpx->frame); + + if (gpx->option.raw == 1) { + for (i = 0; i < FRM_LEN; i++) printf("%02x ", gpx->frame[i]); + if (crc_err==0) printf(" [OK]"); else printf(" [NO]"); + printf("\n"); + } + + if (gpx->option.raw == 0) print_frame(gpx, crc_err, len); + + gpx->frm_pos = 0; + gpx->sf = 0; + } + + } + +} + + +int main(int argc, char **argv) { + + int option_inv = 0; // invertiert Signal + int option_iq = 0; + int option_dc = 0; + int wavloaded = 0; + int sel_wavch = 0; // audio channel: left + + FILE *fp = NULL; + char *fpname = NULL; + + int k; + + int bit, rbit; + int bitpos = 0; + int bitQ; + int pos; + //int headerlen = 0; + + int header_found = 0; + + float thres = 0.76; + float _mv = 0.0; + + int symlen = 1; + int bitofs = 1; // +1 .. +2 + int shift = 0; + + unsigned int bc = 0; + + pcm_t pcm = {0}; + dsp_t dsp = {0}; //memset(&dsp, 0, sizeof(dsp)); +/* + // gpx_t _gpx = {0}; gpx_t *gpx = &_gpx; // stack size ... + gpx_t *gpx = NULL; + gpx = calloc(1, sizeof(gpx_t)); + //memset(gpx, 0, sizeof(gpx_t)); +*/ + gpx_t _gpx = {0}; gpx_t *gpx = &_gpx; + + +#ifdef CYGWIN + _setmode(fileno(stdin), _O_BINARY); // _setmode(_fileno(stdin), _O_BINARY); +#endif + setbuf(stdout, NULL); + + + fpname = argv[0]; + ++argv; + while ((*argv) && (!wavloaded)) { + if ( (strcmp(*argv, "-h") == 0) || (strcmp(*argv, "--help") == 0) ) { + fprintf(stderr, "%s [options] audio.wav\n", fpname); + fprintf(stderr, " options:\n"); + fprintf(stderr, " -v, --verbose\n"); + fprintf(stderr, " -r, --raw\n"); + fprintf(stderr, " --vit (Viterbi)\n"); + fprintf(stderr, " --ecc (Reed-Solomon)\n"); + return 0; + } + else if ( (strcmp(*argv, "-v") == 0) || (strcmp(*argv, "--verbose") == 0) ) { + gpx->option.vbs = 1; + } + else if ( (strcmp(*argv, "-r") == 0) || (strcmp(*argv, "--raw") == 0) ) { + gpx->option.raw = 1; // bytes - rs_ecc_codewords + } + else if ( (strcmp(*argv, "-r0") == 0) || (strcmp(*argv, "--raw0") == 0) ) { + gpx->option.raw = 2; // bytes: sync + codewords + } + else if ( (strcmp(*argv, "-rc") == 0) || (strcmp(*argv, "--rawecc") == 0) ) { + gpx->option.raw = 4; // rs_ecc_codewords + } + else if ( (strcmp(*argv, "-R") == 0) || (strcmp(*argv, "--RAW") == 0) ) { + gpx->option.raw = 8; // rawbits + } + else if (strcmp(*argv, "--ecc" ) == 0) { gpx->option.ecc = 1; } // RS-ECC + else if (strcmp(*argv, "--vit" ) == 0) { gpx->option.vit = 1; } // viterbi + else if ( (strcmp(*argv, "-i") == 0) || (strcmp(*argv, "--invert") == 0) ) { + option_inv = 1; // nicht noetig + } + else if ( (strcmp(*argv, "--dc") == 0) ) { + option_dc = 1; + } + else if ( (strcmp(*argv, "--ch2") == 0) ) { sel_wavch = 1; } // right channel (default: 0=left) + else if ( (strcmp(*argv, "--ths") == 0) ) { + ++argv; + if (*argv) { + thres = atof(*argv); + } + else return -1; + } + else if ( (strcmp(*argv, "-d") == 0) ) { + ++argv; + if (*argv) { + shift = atoi(*argv); + if (shift > 4) shift = 4; + if (shift < -4) shift = -4; + } + else return -1; + } + else if (strcmp(*argv, "--iq0") == 0) { option_iq = 1; } // differential/FM-demod + else if (strcmp(*argv, "--iq2") == 0) { option_iq = 2; } + else if (strcmp(*argv, "--iq3") == 0) { option_iq = 3; } // iq2==iq3 + else if (strcmp(*argv, "--json") == 0) { + gpx->option.jsn = 1; + gpx->option.ecc = 1; + gpx->option.vit = 1; + } + else { + fp = fopen(*argv, "rb"); + if (fp == NULL) { + fprintf(stderr, "%s konnte nicht geoeffnet werden\n", *argv); + return -1; + } + wavloaded = 1; + } + ++argv; + } + if (!wavloaded) fp = stdin; + + + if (gpx->option.raw == 4) gpx->option.ecc = 1; + + // init gpx + memcpy(gpx->blk_rawbits, blk_syncbits, sizeof(blk_syncbits)); + memcpy(gpx->frame, frm_sync, sizeof(frm_sync)); + gpx->frm_pos = 0; // ecc_blk <-> frm_blk + gpx->sf = 0; + + gpx->option.inv = option_inv; // irrelevant + + if (option_iq) sel_wavch = 0; + + pcm.sel_ch = sel_wavch; + k = read_wav_header(&pcm, fp); + if ( k < 0 ) { + fclose(fp); + fprintf(stderr, "error: wav header\n"); + return -1; + } + + symlen = 1; + + // init dsp + // + dsp.fp = fp; + dsp.sr = pcm.sr; + dsp.bps = pcm.bps; + dsp.nch = pcm.nch; + dsp.ch = pcm.sel_ch; + dsp.br = (float)BAUD_RATE; + dsp.sps = (float)dsp.sr/dsp.br; + dsp.symlen = symlen; + dsp.symhd = 1; + dsp._spb = dsp.sps*symlen; + dsp.hdr = rawheader; + dsp.hdrlen = strlen(rawheader); + dsp.BT = 1.5; // bw/time (ISI) // 1.0..2.0 + dsp.h = 0.9; // 1.0 modulation index + dsp.opt_iq = option_iq; + + if ( dsp.sps < 8 ) { + fprintf(stderr, "note: sample rate low (%.1f sps)\n", dsp.sps); + } + + //headerlen = dsp.hdrlen; + + k = init_buffers(&dsp); + if ( k < 0 ) { + fprintf(stderr, "error: init buffers\n"); + return -1; + }; + + + if (gpx->option.vit) { + k = vit_initCodes(gpx); + if (k < 0) return -1; + } + if (gpx->option.ecc) { + rs_init_RS255ccsds(&gpx->RS); // bch_ecc.c + } + + + bitofs += shift; + + + while ( 1 ) + { + + header_found = find_header(&dsp, thres, 3, bitofs, option_dc); + _mv = dsp.mv; + + if (header_found == EOF) break; + + // mv == correlation score + if (_mv*(0.5-gpx->option.inv) < 0) { + gpx->option.inv ^= 0x1; // LMS6: irrelevant + } + + if (header_found) { + + bitpos = 0; + pos = BLOCKSTART; + + if (_mv > 0) bc = 0; else bc = 1; + + while ( pos < RAWBITBLOCK_LEN ) { + + bitQ = read_slbit(&dsp, &rbit, 0, bitofs, bitpos, -1, 0); // symlen=1 + + if (bitQ == EOF) { break; } + + bit = rbit ^ (bc%2); // (c0,inv(c1)) + gpx->blk_rawbits[pos] = 0x30 + bit; + + bc++; + pos++; + bitpos += 1; + } + + gpx->blk_rawbits[pos] = '\0'; + + proc_frame(gpx, pos); + + if (pos < RAWBITBLOCK_LEN) break; + + pos = BLOCKSTART; + header_found = 0; + } + + } + + + free_buffers(&dsp); + if (gpx->vit) { free(gpx->vit); gpx->vit = NULL; } + + fclose(fp); + + return 0; +} + diff --git a/demod/mod/m10mod.c b/demod/mod/m10mod.c new file mode 100644 index 0000000..bc31ab9 --- /dev/null +++ b/demod/mod/m10mod.c @@ -0,0 +1,1100 @@ + +/* + * m10 + * sync header: correlation/matched filter + * files: m10mod.c demod_mod.h demod_mod.c + * compile: + * gcc -c demod_mod.c + * gcc m10mod.c demod_mod.o -lm -o m10mod + * + * author: zilog80 + */ + +#include +#include +#include +#include + +#ifdef CYGWIN + #include // cygwin: _setmode() + #include +#endif + + +#include "demod_mod.h" + + +typedef struct { + i8_t vbs; // verbose output + i8_t raw; // raw frames + i8_t crc; // CRC check output + i8_t ecc; // Reed-Solomon ECC + i8_t sat; // GPS sat data + i8_t ptu; // PTU: temperature + i8_t inv; + i8_t aut; + i8_t col; // colors + i8_t jsn; // JSON output (auto_rx) +} option_t; + + +/* + 9600 baud -> 9616 baud ? +*/ +#define BAUD_RATE 9615 // 9614..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 +static 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) + + +typedef struct { + int week; int tow_ms; 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 vV; + double vx; double vy; double vD2; + ui8_t numSV; + ui8_t utc_ofs; + char SN[12]; + ui8_t frame_bytes[FRAME_LEN+AUX_LEN+4]; + char frame_bits[BITFRAME_LEN+BITAUX_LEN+8]; + int auxlen; // 0 .. 0x76-0x64 + option_t option; +} gpx_t; + + +/* -------------------------------------------------------------------------- */ +/* + * Convert GPS Week and Seconds to Modified Julian Day. + * - Adapted from sci.astro FAQ. + * - Ignores UTC leap seconds. + */ +static 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; +} +/* -------------------------------------------------------------------------- */ + +static 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; +} + +/* -------------------------------------------------------------------------- */ + +/* +M10 w/ trimble GPS + +frame[0x0] = framelen +frame[0x1] = 0x9F (type M10) + +init/noGPS: frame[0x2]=0x23 +GPS: frame[0x2]=0x20 (GPS trimble pck 0x8F-20 sub-id) + +frame[0x02..0x21] = GPS trimble pck 0x8F-20 byte 0..31 (sub-id, vel, tow, lat, lon, alt, fix, NumSV, UTC-ofs, week) +frame[0x22..0x2D] = GPS trimble pck 0x8F-20 byte 32..55:2 (PRN 1..12 only) + +Trimble Copernicus II +GPS packet 0x8F-20 (p.138) +byte +0 sub-pck id (always 0x20) +2-3 velE (i16) 0.005m/s +4-5 velN (i16) 0.005m/s +6-7 velU (i16) 0.005m/s +8-11 TOW (ms) +12-15 lat (scale 2^32/360) (i32) -90..90 +16-19 lon (scale 2^32/360) (ui32) 0..360 <-> (i32) -180..180 +20-23 alt (i32) mm above ellipsoid) +24 bit0: vel-scale (0: 0.005m/s) +26 datum (1: WGS-84) +27 fix: bit0(0:valid fix, 1:invalid fix), bit2(0:3D, 1:2D) +28 numSVs +29 UTC offset = (GPS - UTC) sec +30-31 GPS week +32+2*n PRN_(n+1), bit0-5 + +frame[0x32..0x5C] sensors (rel.hum., temp.) +frame[0x5D..0x61] SN +frame[0x62] counter +frame[0x63..0x64] check (AUX len=0x76: frame[0x63..0x74], frame[0x75..0x76]) + + +6449/10sec-frame: +GPS trimble pck 0x47 (signal levels): numSats sat1 lev1 sat2 lev2 .. +frame[0x0] = framelen +frame[0x1] = 0x49 +frame[0x2] = numSats (max 12) +frame[0x3+2*n] = PRN_(n+1) +frame[0x4+2*n] = signal level (float32 -> i8-byte level) + +*/ + + +#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_GPSsats 0x1E // 1 byte +#define pos_GPSutc 0x1F // 1 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 +*/ + +static int get_GPSweek(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t gpsweek_bytes[2]; + int gpsweek; + + gpx->numSV = gpx->frame_bytes[pos_GPSsats]; + gpx->utc_ofs = gpx->frame_bytes[pos_GPSutc]; + + for (i = 0; i < 2; i++) { + byte = gpx->frame_bytes[pos_GPSweek + i]; + gpsweek_bytes[i] = byte; + } + + gpsweek = (gpsweek_bytes[0] << 8) + gpsweek_bytes[1]; + gpx->week = gpsweek; + + if (gpsweek < 0 || gpsweek > 3000) return -1; + + 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, day; + int ms; + + for (i = 0; i < 4; i++) { + byte = gpx->frame_bytes[pos_GPSTOW + i]; + gpstime_bytes[i] = byte; + } + + gpstime = 0; + for (i = 0; i < 4; i++) { + gpstime |= gpstime_bytes[i] << (8*(3-i)); + } + + gpx->tow_ms = gpstime; + ms = gpstime % 1000; + gpstime /= 1000; + gpx->gpssec = gpstime; + + day = gpstime / (24 * 3600); + if ((day < 0) || (day > 6)) return -1; + + 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 double B60B60 = (1<<30)/90.0; // 2^32/360 = 2^30/90 = 0xB60B60.711x + +static int get_GPSlat(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t gpslat_bytes[4]; + int gpslat; + double lat; + + for (i = 0; i < 4; i++) { + byte = gpx->frame_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; + gpx->lat = lat; + + return 0; +} + +static int get_GPSlon(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t gpslon_bytes[4]; + int gpslon; + double lon; + + for (i = 0; i < 4; i++) { + byte = gpx->frame_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; + gpx->lon = lon; + + return 0; +} + +static int get_GPSalt(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t gpsalt_bytes[4]; + int gpsalt; + double alt; + + for (i = 0; i < 4; i++) { + byte = gpx->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; + gpx->alt = alt; + + return 0; +} + +static int get_GPSvel(gpx_t *gpx) { + 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 = gpx->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 = gpx->frame_bytes[pos_GPSvN + i]; + gpsVel_bytes[i] = byte; + } + vel16 = gpsVel_bytes[0] << 8 | gpsVel_bytes[1]; + vy= vel16 / ms2kn100; // nord + + gpx->vx = vx; + gpx->vy = vy; + 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; + + for (i = 0; i < 2; i++) { + byte = gpx->frame_bytes[pos_GPSvU + i]; + gpsVel_bytes[i] = byte; + } + vel16 = gpsVel_bytes[0] << 8 | gpsVel_bytes[1]; + gpx->vV = vel16 / ms2kn100; + + return 0; +} + +static int get_SN(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t sn_bytes[5]; + + for (i = 0; i < 11; i++) gpx->SN[i] = ' '; gpx->SN[11] = '\0'; + + for (i = 0; i < 5; i++) { + byte = gpx->frame_bytes[pos_SN + i]; + sn_bytes[i] = byte; + } + + byte = sn_bytes[2]; + sprintf(gpx->SN, "%1X%02u", (byte>>4)&0xF, byte&0xF); + byte = sn_bytes[3] | (sn_bytes[4]<<8); + sprintf(gpx->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 +*/ + +static 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; +} + +static 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 +// +static float get_Temp(gpx_t *gpx, 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 = gpx->frame_bytes[0x3E]; // adr_0455h + ADC_RT = (gpx->frame_bytes[0x40] << 8) | gpx->frame_bytes[0x3F]; + ADC_RT -= 0xA000; + Tcal[0] = (gpx->frame_bytes[0x42] << 8) | gpx->frame_bytes[0x41]; + Tcal[1] = (gpx->frame_bytes[0x44] << 8) | gpx->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 (gpx->option.vbs >= 3 && csOK) { // on-chip temperature + ui16_t ADC_Ti_raw = (gpx->frame_bytes[0x49] << 8) | gpx->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) +*/ +static float get_Tntc2(gpx_t *gpx, 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 = (gpx->frame_bytes[0x5A] << 8) | gpx->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 + +static float get_count_RH(gpx_t *gpx) { // capture 1000 rising edges + ui32_t TBCCR1_1000 = gpx->frame_bytes[0x35] | (gpx->frame_bytes[0x36]<<8) | (gpx->frame_bytes[0x37]<<16); + return TBCCR1_1000 / ADR_108A; +} +static float get_TLC555freq(gpx_t *gpx) { + return FREQ_CAPCLK / get_count_RH(gpx); +} +/* +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 = gpx->frame_bytes[0x32] | (gpx->frame_bytes[0x33]<<8) | (gpx->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; +} +*/ +/* -------------------------------------------------------------------------- */ + +static int print_pos(gpx_t *gpx, int csOK) { + int err, err2; + + err = 0; + err |= get_GPSweek(gpx); + err |= get_GPStime(gpx); + err |= get_GPSlat(gpx); + err |= get_GPSlon(gpx); + err |= get_GPSalt(gpx); + err2 = get_GPSvel(gpx); + + if (!err) { + + Gps2Date(gpx->week, gpx->gpssec, &gpx->jahr, &gpx->monat, &gpx->tag); + + if (gpx->option.col) { + fprintf(stdout, col_TXT); + if (gpx->option.vbs >= 3) fprintf(stdout, " (W "col_GPSweek"%d"col_TXT") ", gpx->week); + fprintf(stdout, col_GPSTOW"%s"col_TXT" ", weekday[gpx->wday]); + fprintf(stdout, col_GPSdate"%04d-%02d-%02d"col_TXT" "col_GPSTOW"%02d:%02d:%06.3f"col_TXT" ", + gpx->jahr, gpx->monat, gpx->tag, gpx->std, gpx->min, gpx->sek); + fprintf(stdout, " lat: "col_GPSlat"%.5f"col_TXT" ", gpx->lat); + fprintf(stdout, " lon: "col_GPSlon"%.5f"col_TXT" ", gpx->lon); + fprintf(stdout, " alt: "col_GPSalt"%.2f"col_TXT" ", gpx->alt); + if (!err2) { + //if (gpx->option.vbs == 2) fprintf(stdout, " "col_GPSvel"(%.1f , %.1f : %.1f)"col_TXT" ", gpx->vx, gpx->vy, gpx->vD2); + fprintf(stdout, " vH: "col_GPSvel"%.1f"col_TXT" D: "col_GPSvel"%.1f"col_TXT" vV: "col_GPSvel"%.1f"col_TXT" ", gpx->vH, gpx->vD, gpx->vV); + } + if (gpx->option.vbs >= 2) { + get_SN(gpx); + fprintf(stdout, " SN: "col_SN"%s"col_TXT, gpx->SN); + } + if (gpx->option.vbs >= 2) { + fprintf(stdout, " # "); + if (csOK) fprintf(stdout, " "col_CSok"[OK]"col_TXT); + else fprintf(stdout, " "col_CSno"[NO]"col_TXT); + } + if (gpx->option.ptu) { + float t = get_Temp(gpx, csOK); + if (t > -270.0) fprintf(stdout, " T=%.1fC ", t); + if (gpx->option.vbs >= 3) { + float t2 = get_Tntc2(gpx, csOK); + float fq555 = get_TLC555freq(gpx); + if (t2 > -270.0) fprintf(stdout, " (T2:%.1fC) (%.3fkHz) ", t2, fq555/1e3); + } + } + fprintf(stdout, ANSI_COLOR_RESET""); + } + else { + if (gpx->option.vbs >= 3) fprintf(stdout, " (W %d) ", gpx->week); + 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); + fprintf(stdout, " lat: %.5f ", gpx->lat); + fprintf(stdout, " lon: %.5f ", gpx->lon); + fprintf(stdout, " alt: %.2f ", gpx->alt); + if (!err2) { + //if (gpx->option.vbs == 2) fprintf(stdout, " (%.1f , %.1f : %.1f) ", gpx->vx, gpx->vy, gpx->vD2); + fprintf(stdout, " vH: %.1f D: %.1f vV: %.1f ", gpx->vH, gpx->vD, gpx->vV); + } + if (gpx->option.vbs >= 2) { + get_SN(gpx); + fprintf(stdout, " SN: %s", gpx->SN); + } + if (gpx->option.vbs >= 2) { + fprintf(stdout, " # "); + if (csOK) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); + } + if (gpx->option.ptu) { + float t = get_Temp(gpx, csOK); + if (t > -270.0) fprintf(stdout, " T=%.1fC ", t); + if (gpx->option.vbs >= 3) { + float t2 = get_Tntc2(gpx, csOK); + float fq555 = get_TLC555freq(gpx); + if (t2 > -270.0) fprintf(stdout, " (T2:%.1fC) (%.3fkHz) ", t2, fq555/1e3); + } + } + } + fprintf(stdout, "\n"); + + + if (gpx->option.jsn) { + // Print out telemetry data as JSON + if (csOK) { + int j; + char sn_id[4+12] = "M10-"; + // UTC = GPS - UTC_OFS (ab 1.1.2017: UTC_OFS=18sec) + int utc_s = gpx->gpssec - gpx->utc_ofs; + int utc_week = gpx->week; + int utc_jahr; int utc_monat; int utc_tag; + int utc_std; int utc_min; float utc_sek; + if (utc_s < 0) { + utc_week -= 1; + utc_s += 604800; // 604800sec = 1week + } + Gps2Date(utc_week, utc_s, &utc_jahr, &utc_monat, &utc_tag); + utc_s %= (24*3600); // 86400sec = 1day + utc_std = utc_s/3600; + utc_min = (utc_s%3600)/60; + utc_sek = utc_s%60 + (gpx->tow_ms % 1000)/1000.0; + + strncpy(sn_id+4, gpx->SN, 12); + sn_id[15] = '\0'; + for (j = 0; sn_id[j]; j++) { if (sn_id[j] == ' ') sn_id[j] = '-'; } + + fprintf(stdout, "{ \"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", + sn_id, utc_jahr, utc_monat, utc_tag, utc_std, utc_min, utc_sek, gpx->lat, gpx->lon, gpx->alt, gpx->vH, gpx->vD, gpx->vV, gpx->numSV); + if (gpx->option.ptu) { + float t = get_Temp(gpx, 0); + if (t > -273.0) fprintf(stdout, ", \"temp\": %.1f", t); + } + fprintf(stdout, " }\n"); + fprintf(stdout, "\n"); + } + } + + } + + return err; +} + +static int print_frame(gpx_t *gpx, int pos) { + int i; + ui8_t byte; + int cs1, cs2; + int flen = stdFLEN; // stdFLEN=0x64, auxFLEN=0x76 + + bits2bytes(gpx->frame_bits, gpx->frame_bytes); + flen = gpx->frame_bytes[0]; + if (flen == stdFLEN) gpx->auxlen = 0; + else { + gpx->auxlen = flen - stdFLEN; + if (gpx->auxlen < 0 || gpx->auxlen > AUX_LEN) gpx->auxlen = 0; + } + + cs1 = (gpx->frame_bytes[pos_Check+gpx->auxlen] << 8) | gpx->frame_bytes[pos_Check+gpx->auxlen+1]; + cs2 = checkM10(gpx->frame_bytes, pos_Check+gpx->auxlen); + + if (gpx->option.raw) { + + if (gpx->option.col && gpx->frame_bytes[1] != 0x49) { + fprintf(stdout, col_FRTXT); + for (i = 0; i < FRAME_LEN+gpx->auxlen; i++) { + byte = gpx->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+gpx->auxlen) && (i < pos_Check+gpx->auxlen+2)) fprintf(stdout, col_Check); + fprintf(stdout, "%02x", byte); + fprintf(stdout, col_FRTXT); + } + if (gpx->option.vbs) { + 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+gpx->auxlen; i++) { + byte = gpx->frame_bytes[i]; + fprintf(stdout, "%02x", byte); + } + if (gpx->option.vbs) { + fprintf(stdout, " # %04x", cs2); + if (cs1 == cs2) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); + } + fprintf(stdout, "\n"); + } + + } + else if (gpx->frame_bytes[1] == 0x49) { + if (gpx->option.vbs == 3) { + for (i = 0; i < FRAME_LEN+gpx->auxlen; i++) { + byte = gpx->frame_bytes[i]; + fprintf(stdout, "%02x", byte); + } + fprintf(stdout, "\n"); + } + } + else print_pos(gpx, cs1 == cs2); + + return (gpx->frame_bytes[0]<<8)|gpx->frame_bytes[1]; +} + + +int main(int argc, char **argv) { + + int option_verbose = 0; // ausfuehrliche Anzeige + int option_raw = 0; // rohe Frames + int option_inv = 0; // invertiert Signal + //int option_res = 0; // genauere Bitmessung + int option_color = 0; + int option_ptu = 0; + int option_dc = 0; + int option_iq = 0; + int wavloaded = 0; + int sel_wavch = 0; // audio channel: left + int spike = 0; + + FILE *fp = NULL; + char *fpname = NULL; + + int k; + + int bit, bit0; + int bitpos = 0; + int bitQ; + int pos; + + //int headerlen = 0; + + int header_found = 0; + + float thres = 0.76; + float _mv = 0.0; + + int symlen = 2; + int bitofs = 0; // 0 .. +2 + int shift = 0; + + pcm_t pcm = {0}; + dsp_t dsp = {0}; //memset(&dsp, 0, sizeof(dsp)); + + gpx_t gpx = {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) ) { sel_wavch = 1; } // right channel (default: 0=left) + else if ( (strcmp(*argv, "--ths") == 0) ) { + ++argv; + if (*argv) { + thres = atof(*argv); + } + else return -1; + } + else if ( (strcmp(*argv, "-d") == 0) ) { + ++argv; + if (*argv) { + shift = atoi(*argv); + if (shift > 4) shift = 4; + if (shift < -4) shift = -4; + } + else return -1; + } + else if (strcmp(*argv, "--iq0") == 0) { option_iq = 1; } // differential/FM-demod + else if (strcmp(*argv, "--iq2") == 0) { option_iq = 2; } + else if (strcmp(*argv, "--iq3") == 0) { option_iq = 3; } // iq2==iq3 + else if (strcmp(*argv, "--json") == 0) { gpx.option.jsn = 1; } + 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; + + + gpx.option.inv = option_inv; // irrelevant + gpx.option.vbs = option_verbose; + gpx.option.raw = option_raw; + gpx.option.ptu = option_ptu; + gpx.option.col = option_color; + + + // init gpx + pcm.sel_ch = sel_wavch; + k = read_wav_header(&pcm, fp); + if ( k < 0 ) { + fclose(fp); + fprintf(stderr, "error: wav header\n"); + return -1; + } + + // m10: BT>1?, h=1.2 ? + symlen = 2; + + // init dsp + // + dsp.fp = fp; + dsp.sr = pcm.sr; + dsp.bps = pcm.bps; + dsp.nch = pcm.nch; + dsp.ch = pcm.sel_ch; + dsp.br = (float)BAUD_RATE; + dsp.sps = (float)dsp.sr/dsp.br; + dsp.symlen = symlen; + dsp.symhd = 1; // M10!header + dsp._spb = dsp.sps*symlen; + dsp.hdr = rawheader; + dsp.hdrlen = strlen(rawheader); + dsp.BT = 1.8; // bw/time (ISI) // 1.0..2.0 + dsp.h = 0.9; // 1.2 modulation index + dsp.opt_iq = option_iq; + + if ( dsp.sps < 8 ) { + fprintf(stderr, "note: sample rate low (%.1f sps)\n", dsp.sps); + } + + //headerlen = dsp.hdrlen; + + k = init_buffers(&dsp); + if ( k < 0 ) { + fprintf(stderr, "error: init buffers\n"); + return -1; + }; + + + bitofs += shift; + + while ( 1 ) + { + + header_found = find_header(&dsp, thres, 2, bitofs, option_dc); + _mv = dsp.mv; + + if (header_found == EOF) break; + + // mv == correlation score + if (_mv*(0.5-gpx.option.inv) < 0) { + gpx.option.inv ^= 0x1; // M10: irrelevant + } + + + if (header_found) { + + bitpos = 0; + pos = 0; + pos /= 2; + bit0 = '0'; // oder: _mv[j] > 0 + + while ( pos < BITFRAME_LEN+BITAUX_LEN ) { + + if (option_iq >= 2) { + float bl = -1; + if (option_iq > 2) bl = 4.0; + bitQ = read_slbit(&dsp, &bit, 0/*gpx.option.inv*/, bitofs, bitpos, bl, 0); + } + else { + bitQ = read_slbit(&dsp, &bit, 0/*gpx.option.inv*/, bitofs, bitpos, -1, spike); // symlen=2 + } + + if ( bitQ == EOF ) { break; } + + gpx.frame_bits[pos] = 0x31 ^ (bit0 ^ bit); + pos++; + bit0 = bit; + bitpos += 1; + } + gpx.frame_bits[pos] = '\0'; + print_frame(&gpx, pos); + if (pos < BITFRAME_LEN) break; + + header_found = 0; + + // bis Ende der Sekunde vorspulen; allerdings Doppel-Frame alle 10 sek + if (gpx.option.vbs < 3) { // && (regulare frame) // print_frame-return? + while ( bitpos < 5*BITFRAME_LEN ) { + bitQ = read_slbit(&dsp, &bit, 0/*gpx.option.inv*/, bitofs, bitpos, -1, spike); // symlen=2 + if ( bitQ == EOF) break; + bitpos++; + } + } + + pos = 0; + } + } + + + free_buffers(&dsp); + + fclose(fp); + + return 0; +} + diff --git a/demod/mod/nav_gps_vel.c b/demod/mod/nav_gps_vel.c new file mode 100644 index 0000000..b432874 --- /dev/null +++ b/demod/mod/nav_gps_vel.c @@ -0,0 +1,1892 @@ + +/* + Quellen: + - IS-GPS-200H (bis C: ICD-GPS-200): + http://www.gps.gov/technical/icwg/ + - Borre: http://kom.aau.dk/~borre + - Essential GNSS Project (hier und da etwas unklar) +*/ + + +#define PI (3.1415926535897932384626433832795) + +#define RELATIVISTIC_CLOCK_CORRECTION (-4.442807633e-10) // combined constant defined in IS-GPS-200 [s]/[sqrt(m)] +#define GRAVITY_CONSTANT (3.986005e14) // gravity constant defined on IS-GPS-200 [m^3/s^2] +#define EARTH_ROTATION_RATE (7.2921151467e-05) // IS-GPS-200 [rad/s] +#define SECONDS_IN_WEEK (604800.0) // [s] +#define LIGHTSPEED (299792458.0) // light speed constant defined in IS-GPS-200 [m/s] + +#define RANGE_ESTIMATE (0.072) // approx. GPS-Sat range 0.072s*299792458m/s=21585057m +#define RANGERATE_ESTIMATE (0.000) // + +#define EARTH_a (6378137.0) +#define EARTH_b (6356752.31424518) +#define EARTH_a2_b2 (EARTH_a*EARTH_a - EARTH_b*EARTH_b) + +/* ---------------------------------------------------------------------------------------------------- */ + + +void ecef2elli(double X, double Y, double Z, double *lat, double *lon, double *alt) { + double ea2 = EARTH_a2_b2 / (EARTH_a*EARTH_a), + eb2 = EARTH_a2_b2 / (EARTH_b*EARTH_b); + double phi, lam, R, p, t; + double sint, cost; + + lam = atan2( Y , X ); + + p = sqrt( X*X + Y*Y ); + t = atan2( Z*EARTH_a , p*EARTH_b ); + + sint = sin(t); + cost = cos(t); + + phi = atan2( Z + eb2 * EARTH_b * sint*sint*sint , + p - ea2 * EARTH_a * cost*cost*cost ); + + R = EARTH_a / sqrt( 1 - ea2*sin(phi)*sin(phi) ); + *alt = p / cos(phi) - R; + + *lat = phi*180.0/PI; + *lon = lam*180.0/PI; +} + + +double dist(double X1, double Y1, double Z1, double X2, double Y2, double Z2) { + return sqrt( (X2-X1)*(X2-X1) + (Y2-Y1)*(Y2-Y1) + (Z2-Z1)*(Z2-Z1) ); +} + + +void rotZ(double x1, double y1, double z1, double angle, double *x2, double *y2, double *z2) { + double cosa = cos(angle); + double sina = sin(angle); + *x2 = cosa * x1 + sina * y1; + *y2 = -sina * x1 + cosa * y1; + *z2 = z1; +} + + +/* ---------------------------------------------------------------------------------------------------- */ + + +typedef struct { + ui16_t prn; + ui16_t week; + ui32_t toa; + char epoch[20]; + double toe; + double toc; + double e; + double delta_n; + double delta_i; + double i0; + double OmegaDot; + double sqrta; + double Omega0; + double w; + double M0; + double tgd; + double idot; + double cuc; + double cus; + double crc; + double crs; + double cic; + double cis; + double af0; + double af1; + double af2; + int gpsweek; + ui16_t svn; + ui8_t ura; + ui8_t health; + ui8_t conf; +} EPHEM_t; + +typedef struct { + ui32_t t; + double pseudorange; + double pseudorate; + double clock_corr; + double clock_drift; + double X; + double Y; + double Z; + double vX; + double vY; + double vZ; + int ephhr; + double PR; + double ephtime; + int prn; +} SAT_t; + +typedef struct {double X; double Y; double Z;} LOC_t; + +typedef struct {double X; double Y; double Z; + double vX; double vY; double vZ;} VEL_t; + + +/* ---------------------------------------------------------------------------------------------------- */ + + +int read_SEMalmanac(FILE *fp, EPHEM_t *alm) { + int l, j; + char buf[64]; + unsigned n, week, toa, ui; + double dbl; + + l = fscanf(fp, "%u", &n); if (l != 1) return -1; + l = fscanf(fp, "%s", buf); if (l != 1) return -1; + l = fscanf(fp, "%u", &week); if (l != 1) return -1; + l = fscanf(fp, "%u", &toa); if (l != 1) return -1; + + for (j = 1; j <= n; j++) { + //memset(&ephem, 0, sizeof(ephem)); + + alm[j].week = (ui16_t)week; + alm[j].toa = (ui32_t)toa; + alm[j].toe = (double)toa; + alm[j].toc = alm[j].toe; + + l = fscanf(fp, "%u", &ui); if (l != 1) return -1; alm[j].prn = (ui16_t)ui; + l = fscanf(fp, "%u", &ui); if (l != 1) return -2; alm[j].svn = (ui16_t)ui; + l = fscanf(fp, "%u", &ui); if (l != 1) return -3; alm[j].ura = (ui8_t)ui; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -4; alm[j].e = dbl; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -5; alm[j].delta_i = dbl; + alm[j].i0 = (0.30 + alm[j].delta_i) * PI; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -6; alm[j].OmegaDot = dbl * PI; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -7; alm[j].sqrta = dbl; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -6; alm[j].Omega0 = dbl * PI; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -8; alm[j].w = dbl * PI; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -9; alm[j].M0 = dbl * PI; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -10; alm[j].af0 = dbl; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -11; alm[j].af1 = dbl; + alm[j].af2 = 0; + alm[j].crc = 0; + alm[j].crs = 0; + alm[j].cuc = 0; + alm[j].cus = 0; + alm[j].cic = 0; + alm[j].cis = 0; + alm[j].tgd = 0; + alm[j].idot = 0; + alm[j].delta_n = 0; + l = fscanf(fp, "%u", &ui); if (l != 1) return -12; alm[j].health = (ui8_t)ui; + l = fscanf(fp, "%u", &ui); if (l != 1) return -13; alm[j].conf = (ui8_t)ui; + } + + return 0; +} + +int read_RNXephemeris(FILE *fp, EPHEM_t eph[][24]) { + int l, i; + char buf[64], str[20]; + char buf_header[83]; + //buf_data[80]; // 3 + 4*19 = 79 + char *pbuf; + unsigned ui; + double dbl; + int c; + EPHEM_t ephem = {}; + int hr = 0; + + do { + //l = fread(buf_header, 81, 1, fp); // Zeilen in Header sind nicht immer mit Leerzeichen aufgefuellt + pbuf = fgets(buf_header, 82, fp); // max 82-1 Zeichen + '\0' + buf_header[82] = '\0'; // doppelt haelt besser + //l = strlen(buf_header); + } while ( pbuf && !strstr(buf_header, "END OF HEADER") ); + + //l = fread(buf_data, 80, 1, fp); + //buf_data[79] = '\0'; + + + while (hr < 24) { // brdc/hour-rinex sollte nur Daten von einem Tag enthalten + + //memset(&ephem, 0, sizeof(ephem)); + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; sscanf(buf, "%d", &ui); + ephem.prn = ui; + + + for (i = 0; i < 16; i++) ephem.epoch[i] = '0'; + ephem.epoch[16] = '\0'; + + l = fread(buf, 19, 1, fp); if (l != 1) break; buf[19] = 0; + + for (i = 0; i < 6; i++) { + c = buf[3*i ]; if (c == ' ') c = '0'; str[2*i ] = c; + c = buf[3*i+1]; if (c == ' ') c = '0'; str[2*i+1] = c; + } + str[12] = buf[17]; + str[13] = buf[18]; + str[14] = '\0'; + + strncpy(ephem.epoch , "20", 2); // vorausgesetzt 21.Jhd; Datum steht auch im Header + strncpy(ephem.epoch+2, str, 15); + ephem.epoch[16] = '\0'; + + strncpy(str, buf+9, 2); str[2] = '\0'; + hr = atoi(str); + + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af0 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af1 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af2 = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iode = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.crs = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.delta_n = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.M0 = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cuc = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.e = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cus = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.sqrta = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.toe = dbl; + ephem.toc = ephem.toe; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cic = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.Omega0 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cis = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.i0 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.crc = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.w = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.OmegaDot = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.idot = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.codeL2 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.gpsweek = (int)dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iodc = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.sva = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.health = (ui8_t)(dbl+0.1); + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.tgd = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iodc = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.ttom = dbl; + pbuf = fgets(buf_header, 82, fp); + /* // die letzten beiden Felder (spare) sind manchmal leer (statt 0.00); manchmal fehlt sogar das drittletzte Feld + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.fit = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.spare1 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.spare2 = dbl; + if ((c=fgetc(fp)) == EOF) break; */ + + + ephem.week = 1; // ephem.gpsweek + eph[ephem.prn][hr] = ephem; + + if (pbuf == NULL) break; + } + + return 0; +} + +EPHEM_t *read_RNXpephs(FILE *fp) { + int l, i, n; + char buffer[86]; + char buf[64], str[20]; + char *pbuf; + unsigned ui; + double dbl; + int c; + EPHEM_t ephem = {}, *te = NULL; + int count = 0; + long fpos; + + do { // header-Zeilen: 80 Zeichen + pbuf = fgets(buffer, 84, fp); // max 82-1 Zeichen + buffer[82] = '\0'; + } while ( pbuf && !strstr(buffer, "END OF HEADER") ); + + if (pbuf == NULL) return NULL; + fpos = ftell(fp); + + count = 0; + while (count >= 0) { // data-Zeilen: 79 Zeichen + pbuf = fgets(buffer, 84, fp); if (pbuf == 0) break; + strncpy(str, buffer, 3); + str[3] = '\0'; + sscanf(str, "%d", &ui); + if (ui < 33) count++; + for (i = 1; i < 8; i++) { + pbuf = fgets(buffer, 84, fp); if (pbuf == 0) break; + } + } + //printf("Ephemerides: %d\n", count); + + fseek(fp, fpos, SEEK_SET); + + te = calloc( count+1, sizeof(ephem) ); // calloc( 1, sizeof(ephem) ); + if (te == NULL) return NULL; + + n = 0; + + while (count > 0) { // brdc/hour-rinex sollte nur Daten von einem Tag enthalten + + //memset(&ephem, 0, sizeof(ephem)); + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; sscanf(buf, "%d", &ui); + ephem.prn = ui; + + for (i = 0; i < 16; i++) ephem.epoch[i] = '0'; + ephem.epoch[16] = '\0'; + + l = fread(buf, 19, 1, fp); if (l != 1) break; buf[19] = 0; + + for (i = 0; i < 6; i++) { + c = buf[3*i ]; if (c == ' ') c = '0'; str[2*i ] = c; + c = buf[3*i+1]; if (c == ' ') c = '0'; str[2*i+1] = c; + } + str[12] = buf[17]; + str[13] = buf[18]; + str[14] = '\0'; + + strncpy(ephem.epoch , "20", 2); // vorausgesetzt 21.Jhd; Datum steht auch im Header + strncpy(ephem.epoch+2, str, 15); + ephem.epoch[16] = '\0'; + + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af0 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af1 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af2 = dbl; + while ((c=fgetc(fp)) != '\n') { if (c == EOF) break; } + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iode = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.crs = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.delta_n = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.M0 = dbl; + while ((c=fgetc(fp)) != '\n') { if (c == EOF) break; } + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cuc = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.e = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cus = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.sqrta = dbl; + while ((c=fgetc(fp)) != '\n') { if (c == EOF) break; } + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.toe = dbl; + ephem.toc = ephem.toe; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cic = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.Omega0 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cis = dbl; + while ((c=fgetc(fp)) != '\n') { if (c == EOF) break; } + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.i0 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.crc = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.w = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.OmegaDot = dbl; + while ((c=fgetc(fp)) != '\n') { if (c == EOF) break; } + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.idot = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.codeL2 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.gpsweek = (int)dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iodc = dbl; + while ((c=fgetc(fp)) != '\n') { if (c == EOF) break; } + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.sva = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.health = (ui8_t)(dbl+0.1); + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.tgd = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iodc = dbl; + while ((c=fgetc(fp)) != '\n') { if (c == EOF) break; } + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.ttom = dbl; + pbuf = fgets(buffer, 84, fp); + /* // die letzten beiden Felder (spare) sind manchmal leer (statt 0.00); manchmal fehlt sogar das drittletzte Feld + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.fit = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.spare1 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.spare2 = dbl; + while ((c=fgetc(fp)) != '\n') { if (c == EOF) break; } */ + + ephem.week = 1; // ephem.gpsweek + + te[n] = ephem; + n += 1; + + //tmp = realloc( te, (count+1) * sizeof(ephem) ); + //if (tmp == NULL) break; + //te = tmp; + + if (pbuf == NULL) break; + } + + te[n].prn = 0; + + + return te; +} + + +/* ---------------------------------------------------------------------------------------------------- */ +// +// Satellite Position +// + +void GPS_SatelliteClockCorrection( + const unsigned short transmission_gpsweek, // GPS week when signal was transmit (0-1024+) [weeks] + const double transmission_gpstow, // GPS time of week when signal was transmit [s] + const unsigned short ephem_week, // ephemeris: GPS week (0-1024+) [weeks] + const double toe, // ephemeris: time of week [s] + const double toc, // ephemeris: clock reference time of week [s] + const double af0, // ephemeris: polynomial clock correction coefficient [s], + const double af1, // ephemeris: polynomial clock correction coefficient [s/s], + const double af2, // ephemeris: polynomial clock correction coefficient [s/s^2] + const double ecc, // ephemeris: eccentricity of satellite orbit [] + const double sqrta, // ephemeris: square root of the semi-major axis of orbit [m^(1/2)] + const double delta_n, // ephemeris: mean motion difference from computed value [rad] + const double m0, // ephemeris: mean anomaly at reference time [rad] + const double tgd, // ephemeris: group delay differential between L1 and L2 [s] + double* clock_correction ) // ephemeris: satellite clock correction [m] +{ + int j; + + double tot; // time of transmission (including gps week) [s] + double tk; // time from ephemeris reference epoch [s] + double tc; // time from clock reference epoch [s] + double d_tr; // relativistic correction term [s] + double d_tsv; // SV PRN code phase time offset [s] + double a; // semi-major axis of orbit [m] + double n; // corrected mean motion [rad/s] + double M; // mean anomaly, [rad] + double E; // eccentric anomaly [rad] + + // compute the times from the reference epochs + tot = transmission_gpsweek*SECONDS_IN_WEEK + transmission_gpstow; + tk = tot - (ephem_week*SECONDS_IN_WEEK + toe); + tc = tot - (ephem_week*SECONDS_IN_WEEK + toc); + + // compute the corrected mean motion term + a = sqrta*sqrta; + n = sqrt( GRAVITY_CONSTANT / (a*a*a) ); // mean motion + n += delta_n; // corrected mean motion + + // Kepler-Gleichung M = E - e sin(E) + M = m0 + n*tk; // mean anomaly + E = M; // f(E) = M + e sin(E) hat Fixpunkt fuer e < 1, + for( j = 0; j < 7; j++ ) { // da |f'(E)|=|e cos(E)|<1. + E = M + ecc * sin(E); // waehle Startwert E_1 = M, E_{k+1} = f(E_k) + } // konvergiert langsam gegen E_0 = f(E_0) + + // relativistic correction + d_tr = RELATIVISTIC_CLOCK_CORRECTION * ecc * sqrta * sin(E); // [s] + d_tr *= LIGHTSPEED; + + // clock correction + d_tsv = af0 + af1*tc + af2*tc*tc; // [s] + + // L1 only + d_tsv -= tgd; // [s] + + // clock correction + *clock_correction = d_tsv*LIGHTSPEED + d_tr; // [m] + +} + +void GPS_ComputeSatellitePosition( + const unsigned short transmission_gpsweek, // GPS week when signal was transmit (0-1024+) [weeks] + const double transmission_gpstow, // GPS time of week when signal was transmit [s] + const unsigned short ephem_week, // ephemeris: GPS week (0-1024+) [weeks] + const double toe, // ephemeris: time of week [s] + const double m0, // ephemeris: mean anomaly at reference time [rad] + const double delta_n, // ephemeris: mean motion difference from computed value [rad] + const double ecc, // ephemeris: eccentricity [] + const double sqrta, // ephemeris: square root of the semi-major axis [m^(1/2)] + const double omega0, // ephemeris: longitude of ascending node of orbit plane at weekly epoch [rad] + const double i0, // ephemeris: inclination angle at reference time [rad] + const double w, // ephemeris: argument of perigee [rad] + const double omegadot, // ephemeris: rate of right ascension [rad/s] + const double idot, // ephemeris: rate of inclination angle [rad/s] + const double cuc, // ephemeris: cos harmonic correction term to the argument of latitude [rad] + const double cus, // ephemeris: sin harmonic correction term to the argument of latitude [rad] + const double crc, // ephemeris: cos harmonic correction term to the orbit radius [m] + const double crs, // ephemeris: sin harmonic correction term to the orbit radius [m] + const double cic, // ephemeris: cos harmonic correction term to the angle of inclination [rad] + const double cis, // ephemeris: sin harmonic correction term to the angle of inclination [rad] + double* x, // satellite x [m] + double* y, // satellite y [m] + double* z ) // satellite z [m] +{ + int j; + + double tot; // time of transmission (including gps week) [s] + double tk; // time from ephemeris reference epoch [s] + double a; // semi-major axis of orbit [m] + double n; // corrected mean motion [rad/s] + double M; // mean anomaly, [rad] + double E; // eccentric anomaly [rad] + double v; // true anomaly [rad] + double u; // argument of latitude, corrected [rad] + double r; // radius in the orbital plane [m] + double i; // orbital inclination [rad] + double cos2u; // cos(2*u) [] + double sin2u; // sin(2*u) [] + double d_u; // argument of latitude correction [rad] + double d_r; // radius correction [m] + double d_i; // inclination correction [rad] + double x_op; // x position in the orbital plane [m] + double y_op; // y position in the orbital plane [m] + double omegak; // corrected longitude of the ascending node [rad] + double cos_omegak; // cos(omegak) + double sin_omegak; // sin(omegak) + double cosu; // cos(u) + double sinu; // sin(u) + double cosi; // cos(i) + double sini; // sin(i) + double cosE; // cos(E) + double sinE; // sin(E) + + + // compute the times from the reference epochs + tot = transmission_gpsweek*SECONDS_IN_WEEK + transmission_gpstow; + tk = tot - (ephem_week*SECONDS_IN_WEEK + toe); + + // compute the corrected mean motion term + a = sqrta*sqrta; + n = sqrt( GRAVITY_CONSTANT / (a*a*a) ); // computed mean motion + n += delta_n; // corrected mean motion + + // Kepler-Gleichung M = E - e sin(E) + M = m0 + n*tk; // mean anomaly + E = M; // f(E) = M + e sin(E) hat Fixpunkt fuer e < 1, + for( j = 0; j < 7; j++ ) { // da |f'(E)|=|e cos(E)|<1. + E = M + ecc * sin(E); // waehle Startwert E_1 = M, E_{k+1} = f(E_k) + } // konvergiert langsam gegen E_0 = f(E_0) + + cosE = cos(E); + sinE = sin(E); + + // true anomaly + v = atan2( (sqrt(1.0 - ecc*ecc)*sinE), (cosE - ecc) ); + // argument of latitude + u = v + w; + // radius in orbital plane + r = a * (1.0 - ecc * cos(E)); + // orbital inclination + i = i0; + + // second harmonic perturbations + // + cos2u = cos(2.0*u); + sin2u = sin(2.0*u); + // argument of latitude correction + d_u = cuc * cos2u + cus * sin2u; + // radius correction + d_r = crc * cos2u + crs * sin2u; + // correction to inclination + d_i = cic * cos2u + cis * sin2u; + + // corrected argument of latitude + u += d_u; + // corrected radius + r += d_r; + // corrected inclination + i += d_i + idot * tk; + + // positions in orbital plane + cosu = cos(u); + sinu = sin(u); + x_op = r * cosu; + y_op = r * sinu; + + + omegak = omega0 + omegadot*tk - EARTH_ROTATION_RATE*(tk + toe); + // fuer Bestimmung der Satellitenposition in ECEF, range=0; + // fuer GPS-Loesung (Sats in ECI) Erdrotation hinzuziehen: + // rotZ(EARTH_ROTATION_RATE*0.072), 0.072s*299792458m/s=21585057m + + // compute the WGS84 ECEF coordinates, + // vector r with components x & y is now rotated using, R3(-omegak)*R1(-i) + cos_omegak = cos(omegak); + sin_omegak = sin(omegak); + cosi = cos(i); + sini = sin(i); + + *x = x_op * cos_omegak - y_op * sin_omegak * cosi; + *y = x_op * sin_omegak + y_op * cos_omegak * cosi; + *z = y_op * sini; + +} + +void GPS_SatellitePosition_Ephem( + const unsigned short gpsweek, // gps week of signal transmission (0-1024+) [week] + const double gpstow, // time of week of signal transmission (gpstow-psr/c) [s] + EPHEM_t ephem, + double* clock_correction, // clock correction for this satellite for this epoch [m] + double* satX, // satellite X position WGS84 ECEF [m] + double* satY, // satellite Y position WGS84 ECEF [m] + double* satZ // satellite Z position WGS84 ECEF [m] + ) +{ + double tow; // user time of week adjusted with the clock corrections [s] + double x; // sat X position [m] + double y; // sat Y position [m] + double z; // sat Z position [m] + unsigned short week; // user week adjusted with the clock correction if needed [week] + + + x = y = z = 0.0; + + GPS_SatelliteClockCorrection( gpsweek, gpstow, + ephem.week, ephem.toe, ephem.toc, ephem.af0, + ephem.af1, ephem.af2, ephem.e, ephem.sqrta, + ephem.delta_n, ephem.M0, ephem.tgd, clock_correction ); + + + // adjust for week rollover + week = gpsweek; + tow = gpstow + (*clock_correction)/LIGHTSPEED; + if ( tow < 0.0 ) { + tow += SECONDS_IN_WEEK; + week--; + } + if ( tow > SECONDS_IN_WEEK ) { + tow -= SECONDS_IN_WEEK; + week++; + } + + //range = 0.072s*299792458m/s=21585057m + GPS_ComputeSatellitePosition( week, tow, + ephem.week, ephem.toe, ephem.M0, ephem.delta_n, ephem.e, ephem.sqrta, + ephem.Omega0, ephem.i0, ephem.w, ephem.OmegaDot, ephem.idot, + ephem.cuc, ephem.cus, ephem.crc, ephem.crs, ephem.cic, ephem.cis, + &x, &y, &z ); + + *satX = x; + *satY = y; + *satZ = z; + +} + +/* ---------------------------------------------------------------------------------------------------- */ + + +int NAV_ClosedFormSolution_FromPseudorange( + SAT_t sats[4], // input: satellite position and pseudorange + double* latitude, // output: ellipsoid latitude [rad] + double* longitude, // ellipsoid longitude [rad] + double* height, // ellipsoid height [m] + double* rx_clock_bias, // receiver clock bias [m] + double pos_ecef[3] ) // ECEF coordinates [m] +{ + + double p1 = sats[0].pseudorange + sats[0].clock_corr; // pseudorange measurement + clock correction [m] + double p2 = sats[1].pseudorange + sats[1].clock_corr; + double p3 = sats[2].pseudorange + sats[2].clock_corr; + double p4 = sats[3].pseudorange + sats[3].clock_corr; + + double x1, y1, z1; // Sat1 ECEF + double x2, y2, z2; // Sat2 ECEF + double x3, y3, z3; // Sat3 ECEF + double x4, y4, z4; // Sat4 ECEF + + // Erdrotation ECEF-ECI, 0.070s*299792458m/s=20985472m, 0.072s*299792458m/s=21585057m + rotZ(sats[0].X, sats[0].Y, sats[0].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE, &x1, &y1, &z1); + rotZ(sats[1].X, sats[1].Y, sats[1].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE, &x2, &y2, &z2); + rotZ(sats[2].X, sats[2].Y, sats[2].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE, &x3, &y3, &z3); + rotZ(sats[3].X, sats[3].Y, sats[3].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE, &x4, &y4, &z4); + + + double x12, x13, x14; // 'xij' = 'xi' - 'xj' [m] + double y12, y13, y14; + double z12, z13, z14; + double p21, p31, p41; // 'pij' = 'pi' - 'pj' [m] + + double k1, k2, k3; // coefficients + double c1, c2, c3; + double f1, f2, f3; + double m1, m2, m3; + + double d1; // clock bias, solution 1 [m] + double d2; // clock bias, solution 2 [m] + double detA; // determinant of A + double tmp1; + double tmp2; + double tmp3; + + double A[3][3]; + double D[3][3]; // D is A^-1 * detA + + typedef struct { + double x; + double y; + double z; + } struct_SOLN; + + struct_SOLN s1; + struct_SOLN s2; + + struct_SOLN stmp; + double dtmp; + double x0, y0, z0; + double latS, lonS, altS, + lat1, lon1, alt1, + lat2, lon2, alt2; + double d2_1, d2_2; + + + x12 = x1 - x2; + x13 = x1 - x3; + x14 = x1 - x4; + + y12 = y1 - y2; + y13 = y1 - y3; + y14 = y1 - y4; + + z12 = z1 - z2; + z13 = z1 - z3; + z14 = z1 - z4; + + p21 = p2 - p1; + p31 = p3 - p1; + p41 = p4 - p1; + + k1 = x12*x12 + y12*y12 + z12*z12 - p21*p21; + k2 = x13*x13 + y13*y13 + z13*z13 - p31*p31; + k3 = x14*x14 + y14*y14 + z14*z14 - p41*p41; + + A[0][0] = 2.0*x12; + A[1][0] = 2.0*x13; + A[2][0] = 2.0*x14; + + A[0][1] = 2.0*y12; + A[1][1] = 2.0*y13; + A[2][1] = 2.0*y14; + + A[0][2] = 2.0*z12; + A[1][2] = 2.0*z13; + A[2][2] = 2.0*z14; + + tmp1 = A[1][1]*A[2][2] - A[2][1]*A[1][2]; + tmp2 = A[1][0]*A[2][2] - A[2][0]*A[1][2]; + tmp3 = A[1][0]*A[2][1] - A[2][0]*A[1][1]; + + detA = A[0][0]*tmp1 - A[0][1]*tmp2 + A[0][2]*tmp3; + + D[0][0] = tmp1; + D[1][0] = -tmp2; + D[2][0] = tmp3; + + D[0][1] = -A[0][1]*A[2][2] + A[2][1]*A[0][2]; + D[1][1] = A[0][0]*A[2][2] - A[2][0]*A[0][2]; + D[2][1] = -A[0][0]*A[2][1] + A[2][0]*A[0][1]; + + D[0][2] = A[0][1]*A[1][2] - A[1][1]*A[0][2]; + D[1][2] = -A[0][0]*A[1][2] + A[1][0]*A[0][2]; + D[2][2] = A[0][0]*A[1][1] - A[1][0]*A[0][1]; + + c1 = (D[0][0]*p21 + D[0][1]*p31 + D[0][2]*p41) * 2.0 / detA; + c2 = (D[1][0]*p21 + D[1][1]*p31 + D[1][2]*p41) * 2.0 / detA; + c3 = (D[2][0]*p21 + D[2][1]*p31 + D[2][2]*p41) * 2.0 / detA; + + f1 = (D[0][0]*k1 + D[0][1]*k2 + D[0][2]*k3) / detA; + f2 = (D[1][0]*k1 + D[1][1]*k2 + D[1][2]*k3) / detA; + f3 = (D[2][0]*k1 + D[2][1]*k2 + D[2][2]*k3) / detA; + + m1 = c1*c1 + c2*c2 + c3*c3 - 1.0; + m2 = -2.0*( c1*f1 + c2*f2 + c3*f3 ); + m3 = f1*f1 + f2*f2 + f3*f3; + + tmp1 = m2*m2 - 4.0*m1*m3; + if ( tmp1 < 0 ) { // not good, there is no solution + return -1; //FALSE + } + + d1 = ( -m2 - sqrt( tmp1 ) ) * 0.5 / m1; // +-Reihenfolge vertauscht + d2 = ( -m2 + sqrt( tmp1 ) ) * 0.5 / m1; + + // two solutions + s1.x = c1*d1 - f1 + x1; + s1.y = c2*d1 - f2 + y1; + s1.z = c3*d1 - f3 + z1; + + s2.x = c1*d2 - f1 + x1; + s2.y = c2*d2 - f2 + y1; + s2.z = c3*d2 - f3 + z1; + + tmp1 = sqrt( s1.x*s1.x + s1.y*s1.y + s1.z*s1.z ); + tmp2 = sqrt( s2.x*s2.x + s2.y*s2.y + s2.z*s2.z ); + + // choose the correct solution based + // on whichever solution is closest to + // the Earth's surface + tmp1 = fabs( tmp1 - 6371000.0 ); + tmp2 = fabs( tmp2 - 6371000.0 ); + + // nur (tmp2 < tmp1) geht manchmal schief + if ( tmp2 < tmp1 && tmp1 >= 60000 ) { // swap solutions + stmp = s1; s1 = s2; s2 = stmp; // s1 = s2; + dtmp = d1; d1 = d2; d2 = dtmp; // d1 = d2; + } + else if ( tmp2 < 60000 ) { // interessant wenn tmp1 50000.0 ) { + return -2; + } + + return 0; +} + + +/* ---------------------------------------------------------------------------------------------------- */ + + +int trace_invert(double mat[4][4], double trinv[4]) // trace-invert +/* selected elements from 4x4 matrix inversion */ +{ + // Find all NECESSARY 2x2 subdeterminants + double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]; + double Det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]; + //double Det2_12_03 = mat[1][0]*mat[2][3] - mat[1][3]*mat[2][0]; + double Det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]; + //double Det2_12_13 = mat[1][1]*mat[2][3] - mat[1][3]*mat[2][1]; + //double Det2_12_23 = mat[1][2]*mat[2][3] - mat[1][3]*mat[2][2]; + double Det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0]; + //double Det2_13_02 = mat[1][0]*mat[3][2] - mat[1][2]*mat[3][0]; + double Det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0]; + //double Det2_13_12 = mat[1][1]*mat[3][2] - mat[1][2]*mat[3][1]; + double Det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1]; + //double Det2_13_23 = mat[1][2]*mat[3][3] - mat[1][3]*mat[3][2]; + double Det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0]; + double Det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0]; + double Det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0]; + double Det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1]; + double Det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1]; + double Det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2]; + + // Find all NECESSARY 3x3 subdeterminants + double Det3_012_012 = mat[0][0] * Det2_12_12 - mat[0][1] * Det2_12_02 + mat[0][2] * Det2_12_01; + //double Det3_012_013 = mat[0][0]*Det2_12_13 - mat[0][1]*Det2_12_03 + mat[0][3]*Det2_12_01; + //double Det3_012_023 = mat[0][0]*Det2_12_23 - mat[0][2]*Det2_12_03 + mat[0][3]*Det2_12_02; + //double Det3_012_123 = mat[0][1]*Det2_12_23 - mat[0][2]*Det2_12_13 + mat[0][3]*Det2_12_12; + //double Det3_013_012 = mat[0][0]*Det2_13_12 - mat[0][1]*Det2_13_02 + mat[0][2]*Det2_13_01; + double Det3_013_013 = mat[0][0] * Det2_13_13 - mat[0][1] * Det2_13_03 + mat[0][3] * Det2_13_01; + //double Det3_013_023 = mat[0][0]*Det2_13_23 - mat[0][2]*Det2_13_03 + mat[0][3]*Det2_13_02; + //double Det3_013_123 = mat[0][1]*Det2_13_23 - mat[0][2]*Det2_13_13 + mat[0][3]*Det2_13_12; + //double Det3_023_012 = mat[0][0]*Det2_23_12 - mat[0][1]*Det2_23_02 + mat[0][2]*Det2_23_01; + //double Det3_023_013 = mat[0][0]*Det2_23_13 - mat[0][1]*Det2_23_03 + mat[0][3]*Det2_23_01; + double Det3_023_023 = mat[0][0] * Det2_23_23 - mat[0][2] * Det2_23_03 + mat[0][3] * Det2_23_02; + //double Det3_023_123 = mat[0][1]*Det2_23_23 - mat[0][2]*Det2_23_13 + mat[0][3]*Det2_23_12; + double Det3_123_012 = mat[1][0] * Det2_23_12 - mat[1][1] * Det2_23_02 + mat[1][2] * Det2_23_01; + double Det3_123_013 = mat[1][0] * Det2_23_13 - mat[1][1] * Det2_23_03 + mat[1][3] * Det2_23_01; + double Det3_123_023 = mat[1][0] * Det2_23_23 - mat[1][2] * Det2_23_03 + mat[1][3] * Det2_23_02; + double Det3_123_123 = mat[1][1] * Det2_23_23 - mat[1][2] * Det2_23_13 + mat[1][3] * Det2_23_12; + + // Find the 4x4 determinant + static double det; + det = mat[0][0] * Det3_123_123 + - mat[0][1] * Det3_123_023 + + mat[0][2] * Det3_123_013 + - mat[0][3] * Det3_123_012; + + // Very small determinants probably reflect floating-point fuzz near zero + if (fabs(det) < 0.0001) + return -1; + + //inv[0][0] = Det3_123_123 / det; + //inv[0][1] = -Det3_023_123 / det; + //inv[0][2] = Det3_013_123 / det; + //inv[0][3] = -Det3_012_123 / det; + + //inv[1][0] = -Det3_123_023 / det; + //inv[1][1] = Det3_023_023 / det; + //inv[1][2] = -Det3_013_023 / det; + //inv[1][3] = Det3_012_023 / det; + + //inv[2][0] = Det3_123_013 / det; + //inv[2][1] = -Det3_023_013 / det; + //inv[2][2] = Det3_013_013 / det; + //inv[2][3] = -Det3_012_013 / det; + + //inv[3][0] = -Det3_123_012 / det; + //inv[3][1] = Det3_023_012 / det; + //inv[3][2] = -Det3_013_012 / det; + //inv[3][3] = Det3_012_012 / det; + + trinv[0] = Det3_123_123 / det; + + trinv[1] = Det3_023_023 / det; + trinv[2] = Det3_013_013 / det; + trinv[3] = Det3_012_012 / det; + + return 0; +} + +int calc_DOPn(int n, SAT_t satss[], double pos_ecef[3], double DOP[4]) { + int i, j, k; + double norm[n], satpos[n][3]; + double SATS[n][4], AtA[4][4]; + + for (i = 0; i < n; i++) { + satpos[i][0] = satss[i].X-pos_ecef[0]; + satpos[i][1] = satss[i].Y-pos_ecef[1]; + satpos[i][2] = satss[i].Z-pos_ecef[2]; + } + + + for (i = 0; i < n; i++) { + norm[i] = sqrt( satpos[i][0]*satpos[i][0] + satpos[i][1]*satpos[i][1] + satpos[i][2]*satpos[i][2] ); + for (j = 0; j < 3; j++) { + SATS[i][j] = satpos[i][j] / norm[i]; + } + SATS[i][3] = 1; + } + + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + AtA[i][j] = 0.0; + for (k = 0; k < n; k++) { + AtA[i][j] += SATS[k][i]*SATS[k][j]; + } + } + } + + return trace_invert(AtA, DOP); +} + +/* ---------------------------------------------------------------------------------------------------- */ + +int rotE(SAT_t sat, double *x, double *y, double *z) { + // Erdrotation ECEF-ECI, 0.070s*299792458m/s=20985472m, 0.072s*299792458m/s=21585057m + double range = sat.PR/LIGHTSPEED; + if (range < 19e6 || range > 30e6) range = 21e6; + rotZ(sat.X, sat.Y, sat.Z, EARTH_ROTATION_RATE*(range/LIGHTSPEED), x, y, z); + return 0; +} + + +double lorentz(double a[4], double b[4]) { + return a[0]*b[0] + a[1]*b[1] +a[2]*b[2] - a[3]*b[3]; +} + +int matrix_invert(double mat[4][4], double inv[4][4]) +{ + // 2x2 subdeterminants + double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]; + double Det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]; + double Det2_12_03 = mat[1][0] * mat[2][3] - mat[1][3] * mat[2][0]; + double Det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]; + double Det2_12_13 = mat[1][1] * mat[2][3] - mat[1][3] * mat[2][1]; + double Det2_12_23 = mat[1][2] * mat[2][3] - mat[1][3] * mat[2][2]; + double Det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0]; + double Det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0]; + double Det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0]; + double Det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1]; + double Det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1]; + double Det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2]; + double Det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0]; + double Det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0]; + double Det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0]; + double Det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1]; + double Det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1]; + double Det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2]; + + // 3x3 subdeterminants + double Det3_012_012 = mat[0][0] * Det2_12_12 - mat[0][1] * Det2_12_02 + mat[0][2] * Det2_12_01; + double Det3_012_013 = mat[0][0] * Det2_12_13 - mat[0][1] * Det2_12_03 + mat[0][3] * Det2_12_01; + double Det3_012_023 = mat[0][0] * Det2_12_23 - mat[0][2] * Det2_12_03 + mat[0][3] * Det2_12_02; + double Det3_012_123 = mat[0][1] * Det2_12_23 - mat[0][2] * Det2_12_13 + mat[0][3] * Det2_12_12; + double Det3_013_012 = mat[0][0] * Det2_13_12 - mat[0][1] * Det2_13_02 + mat[0][2] * Det2_13_01; + double Det3_013_013 = mat[0][0] * Det2_13_13 - mat[0][1] * Det2_13_03 + mat[0][3] * Det2_13_01; + double Det3_013_023 = mat[0][0] * Det2_13_23 - mat[0][2] * Det2_13_03 + mat[0][3] * Det2_13_02; + double Det3_013_123 = mat[0][1] * Det2_13_23 - mat[0][2] * Det2_13_13 + mat[0][3] * Det2_13_12; + double Det3_023_012 = mat[0][0] * Det2_23_12 - mat[0][1] * Det2_23_02 + mat[0][2] * Det2_23_01; + double Det3_023_013 = mat[0][0] * Det2_23_13 - mat[0][1] * Det2_23_03 + mat[0][3] * Det2_23_01; + double Det3_023_023 = mat[0][0] * Det2_23_23 - mat[0][2] * Det2_23_03 + mat[0][3] * Det2_23_02; + double Det3_023_123 = mat[0][1] * Det2_23_23 - mat[0][2] * Det2_23_13 + mat[0][3] * Det2_23_12; + double Det3_123_012 = mat[1][0] * Det2_23_12 - mat[1][1] * Det2_23_02 + mat[1][2] * Det2_23_01; + double Det3_123_013 = mat[1][0] * Det2_23_13 - mat[1][1] * Det2_23_03 + mat[1][3] * Det2_23_01; + double Det3_123_023 = mat[1][0] * Det2_23_23 - mat[1][2] * Det2_23_03 + mat[1][3] * Det2_23_02; + double Det3_123_123 = mat[1][1] * Det2_23_23 - mat[1][2] * Det2_23_13 + mat[1][3] * Det2_23_12; + + // 4x4 determinant + static double det; + det = mat[0][0] * Det3_123_123 + - mat[0][1] * Det3_123_023 + + mat[0][2] * Det3_123_013 + - mat[0][3] * Det3_123_012; + + // Very small determinants probably reflect floating-point fuzz near zero + if (fabs(det) < 0.0001) + return -1; + + inv[0][0] = Det3_123_123 / det; + inv[0][1] = -Det3_023_123 / det; + inv[0][2] = Det3_013_123 / det; + inv[0][3] = -Det3_012_123 / det; + + inv[1][0] = -Det3_123_023 / det; + inv[1][1] = Det3_023_023 / det; + inv[1][2] = -Det3_013_023 / det; + inv[1][3] = Det3_012_023 / det; + + inv[2][0] = Det3_123_013 / det; + inv[2][1] = -Det3_023_013 / det; + inv[2][2] = Det3_013_013 / det; + inv[2][3] = -Det3_012_013 / det; + + inv[3][0] = -Det3_123_012 / det; + inv[3][1] = Det3_023_012 / det; + inv[3][2] = -Det3_013_012 / det; + inv[3][3] = Det3_012_012 / det; + + return 0; +} + +int NAV_bancroft1(int N, SAT_t sats[], double pos_ecef[3], double *cc) { + + int i, j, k; + double B[N][4], BtB[4][4], BBinv[4][4], BBB[4][N]; + double a[N], Be[4], Ba[4]; + + double q0, q1, q2, p, q, sq, x1, x2; + double Lsg1[4], Lsg2[4]; + + double tmp1, tmp2; + double X, Y, Z; + + + if (N < 4 || N > 12) return -1; + + for (i = 0; i < N; i++) { + rotZ(sats[i].X, sats[i].Y, sats[i].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE, B[i], B[i]+1, B[i]+2); + B[i][3] = sats[i].pseudorange + sats[i].clock_corr; + } + + if (N == 4) { + matrix_invert(B, BBB); + } + else { + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + BtB[i][j] = 0.0; + for (k = 0; k < N; k++) { + BtB[i][j] += B[k][i]*B[k][j]; + } + } + } + matrix_invert(BtB, BBinv); + for (i = 0; i < 4; i++) { + for (j = 0; j < N; j++) { + BBB[i][j] = 0.0; + for (k = 0; k < 4; k++) { + BBB[i][j] += BBinv[i][k]*B[j][k]; + } + } + } + } + + for (i = 0; i < 4; i++) { + Be[i] = 0.0; + for (k = 0; k < N; k++) { + Be[i] += BBB[i][k]*1.0; + } + } + + for (i = 0; i < N; i++) { + a[i] = 0.5 * lorentz(B[i], B[i]); + } + + for (i = 0; i < 4; i++) { + Ba[i] = 0.0; + for (k = 0; k < N; k++) { + Ba[i] += BBB[i][k]*a[k]; + } + } + + q2 = lorentz(Be, Be); + q1 = lorentz(Ba, Be)-1; + q0 = lorentz(Ba, Ba); + + if (q2 == 0) return -2; + + p = q1/q2; + q = q0/q2; + + sq = p*p - q; + if (sq < 0) return -2; + + x1 = -p + sqrt(sq); + x2 = -p - sqrt(sq); + + for (i = 0; i < 4; i++) { + Lsg1[i] = x1*Be[i]+Ba[i]; + Lsg2[i] = x2*Be[i]+Ba[i]; + } + Lsg1[3] = -Lsg1[3]; + Lsg2[3] = -Lsg2[3]; + + + tmp1 = sqrt( Lsg1[0]*Lsg1[0] + Lsg1[1]*Lsg1[1] + Lsg1[2]*Lsg1[2] ); + tmp2 = sqrt( Lsg2[0]*Lsg2[0] + Lsg2[1]*Lsg2[1] + Lsg2[2]*Lsg2[2] ); + + tmp1 = fabs( tmp1 - 6371000.0 ); + tmp2 = fabs( tmp2 - 6371000.0 ); + + if (tmp1 < tmp2) { + X = Lsg1[0]; Y = Lsg1[1]; Z = Lsg1[2]; *cc = Lsg1[3]; + } else { + X = Lsg2[0]; Y = Lsg2[1]; Z = Lsg2[2]; *cc = Lsg2[3]; + } + pos_ecef[0] = X; + pos_ecef[1] = Y; + pos_ecef[2] = Z; + + return 0; +} + +int NAV_bancroft2(int N, SAT_t sats[], double pos_ecef[3], double *cc) { + + int i, j, k; + double B[N][4], BtB[4][4], BBinv[4][4], BBB[4][N]; + double a[N], Be[4], Ba[4]; + + double q0, q1, q2, p, q, sq, x1, x2; + double Lsg1[4], Lsg2[4]; + + double tmp1, tmp2; + double X, Y, Z; + + + if (N < 4 || N > 12) return -1; + + for (i = 0; i < N; i++) { + rotE(sats[i], B[i], B[i]+1, B[i]+2); + B[i][3] = sats[i].PR; + } + + if (N == 4) { + matrix_invert(B, BBB); + } + else { + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + BtB[i][j] = 0.0; + for (k = 0; k < N; k++) { + BtB[i][j] += B[k][i]*B[k][j]; + } + } + } + matrix_invert(BtB, BBinv); + for (i = 0; i < 4; i++) { + for (j = 0; j < N; j++) { + BBB[i][j] = 0.0; + for (k = 0; k < 4; k++) { + BBB[i][j] += BBinv[i][k]*B[j][k]; + } + } + } + } + + for (i = 0; i < 4; i++) { + Be[i] = 0.0; + for (k = 0; k < N; k++) { + Be[i] += BBB[i][k]*1.0; + } + } + + for (i = 0; i < N; i++) { + a[i] = 0.5 * lorentz(B[i], B[i]); + } + + for (i = 0; i < 4; i++) { + Ba[i] = 0.0; + for (k = 0; k < N; k++) { + Ba[i] += BBB[i][k]*a[k]; + } + } + + q2 = lorentz(Be, Be); + q1 = lorentz(Ba, Be)-1; + q0 = lorentz(Ba, Ba); + + if (q2 == 0) return -2; + + p = q1/q2; + q = q0/q2; + + sq = p*p - q; + if (sq < 0) return -2; + + x1 = -p + sqrt(sq); + x2 = -p - sqrt(sq); + + for (i = 0; i < 4; i++) { + Lsg1[i] = x1*Be[i]+Ba[i]; + Lsg2[i] = x2*Be[i]+Ba[i]; + } + Lsg1[3] = -Lsg1[3]; + Lsg2[3] = -Lsg2[3]; + + + tmp1 = sqrt( Lsg1[0]*Lsg1[0] + Lsg1[1]*Lsg1[1] + Lsg1[2]*Lsg1[2] ); + tmp2 = sqrt( Lsg2[0]*Lsg2[0] + Lsg2[1]*Lsg2[1] + Lsg2[2]*Lsg2[2] ); + + tmp1 = fabs( tmp1 - 6371000.0 ); + tmp2 = fabs( tmp2 - 6371000.0 ); + + if (tmp1 < tmp2) { + X = Lsg1[0]; Y = Lsg1[1]; Z = Lsg1[2]; *cc = Lsg1[3]; + } else { + X = Lsg2[0]; Y = Lsg2[1]; Z = Lsg2[2]; *cc = Lsg2[3]; + } + pos_ecef[0] = X; + pos_ecef[1] = Y; + pos_ecef[2] = Z; + + return 0; +} + +/* ---------------------------------------------------------------------------------------------------- */ + +int NAV_bancroft3(int N, SAT_t sats[], double pos_ecef1[3], double *cc1 , double pos_ecef2[3], double *cc2) { + + int i, j, k; + double B[N][4], BtB[4][4], BBinv[4][4], BBB[4][N]; + double a[N], Be[4], Ba[4]; + + double q0, q1, q2, p, q, sq, x1, x2; + double Lsg1[4], Lsg2[4]; + + double tmp1, tmp2; + double X1, Y1, Z1; + double X2, Y2, Z2; + + + if (N < 4 || N > 12) return -1; + + for (i = 0; i < N; i++) { // Test: nicht hier rotieren, sondern spaeter Lsg rotieren... + rotZ(sats[i].X, sats[i].Y, sats[i].Z, 0.0, B[i], B[i]+1, B[i]+2); + //B[i][3] = sats[i].PR; + B[i][3] = sats[i].pseudorange + sats[i].clock_corr; + } + + if (N == 4) { + matrix_invert(B, BBB); + } + else { + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + BtB[i][j] = 0.0; + for (k = 0; k < N; k++) { + BtB[i][j] += B[k][i]*B[k][j]; + } + } + } + matrix_invert(BtB, BBinv); + for (i = 0; i < 4; i++) { + for (j = 0; j < N; j++) { + BBB[i][j] = 0.0; + for (k = 0; k < 4; k++) { + BBB[i][j] += BBinv[i][k]*B[j][k]; + } + } + } + } + + for (i = 0; i < 4; i++) { + Be[i] = 0.0; + for (k = 0; k < N; k++) { + Be[i] += BBB[i][k]*1.0; + } + } + + for (i = 0; i < N; i++) { + a[i] = 0.5 * lorentz(B[i], B[i]); + } + + for (i = 0; i < 4; i++) { + Ba[i] = 0.0; + for (k = 0; k < N; k++) { + Ba[i] += BBB[i][k]*a[k]; + } + } + + q2 = lorentz(Be, Be); + q1 = lorentz(Ba, Be)-1; + q0 = lorentz(Ba, Ba); + + if (q2 == 0) return -2; + + p = q1/q2; + q = q0/q2; + + sq = p*p - q; + if (sq < 0) return -2; + + x1 = -p + sqrt(sq); + x2 = -p - sqrt(sq); + + for (i = 0; i < 4; i++) { + Lsg1[i] = x1*Be[i]+Ba[i]; + Lsg2[i] = x2*Be[i]+Ba[i]; + } + Lsg1[3] = -Lsg1[3]; + Lsg2[3] = -Lsg2[3]; + + + tmp1 = sqrt( Lsg1[0]*Lsg1[0] + Lsg1[1]*Lsg1[1] + Lsg1[2]*Lsg1[2] ); + tmp2 = sqrt( Lsg2[0]*Lsg2[0] + Lsg2[1]*Lsg2[1] + Lsg2[2]*Lsg2[2] ); + + tmp1 = tmp1 - 6371000.0; + tmp2 = tmp2 - 6371000.0; + + if ( fabs(tmp1) < fabs(tmp2) ) { + X1 = Lsg1[0]; Y1 = Lsg1[1]; Z1 = Lsg1[2]; *cc1 = Lsg1[3]; + X2 = Lsg2[0]; Y2 = Lsg2[1]; Z2 = Lsg2[2]; *cc2 = Lsg2[3]; + } else { + X1 = Lsg2[0]; Y1 = Lsg2[1]; Z1 = Lsg2[2]; *cc1 = Lsg2[3]; + X2 = Lsg1[0]; Y2 = Lsg1[1]; Z2 = Lsg1[2]; *cc2 = Lsg1[3]; + } + + rotZ(X1, Y1, Z1, EARTH_ROTATION_RATE*RANGE_ESTIMATE, pos_ecef1, pos_ecef1+1, pos_ecef1+2); + rotZ(X2, Y2, Z2, EARTH_ROTATION_RATE*RANGE_ESTIMATE, pos_ecef2, pos_ecef2+1, pos_ecef2+2); + + return 0; +} + + +/* ---------------------------------------------------------------------------------------------------- */ +/* ---------------------------------------------------------------------------------------------------- */ +// +// Satellite Position and Velocity +// + +void GPS_SatelliteClockDriftCorrection( + const unsigned short transmission_gpsweek, // GPS week when signal was transmit (0-1024+) [weeks] + const double transmission_gpstow, // GPS time of week when signal was transmit [s] + const unsigned short ephem_week, // ephemeris: GPS week (0-1024+) [weeks] + const double toe, // ephemeris: time of week [s] + const double toc, // ephemeris: clock reference time of week [s] + const double af0, // ephemeris: polynomial clock correction coefficient [s], + const double af1, // ephemeris: polynomial clock correction coefficient [s/s], + const double af2, // ephemeris: polynomial clock correction coefficient [s/s^2] + const double ecc, // ephemeris: eccentricity of satellite orbit [] + const double sqrta, // ephemeris: square root of the semi-major axis of orbit [m^(1/2)] + const double delta_n, // ephemeris: mean motion difference from computed value [rad] + const double m0, // ephemeris: mean anomaly at reference time [rad] + const double tgd, // ephemeris: group delay differential between L1 and L2 [s] + double* clock_correction, // ephemeris: satellite clock correction [m] + double* clock_drift ) // ephemeris: satellite clock drift correction [m/s] +{ + int j; + + double tot; // time of transmission (including gps week) [s] + double tk; // time from ephemeris reference epoch [s] + double tc; // time from clock reference epoch [s] + double d_tr; // relativistic correction term [s] + double d_tsv; // SV PRN code phase time offset [s] + double a; // semi-major axis of orbit [m] + double n; // corrected mean motion [rad/s] + double M; // mean anomaly, [rad] + double E; // eccentric anomaly [rad] + + // compute the times from the reference epochs + tot = transmission_gpsweek*SECONDS_IN_WEEK + transmission_gpstow; + tk = tot - (ephem_week*SECONDS_IN_WEEK + toe); + tc = tot - (ephem_week*SECONDS_IN_WEEK + toc); + + // compute the corrected mean motion term + a = sqrta*sqrta; + n = sqrt( GRAVITY_CONSTANT / (a*a*a) ); // mean motion + n += delta_n; // corrected mean motion + + // Kepler-Gleichung M = E - e sin(E) + M = m0 + n*tk; // mean anomaly + E = M; // f(E) = M + e sin(E) hat Fixpunkt fuer e < 1, + for( j = 0; j < 7; j++ ) { // da |f'(E)|=|e cos(E)|<1. + E = M + ecc * sin(E); // waehle Startwert E_1 = M, E_{k+1} = f(E_k) + } // konvergiert langsam gegen E_0 = f(E_0) + + // relativistic correction + d_tr = RELATIVISTIC_CLOCK_CORRECTION * ecc * sqrta * sin(E); // [s] + d_tr *= LIGHTSPEED; + + // clock correction + d_tsv = af0 + af1*tc + af2*tc*tc; // [s] + + // L1 only + d_tsv -= tgd; // [s] + + // clock correction + *clock_correction = d_tsv*LIGHTSPEED + d_tr; // [m] + + // clock drift + *clock_drift = (af1 + 2.0*af2*tc) * LIGHTSPEED; // [m/s] + +} + +void GPS_ComputeSatellitePositionVelocity( + const unsigned short transmission_gpsweek, // GPS week when signal was transmit (0-1024+) [weeks] + const double transmission_gpstow, // GPS time of week when signal was transmit [s] + const unsigned short ephem_week, // ephemeris: GPS week (0-1024+) [weeks] + const double toe, // ephemeris: time of week [s] + const double m0, // ephemeris: mean anomaly at reference time [rad] + const double delta_n, // ephemeris: mean motion difference from computed value [rad] + const double ecc, // ephemeris: eccentricity [] + const double sqrta, // ephemeris: square root of the semi-major axis [m^(1/2)] + const double omega0, // ephemeris: longitude of ascending node of orbit plane at weekly epoch [rad] + const double i0, // ephemeris: inclination angle at reference time [rad] + const double w, // ephemeris: argument of perigee [rad] + const double omegadot, // ephemeris: rate of right ascension [rad/s] + const double idot, // ephemeris: rate of inclination angle [rad/s] + const double cuc, // ephemeris: cos harmonic correction term to the argument of latitude [rad] + const double cus, // ephemeris: sin harmonic correction term to the argument of latitude [rad] + const double crc, // ephemeris: cos harmonic correction term to the orbit radius [m] + const double crs, // ephemeris: sin harmonic correction term to the orbit radius [m] + const double cic, // ephemeris: cos harmonic correction term to the angle of inclination [rad] + const double cis, // ephemeris: sin harmonic correction term to the angle of inclination [rad] + double* x, // satellite x [m] + double* y, // satellite y [m] + double* z, // satellite z [m] + double* vx, // satellite velocity x [m/s] + double* vy, // satellite velocity y [m/s] + double* vz ) // satellite velocity z [m/s] +{ + int j; + + double tot; // time of transmission (including gps week) [s] + double tk; // time from ephemeris reference epoch [s] + double a; // semi-major axis of orbit [m] + double n; // corrected mean motion [rad/s] + double M; // mean anomaly, [rad] + double E; // eccentric anomaly [rad] + double v; // true anomaly [rad] + double u; // argument of latitude, corrected [rad] + double r; // radius in the orbital plane [m] + double i; // orbital inclination [rad] + double cos2u; // cos(2*u) [] + double sin2u; // sin(2*u) [] + double d_u; // argument of latitude correction [rad] + double d_r; // radius correction [m] + double d_i; // inclination correction [rad] + double x_op; // x position in the orbital plane [m] + double y_op; // y position in the orbital plane [m] + double omegak; // corrected longitude of the ascending node [rad] + double cos_omegak; // cos(omegak) + double sin_omegak; // sin(omegak) + double cosu; // cos(u) + double sinu; // sin(u) + double cosi; // cos(i) + double sini; // sin(i) + double cosE; // cos(E) + double sinE; // sin(E) + + + // compute the times from the reference epochs + tot = transmission_gpsweek*SECONDS_IN_WEEK + transmission_gpstow; + tk = tot - (ephem_week*SECONDS_IN_WEEK + toe); + + // compute the corrected mean motion term + a = sqrta*sqrta; + n = sqrt( GRAVITY_CONSTANT / (a*a*a) ); // computed mean motion + n += delta_n; // corrected mean motion + + // Kepler-Gleichung M = E - e sin(E) + M = m0 + n*tk; // mean anomaly + E = M; // f(E) = M + e sin(E) hat Fixpunkt fuer e < 1, + for( j = 0; j < 7; j++ ) { // da |f'(E)|=|e cos(E)|<1. + E = M + ecc * sin(E); // waehle Startwert E_1 = M, E_{k+1} = f(E_k) + } // konvergiert langsam gegen E_0 = f(E_0) + + cosE = cos(E); + sinE = sin(E); + + // true anomaly + v = atan2( (sqrt(1.0 - ecc*ecc)*sinE), (cosE - ecc) ); + // argument of latitude + u = v + w; + // radius in orbital plane + r = a * (1.0 - ecc * cos(E)); + // orbital inclination + i = i0; + + // second harmonic perturbations + // + cos2u = cos(2.0*u); + sin2u = sin(2.0*u); + // argument of latitude correction + d_u = cuc * cos2u + cus * sin2u; + // radius correction + d_r = crc * cos2u + crs * sin2u; + // correction to inclination + d_i = cic * cos2u + cis * sin2u; + + // corrected argument of latitude + u += d_u; + // corrected radius + r += d_r; + // corrected inclination + i += d_i + idot * tk; + + // positions in orbital plane + cosu = cos(u); + sinu = sin(u); + x_op = r * cosu; + y_op = r * sinu; + + + omegak = omega0 + omegadot*tk - EARTH_ROTATION_RATE*(tk + toe); + // fuer Bestimmung der Satellitenposition in ECEF, range=0; + // fuer GPS-Loesung (Sats in ECI) Erdrotation hinzuziehen: + // rotZ(EARTH_ROTATION_RATE*0.072), 0.072s*299792458m/s=21585057m + + // compute the WGS84 ECEF coordinates, + // vector r with components x & y is now rotated using, R3(-omegak)*R1(-i) + cos_omegak = cos(omegak); + sin_omegak = sin(omegak); + cosi = cos(i); + sini = sin(i); + + *x = x_op * cos_omegak - y_op * sin_omegak * cosi; + *y = x_op * sin_omegak + y_op * cos_omegak * cosi; + *z = y_op * sini; + + + // Satellite Velocity Computations are below + // see Reference + // Remodi, B. M (2004). GPS Tool Box: Computing satellite velocities using the broadcast ephemeris. + // GPS Solutions. Volume 8(3), 2004. pp. 181-183 + // + // example source code was available at [http://www.ngs.noaa.gov/gps-toolbox/bc_velo/bc_velo.c] + + // recomputed the cos and sin of the corrected argument of latitude + + double omegadotk; // corrected rate of right ascension [rad/s] + double edot; // edot = n/(1.0 - ecc*cos(E)), [rad/s] + double vdot; // d/dt of true anomaly [rad/s] + double udot; // d/dt of argument of latitude [rad/s] + double idotdot; // d/dt of the rate of the inclination angle [rad/s^2] + double rdot; // d/dt of the radius in the orbital plane [m/s] + double tmpa; // temp + double tmpb; // temp + double vx_op; // x velocity in the orbital plane [m/s] + double vy_op; // y velocity in the orbital plane [m/s] + + cos2u = cos(2.0*u); + sin2u = sin(2.0*u); + + edot = n / (1.0 - ecc*cosE); + vdot = sinE*edot*(1.0 + ecc*cos(v)) / ( sin(v)*(1.0-ecc*cosE) ); + udot = vdot + 2.0*(cus*cos2u - cuc*sin2u)*vdot; + rdot = a*ecc*sinE*n/(1.0-ecc*cosE) + 2.0*(crs*cos2u - crc*sin2u)*vdot; + idotdot = idot + (cis*cos2u - cic*sin2u)*2.0*vdot; + + vx_op = rdot*cosu - y_op*udot; + vy_op = rdot*sinu + x_op*udot; + + // corrected rate of right ascension including similarily as above, + // for omegak, compensation for the Sagnac effect + omegadotk = omegadot - EARTH_ROTATION_RATE /* - EARTH_ROTATION_RATE*RANGERATE_ESTIMATE/LIGHTSPEED */ ; + + tmpa = vx_op - y_op*cosi*omegadotk; + tmpb = x_op*omegadotk + vy_op*cosi - y_op*sini*idotdot; + + *vx = tmpa * cos_omegak - tmpb * sin_omegak; + *vy = tmpa * sin_omegak + tmpb * cos_omegak; + *vz = vy_op*sini + y_op*cosi*idotdot; +} + +void GPS_SatellitePositionVelocity_Ephem( + const unsigned short gpsweek, // gps week of signal transmission (0-1024+) [week] + const double gpstow, // time of week of signal transmission (gpstow-psr/c) [s] + EPHEM_t ephem, + double* clock_correction, // clock correction for this satellite for this epoch [m] + double* clock_drift, // clock correction for this satellite for this epoch [m] + double* satX, // satellite X position WGS84 ECEF [m] + double* satY, // satellite Y position WGS84 ECEF [m] + double* satZ, // satellite Z position WGS84 ECEF [m] + double* satvX, // satellite X velocity WGS84 ECEF [m] + double* satvY, // satellite Y velocity WGS84 ECEF [m] + double* satvZ // satellite Z velocity WGS84 ECEF [m] + ) +{ + double tow; // user time of week adjusted with the clock corrections [s] + double x; // sat X position [m] + double y; // sat Y position [m] + double z; // sat Z position [m] + double vx; // sat vX velocity [m] + double vy; // sat VY velocity [m] + double vz; // sat VZ velocity [m] + unsigned short week; // user week adjusted with the clock correction if needed [week] + + + x = y = z = 0.0; + + GPS_SatelliteClockDriftCorrection( gpsweek, gpstow, + ephem.week, ephem.toe, ephem.toc, ephem.af0, + ephem.af1, ephem.af2, ephem.e, ephem.sqrta, + ephem.delta_n, ephem.M0, ephem.tgd, clock_correction, clock_drift ); + + + // adjust for week rollover + week = gpsweek; + tow = gpstow + (*clock_correction)/LIGHTSPEED; + if ( tow < 0.0 ) { + tow += SECONDS_IN_WEEK; + week--; + } + if ( tow > SECONDS_IN_WEEK ) { + tow -= SECONDS_IN_WEEK; + week++; + } + + //range = 0.072s*299792458m/s=21585057m + GPS_ComputeSatellitePositionVelocity( week, tow, + ephem.week, ephem.toe, ephem.M0, ephem.delta_n, ephem.e, ephem.sqrta, + ephem.Omega0, ephem.i0, ephem.w, ephem.OmegaDot, ephem.idot, + ephem.cuc, ephem.cus, ephem.crc, ephem.crs, ephem.cic, ephem.cis, + &x, &y, &z, &vx, &vy, &vz ); + + *satX = x; + *satY = y; + *satZ = z; + *satvX = vx; + *satvY = vy; + *satvZ = vz; + +} + +/* ---------------------------------------------------------------------------------------------------- */ + + +double NAV_relVel(LOC_t loc, VEL_t vel) { + double d; + double x,y,z; + double norm; + + x = vel.X-loc.X; + y = vel.Y-loc.Y; + z = vel.Z-loc.Z; + norm = sqrt(x*x+y*y+z*z); + x /= norm; + y /= norm; + z /= norm; + + d = vel.vX*x + vel.vY*y + vel.vZ*z; + + return d; +} + +int NAV_LinP(int N, SAT_t satv[], double pos_ecef[3], double dt, + double dpos_ecef[3], double *cc) { + + int i, j, k; + double B[N][4], Binv[4][N], BtB[4][4], BBinv[4][4]; + double a[N], Ba[N]; + + double X, Y, Z; + double norm[N]; + double range, obs_range, prox_range; + + if (N < 4 || N > 12) return -1; + + for (i = 0; i < N; i++) { + + range = dist( pos_ecef[0], pos_ecef[1], pos_ecef[2], satv[i].X, satv[i].Y, satv[i].Z ); + range /= LIGHTSPEED; + if (range < 0.06 || range > 0.1) range = RANGE_ESTIMATE; + rotZ(satv[i].X, satv[i].Y, satv[i].Z, EARTH_ROTATION_RATE*range, B[i], B[i]+1, B[i]+2); + //rotZ(satv[i].X, satv[i].Y, satv[i].Z, 0.0, B[i], B[i]+1, B[i]+2); // est. RANGE_RATE = 0.0 + + X = B[i][0]-pos_ecef[0]; + Y = B[i][1]-pos_ecef[1]; + Z = B[i][2]-pos_ecef[2]; + norm[i] = sqrt(X*X+Y*Y+Z*Z); + + B[i][0] = X/norm[i]; + B[i][1] = Y/norm[i]; + B[i][2] = Z/norm[i]; + + B[i][3] = 1; + } + + if (N == 4) { + matrix_invert(B, Binv); + } + else { + + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + BtB[i][j] = 0.0; + for (k = 0; k < N; k++) { + BtB[i][j] += B[k][i]*B[k][j]; + } + } + } + matrix_invert(BtB, BBinv); + for (i = 0; i < 4; i++) { + for (j = 0; j < N; j++) { + Binv[i][j] = 0.0; + for (k = 0; k < 4; k++) { + Binv[i][j] += BBinv[i][k]*B[j][k]; + } + } + } + + } + + + for (i = 0; i < N; i++) { + obs_range = satv[i].pseudorange + satv[i].clock_corr; //satv[i].PR; + prox_range = norm[i] - dt; + a[i] = prox_range - obs_range; + } + + for (i = 0; i < 4; i++) { + Ba[i] = 0.0; + for (k = 0; k < N; k++) { + Ba[i] += Binv[i][k]*a[k]; + } + } + + dpos_ecef[0] = Ba[0]; + dpos_ecef[1] = Ba[1]; + dpos_ecef[2] = Ba[2]; + + *cc = Ba[3]; + + return 0; +} + +int NAV_LinV(int N, SAT_t satv[], double pos_ecef[3], + double vel_ecef[3], double dt, + double dvel_ecef[3], double *cc) { + + int i, j, k; + double B[N][4], Binv[4][N], BtB[4][4], BBinv[4][4]; + double a[N], Ba[N]; + + double X, Y, Z; + double norm[N]; + double v_proj; + double obs_rate, prox_rate; + LOC_t loc; + VEL_t vel; + + if (N < 4 || N > 12) return -1; + + loc.X = pos_ecef[0]; + loc.Y = pos_ecef[1]; + loc.Z = pos_ecef[2]; + + if (N < 4 || N > 12) return -1; + + for (i = 0; i < N; i++) { + rotZ(satv[i].X, satv[i].Y, satv[i].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE, B[i], B[i]+1, B[i]+2); + //rotZ(satv[i].X, satv[i].Y, satv[i].Z, 0.0, B[i], B[i]+1, B[i]+2); // est. RANGE_RATE = 0.0 + + X = B[i][0]-pos_ecef[0]; + Y = B[i][1]-pos_ecef[1]; + Z = B[i][2]-pos_ecef[2]; + norm[i] = sqrt(X*X+Y*Y+Z*Z); + B[i][0] = X/norm[i]; + B[i][1] = Y/norm[i]; + B[i][2] = Z/norm[i]; + + // SatSpeed = sqrt( satv[i].vX*satv[i].vX + satv[i].vY*satv[i].vY + satv[i].vZ*satv[i].vZ ); + + B[i][3] = 1; + } + + if (N == 4) { + matrix_invert(B, Binv); + } + else { + + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + BtB[i][j] = 0.0; + for (k = 0; k < N; k++) { + BtB[i][j] += B[k][i]*B[k][j]; + } + } + } + matrix_invert(BtB, BBinv); + for (i = 0; i < 4; i++) { + for (j = 0; j < N; j++) { + Binv[i][j] = 0.0; + for (k = 0; k < 4; k++) { + Binv[i][j] += BBinv[i][k]*B[j][k]; + } + } + } + + } + + + for (i = 0; i < N; i++) { + obs_rate = satv[i].pseudorate; // + satv[i].clock_drift; + vel.X = satv[i].X; + vel.Y = satv[i].Y; + vel.Z = satv[i].Z; + vel.vX = satv[i].vX - vel_ecef[0]; + vel.vY = satv[i].vY - vel_ecef[1]; + vel.vZ = satv[i].vZ - vel_ecef[2]; + v_proj = NAV_relVel(loc, vel); + prox_rate = v_proj - dt; + a[i] = prox_rate - obs_rate; + } + + for (i = 0; i < 4; i++) { + Ba[i] = 0.0; + for (k = 0; k < N; k++) { + Ba[i] += Binv[i][k]*a[k]; + } + } + + dvel_ecef[0] = Ba[0]; + dvel_ecef[1] = Ba[1]; + dvel_ecef[2] = Ba[2]; + + *cc = Ba[3]; + + return 0; +} + diff --git a/demod/mod/rs41mod.c b/demod/mod/rs41mod.c new file mode 100644 index 0000000..e206a4c --- /dev/null +++ b/demod/mod/rs41mod.c @@ -0,0 +1,1620 @@ + +/* + * rs41 + * sync header: correlation/matched filter + * files: rs41mod.c bch_ecc_mod.c demod_mod.c demod_mod.h + * compile, either (a) or (b): + * (a) + * gcc -c demod_mod.c + * gcc -DINCLUDESTATIC rs41mod.c demod_mod.o -lm -o rs41mod + * (b) + * gcc -c demod_mod.c + * gcc -c bch_ecc_mod.c + * gcc rs41mod.c demod_mod.o bch_ecc_mod.o -lm -o rs41mod + * + * 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_mod.h" + +//#define INCLUDESTATIC 1 +#ifdef INCLUDESTATIC + #include "bch_ecc_mod.c" +#else + #include "bch_ecc_mod.h" +#endif + + +typedef struct { + i8_t vbs; // verbose output + i8_t raw; // raw frames + i8_t crc; // CRC check output + i8_t ecc; // Reed-Solomon ECC + i8_t sat; // GPS sat data + i8_t ptu; // PTU: temperature + i8_t inv; + i8_t aut; + 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; + +static 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 tow_ms; 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; + 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 + float ptu_calH[2]; // calibration Hum + ui16_t conf_fw; // firmware + ui8_t conf_cd; // kill countdown (sec) (kt or bt) + 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; + RS_t RS; +} gpx_t; + + +#define BITS 8 +#define HEADLEN 64 +#define FRAMESTART ((HEADLEN)/BITS) + +/* 10 B6 CA 11 22 96 12 F8 */ +static char rs41_header[] = "0000100001101101010100111000100001000100011010010100100000011111"; + +static ui8_t rs41_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) + +#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) +*/ +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_cd = -1; + 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_calH+0, gpx->calibytes+117, 4); // 0x07*0x10+ 5 + memcpy(gpx->ptu_calH+1, gpx->calibytes+121, 4); // 0x07*0x10+ 9 + + 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; +} +*/ +// T_RH-sensor +static float get_TH(gpx_t *gpx, ui32_t f, ui32_t f1, ui32_t f2) { + float *p = gpx->ptu_co2; + float *c = gpx->ptu_calT2; + 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; +} +// T-sensor, platinum resistor +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; +} + +// rel.hum., capacitor +// (data:) ftp://ftp-cdc.dwd.de/pub/CDC/observations_germany/radiosondes/ +// (diffAlt: Ellipsoid-Geoid) +static float get_RH(gpx_t *gpx, ui32_t f, ui32_t f1, ui32_t f2, float T) { + float b0 = gpx->ptu_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; +} + +static int get_PTU(gpx_t *gpx) { + 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(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]; + bH = gpx->calfrchk[0x07]; + + 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 (bR && bc2 && bT2) { + TH = get_TH(gpx, meas[6], meas[7], meas[8]); + } + + if (bH) { + RH = get_RH(gpx, meas[3], meas[4], meas[5], Tc); // TH, TH-Tc (sensorT - T) + } + gpx->RH = RH; + + + if (gpx->option.vbs == 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, 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 ", gpx->ptu_calH[0], gpx->ptu_calH[1]); + //printf("; %8d ; %8d ; %8d ", meas[3], meas[4], meas[5]); + 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); + + gpx->tow_ms = gpstime; + 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 == 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 == 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) { // 0x5E, 0x5A..0x5B + ui8_t bk = gpx->frame[pos_Calburst]; // fw >= 0x4ef5, burst-killtimer in 0x31 relevant + ui16_t kt = gpx->frame[pos_CalData+8] + (gpx->frame[pos_CalData+9] << 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 == 0x31) { // 0x59..0x5A + ui16_t bt = gpx->frame[pos_CalData+7] + (gpx->frame[pos_CalData+8] << 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 == 0x32) { + ui16_t cd = gpx->frame[pos_CalData+1] + (gpx->frame[pos_CalData+2] << 8); // countdown (bt or kt) (short?) + if (out && gpx->option.vbs && cd != 0xFFFF) fprintf(stdout, ": cd %.1fmin ", cd/60.0); + gpx->conf_cd = cd; + } + + 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(&gpx->RS, cw1, err_pos1, err_val1); + errors2 = rs_decode(&gpx->RS, 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(&gpx->RS, cw1, err_pos1, err_val1); + errors2 = rs_decode(&gpx->RS, 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; + int encrypted; + + gpx->out = 0; + gpx->aux = 0; + + err = get_FrameConf(gpx); + + // Quick check for an encrypted packet (RS41-SGM) + // These sondes have a type 0x80 packet in place of the regular PTU packet. + if (gpx->frame[pos_PTU] == pck_ENCRYPTED){ + encrypted = 1; + // Continue with the rest of the extraction (which will result in null data) + } + + 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 && gpx->option.aut && gpx->option.vbs == 3) fprintf(stdout, "<%c> ", gpx->option.inv?'-':'+'); + + if (!err) { + fprintf(stdout, "[%5d] ", gpx->frnr); + fprintf(stdout, "(%s) ", gpx->id); + } + if (encrypted) { + fprintf(stdout, " Encrypted payload (RS41-SGM) "); + } + 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) { + printf(" "); + if (gpx->T > -273.0) printf(" T=%.1fC ", gpx->T); + if (gpx->RH > -0.5) printf(" RH=%.0f%% ", gpx->RH); + } + + + 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 out telemetry data as JSON + if ((!err && !err1 && !err3) || (!err && encrypted)) { // frame-nb/id && gps-time && gps-position (crc-)ok; 3 CRCs, RS not needed + // eigentlich GPS, d.h. UTC = GPS - 18sec (ab 1.1.2017) + fprintf(stdout, "{ \"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) { + fprintf(stdout, ", \"temp\": %.1f", gpx->T ); + } + if (gpx->option.ptu && !err0 && gpx->RH > -0.5) { + fprintf(stdout, ", \"humidity\": %.1f", gpx->RH ); + } + if (gpx->aux) { // <=> gpx->xdata[0]!='\0' + fprintf(stdout, ", \"aux\": \"%s\"", gpx->xdata ); + } + if (encrypted){ + printf(",\"encrypted\": true"); + } + fprintf(stdout, " }\n"); + fprintf(stdout, "\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); + } +} + +/* -------------------------------------------------------------------------- */ + + +// header bit buffer +typedef struct { + char *hdr; + char *buf; + char len; + int bufpos; + float ths; +} hdb_t; + +static float cmp_hdb(hdb_t *hdb) { // bit-errors? + int i, j; + int headlen = hdb->len; + int berrs1 = 0, berrs2 = 0; + + i = 0; + j = hdb->bufpos; + while (i < headlen) { + if (j < 0) j = headlen-1; + if (hdb->buf[j] != hdb->hdr[headlen-1-i]) berrs1 += 1; + j--; + i++; + } + + i = 0; + j = hdb->bufpos; + while (i < headlen) { + if (j < 0) j = headlen-1; + if ((hdb->buf[j]^0x01) != hdb->hdr[headlen-1-i]) berrs2 += 1; + j--; + i++; + } + + if (berrs2 < berrs1) return (-headlen+berrs2)/(float)headlen; + else return ( headlen-berrs1)/(float)headlen; + + return 0; +} + +static int find_binhead(FILE *fp, hdb_t *hdb, float *score) { + int bit; + int headlen = hdb->len; + float mv; + + //*score = 0.0; + + while ( (bit = fgetc(fp)) != EOF ) + { + bit &= 1; + + hdb->bufpos = (hdb->bufpos+1) % headlen; + hdb->buf[hdb->bufpos] = 0x30 | bit; // Ascii + + mv = cmp_hdb(hdb); + if ( fabs(mv) > hdb->ths ) { + *score = mv; + return 1; + } + } + + return EOF; +} + + +int main(int argc, char *argv[]) { + + //int option_inv = 0; // invertiert Signal + int option_iq = 0; + int option_ofs = 0; + int option_bin = 0; + int wavloaded = 0; + int sel_wavch = 0; // audio channel: left + int rawhex = 0, xorhex = 0; + + FILE *fp; + char *fpname = NULL; + + int k; + + char bitbuf[8]; + int bitpos = 0, + b8pos = 0, + byte_count = FRAMESTART; + int bit, byte; + int bitQ; + + int header_found = 0; + + float thres = 0.7; // dsp.mv threshold + float _mv = 0.0; + + int symlen = 1; + int bitofs = 2; // +0 .. +3 + int shift = 0; + + pcm_t pcm = {0}; + dsp_t dsp = {0}; //memset(&dsp, 0, sizeof(dsp)); + + gpx_t gpx = {0}; + + hdb_t hdb = {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, " --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) ) { + gpx.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, "--auto") == 0) ) { gpx.option.aut = 1; } + else if (strcmp(*argv, "--bin") == 0) { option_bin = 1; } // bit/byte binary input + 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; // turn off for ber-measurement + + if (gpx.option.ecc) { + rs_init_RS255(&gpx.RS); // RS, GF + } + + // init gpx + memcpy(gpx.frame, rs41_header_bytes, sizeof(rs41_header_bytes)); // 8 header bytes + + + if (!rawhex) { + + if (!option_bin) { + + 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 + // + dsp.fp = fp; + dsp.sr = pcm.sr; + dsp.bps = pcm.bps; + dsp.nch = pcm.nch; + dsp.ch = pcm.sel_ch; + dsp.br = (float)BAUD_RATE; + dsp.sps = (float)dsp.sr/dsp.br; + dsp.symlen = symlen; + dsp.symhd = symlen; + dsp._spb = dsp.sps*symlen; + dsp.hdr = rs41_header; + dsp.hdrlen = strlen(rs41_header); + dsp.BT = 0.5; // bw/time (ISI) // 0.3..0.5 + dsp.h = 0.8; //0.7; // 0.7..0.8? modulation index abzgl. BT + dsp.opt_iq = option_iq; + + if ( dsp.sps < 8 ) { + fprintf(stderr, "note: sample rate low (%.1f sps)\n", dsp.sps); + } + } + else { + // init circular header bit buffer + hdb.hdr = rs41_header; + hdb.len = strlen(rs41_header); + hdb.ths = 1.0 - 3.1/(float)hdb.len; // 1.0-max_bit_errors/hdrlen + hdb.bufpos = -1; + hdb.buf = calloc(hdb.len, sizeof(char)); + if (hdb.buf == NULL) { + fprintf(stderr, "error: malloc\n"); + return -1; + } + } + + + 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; // FM: +1 , IQ: +2 + bitofs += shift; + + while ( 1 ) + { + if (option_bin) { + header_found = find_binhead(fp, &hdb, &_mv); + } + else { + header_found = find_header(&dsp, thres, 3, bitofs, 0); + _mv = dsp.mv; + } + if (header_found == EOF) break; + + // mv == correlation score + if (_mv *(0.5-gpx.option.inv) < 0) { + if (gpx.option.aut == 0) header_found = 0; + else gpx.option.inv ^= 0x1; + } + + if (header_found) + { + byte_count = FRAMESTART; + bitpos = 0; // byte_count*8-HEADLEN + b8pos = 0; + + while ( byte_count < FRAME_LEN ) + { + if (option_bin) { + bitQ = fgetc(fp); + if (bitQ != EOF) bit = bitQ & 0x1; + } + else { + if (option_iq >= 2) { + float bl = -1; + if (option_iq > 2) bl = 1.0; + bitQ = read_slbit(&dsp, &bit, 0/*gpx.option.inv*/, bitofs, bitpos, bl, 0); + } + else { + bitQ = read_slbit(&dsp, &bit, 0/*gpx.option.inv*/, bitofs, bitpos, -1, 0); + } + } + if ( bitQ == EOF ) break; // liest 2x EOF + + if (gpx.option.inv) bit ^= 1; + + bitpos += 1; + bitbuf[b8pos] = bit; + b8pos++; + if (b8pos == BITS) { + b8pos = 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; + } + } + + if (!option_bin) free_buffers(&dsp); + else { + if (hdb.buf) { free(hdb.buf); hdb.buf = NULL; } + } + } + 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/mod/rs92mod.c b/demod/mod/rs92mod.c new file mode 100644 index 0000000..9c91298 --- /dev/null +++ b/demod/mod/rs92mod.c @@ -0,0 +1,1531 @@ + +/* + * rs92 + * sync header: correlation/matched filter + * files: rs92mod.c nav_gps_vel.c bch_ecc_mod.c demod_mod.c demod_mod.h + * compile: + * (a) + * gcc -c demod_mod.c + * gcc -DINCLUDESTATIC rs92mod.c demod_mod.o -lm -o rs92mod + * (b) + * gcc -c demod_mod.c + * gcc -c bch_ecc_mod.c + * gcc rs92mod.c demod_mod.o bch_ecc_mod.o -lm -o rs92mod + * + * 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_mod.h" + +//#define INCLUDESTATIC 1 +#ifdef INCLUDESTATIC + #include "bch_ecc_mod.c" +#else + #include "bch_ecc_mod.h" +#endif + + +typedef struct { + i8_t vbs; // verbose output + i8_t raw; // raw frames + i8_t crc; // CRC check output + i8_t ecc; // Reed-Solomon ECC + i8_t sat; // GPS sat data + i8_t ptu; // PTU: temperature + i8_t inv; + i8_t aut; + i8_t aux; // aux/ozone + i8_t jsn; // JSON output (auto_rx) + i8_t ngp; +} option_t; + +typedef struct { + int typ; + int msglen; + int msgpos; + int parpos; + int hdrlen; + int frmlen; +} rscfg_t; + +static rscfg_t cfg_rs92 = { 92, 240-6-24, 6, 240-24, 6, 240}; + + +/* --- RS92-SGP: 8N1 manchester --- */ +#define BITS (1+8+1) // 10 +//#define HEADLEN 60 + +#define FRAMESTART 6 //((HEADLEN)/BITS) +#define FRAME_LEN 240 + +/* 2A 10*/ +static char rs92_rawheader[] = //"10100110011001101001" + //"10100110011001101001" + //"10100110011001101001" + "10100110011001101001" + "1010011001100110100110101010100110101001"; + +static ui8_t rs92_header_bytes[6] = { 0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 0x10}; + + +#include "nav_gps_vel.c" + +typedef struct { + i8_t opt_vergps; + i8_t opt_iter; + i8_t opt_vel; + float dop_limit; // 9.9 + float d_err; // 10000 + int almanac; + int ephem; + int exSat; // -1 + ui8_t sat_status[12]; + ui8_t prn[12]; // valide PRN 0,..,k-1 + ui8_t prn32toggle; // 0x1 + ui8_t prn32next; + EPHEM_t alm[33]; + EPHEM_t *ephs; + SAT_t sat[33]; + SAT_t sat1s[33]; +} GPS_t; + +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; + ui16_t conf_kt; // kill timer (sec) + int freq; + ui32_t crc; + ui8_t frame[FRAME_LEN]; // { 0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 0x10} + unsigned short aux[4]; + double diter; + option_t option; + RS_t RS; + GPS_t gps; +} gpx_t; + +/* --- RS92-SGP ------------------- */ + + +#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 + */ + +/* ------------------------------------------------------------------------------------ */ + +#define BAUD_RATE 4800 + +/* ------------------------------------------------------------------------------------ */ + +// manchester1 1->10,0->01: 1.bit +// manchester2 0->10,1->01: 2.bit +// RS92-SGP: 8N1 manchester2 +static 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]; +} +*/ + +/* ------------------------------------------------------------------------------------ */ + +#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. + */ +// 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; +} + +/* ------------------------------------------------------------------------------------ */ + +#define crc_FRAME (1<<0) +#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 crc_GPS (1<<2) +#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 crc_PTU (1<<1) +#define pos_PTU 0x2C // 24 byte + +#define crc_AUX (1<<3) +#define pos_AUX 0xC6 // 10 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)) +#define LEN_AUX (2*(BLOCK_AUX & 0xFF)) + + +static int crc16(gpx_t *gpx, 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 = 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 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 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 = gpx->frame[pos_FrameNb+LEN_CFG] | (gpx->frame[pos_FrameNb+LEN_CFG+1] << 8); + crc = crc16(gpx, pos_FrameNb, LEN_CFG); + if (crc_frame != crc) gpx->crc |= crc_FRAME; +/* + if (gpx->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 &&*/ gpx->option.crc && crc != crc_frame) { + ret = -2; // erst wichtig, wenn Cal/Cfg-Data + } + + for (i = 0; i < 8; i++) { + byte = gpx->frame[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; +} + +static int get_PTU(gpx_t *gpx) { + int ret=0; + int crc_frame, crc; + + crc_frame = gpx->frame[pos_PTU+LEN_PTU] | (gpx->frame[pos_PTU+LEN_PTU+1] << 8); + crc = crc16(gpx, pos_PTU, LEN_PTU); + if (crc_frame != crc) gpx->crc |= crc_PTU; + + ret = 0; + if (gpx->option.crc && crc != crc_frame) { + ret = -2; + } + + return ret; +} + +//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, ret=0; + unsigned byte; + ui8_t gpstime_bytes[4]; + ui32_t gpstime = 0; // 32bit + int day; + int ms; + int crc_frame, crc; + + // BLOCK_GPS == frame[posGPS_TOW-2 .. posGPS_TOW-1] ? + crc_frame = gpx->frame[posGPS_TOW+LEN_GPS] | (gpx->frame[posGPS_TOW+LEN_GPS+1] << 8); + crc = crc16(gpx, posGPS_TOW, LEN_GPS); + if (crc_frame != crc) gpx->crc |= crc_GPS; +/* + if (gpx->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 (gpx->option.crc && crc != crc_frame) { + ret = -2; + } + + for (i = 0; i < 4; i++) { + byte = gpx->frame[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; +} + +static int get_Aux(gpx_t *gpx) { + int i, ret=0; + unsigned short byte; + int crc_frame, crc; + + crc_frame = gpx->frame[pos_AUX+LEN_AUX] | (gpx->frame[pos_AUX+LEN_AUX+1] << 8); + crc = crc16(gpx, pos_AUX, LEN_AUX); + if (crc_frame != crc) gpx->crc |= crc_AUX; + + ret = 0; + if (gpx->option.crc && crc != crc_frame) { + ret = -2; + } + + for (i = 0; i < 4; i++) { + byte = gpx->frame[pos_AuxData+2*i] + (gpx->frame[pos_AuxData+2*i+1]<<8); + gpx->aux[i] = byte; + } + + return ret; +} + +static int get_Cal(gpx_t *gpx) { + int i; + unsigned byte; + ui8_t calfr = 0; + //ui8_t burst = 0; + ui8_t bytes[2]; + int freq = 0; + ui16_t killtime = 0; + + byte = gpx->frame[pos_CalData]; + calfr = byte; + + if (gpx->option.vbs == 4) { + fprintf(stdout, "\n"); + 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 ((gpx->crc & crc_FRAME)==0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); + } + + if (gpx->option.aux) { + if (gpx->option.vbs == 4) { + fprintf(stdout, " # "); + for (i = 0; i < 8; i++) { + byte = gpx->frame[pos_AuxData+i]; + fprintf(stdout, "%02x ", byte); + } + } + } + + if (calfr == 0x00) { + for (i = 0; i < 2; i++) { + bytes[i] = gpx->frame[pos_Calfreq + i]; + } + byte = bytes[0] + (bytes[1] << 8); + //fprintf(stdout, ":%04x ", byte); + freq = 400000 + 10*byte; // kHz; + if (gpx->option.ngp) freq = 1600000 + 10*byte; // kHz + gpx->freq = freq; + fprintf(stdout, ": fq %d", freq); + for (i = 0; i < 2; i++) { + bytes[i] = gpx->frame[pos_Calfreq + 2 + i]; + } + killtime = bytes[0] + (bytes[1] << 8); // signed? + if (killtime < 0xFFFF && gpx->option.vbs == 4) { + fprintf(stdout, "; KT:%ds", killtime); + } + gpx->conf_kt = killtime; + } + + return 0; +} + + +/* ---------------------------------------------------------------------------------------------------- */ + + +static 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; +} + +static void prn12(GPS_t *gps, ui8_t *prn_le, ui8_t prns[12]) { + int i, j, d; + ui8_t ind_prn32 = 32; + + 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; + } + } + for (i = 0; i < 12; i++) { + // PRN-32 overflow + if ( (prns[i] == 0) && (gps->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 ((gps->sat_status[i] & 0x0F) == 0) { // erste beiden bits: 0x03 ? + prns[i] = 0; + } + } + + gps->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 ((gps->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]^gps->prn32toggle) && (gps->sat_status[j] & 0x0F)) break; + } + if (j < ind_prn32) { gps->prn32toggle ^= 0x1; } + else { + for (j = ind_prn32+2; j < 12; j++) { + if (prns[j] == (prns[ind_prn32+1]^gps->prn32toggle) && (gps->sat_status[j] & 0x0F)) break; + } + if (j < 12) { gps->prn32toggle ^= 0x1; } + } + prns[ind_prn32+1] ^= gps->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) { gps->prn32toggle ^= 0x1; } + */ + } + gps->prn32next = prns[ind_prn32+1]; // -> ind_prn32<11 && ind_prn32 % 3 != 2 + } + } +} + +static int calc_satpos_alm(gpx_t *gpx, double t, SAT_t *satp) { + double X, Y, Z, vX, vY, vZ; + int j; + int week; + double cl_corr, cl_drift; + int rollover = 0; + EPHEM_t *alm = gpx->gps.alm; + + 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 (gpx->gps.opt_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; +} + +static int calc_satpos_rnx2(gpx_t *gpx, 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; + int rollover = 0; + EPHEM_t *eph = gpx->gps.ephs; + + 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 (gpx->gps.opt_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; + +// 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 + +static int get_pseudorange(gpx_t *gpx) { + 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; + ui8_t prn_le[12*5+4]; // le - little endian + ui8_t prns[12]; // PRNs in data + RANGE_t range[33]; + + memset(prn_le, 0, sizeof(prn_le)); + memset(prns, 0, sizeof(prns)); + memset(range, 0, sizeof(range)); + + // GPS-TOW in ms + for (i = 0; i < 4; i++) { + gpstime_bytes[i] = gpx->frame[posGPS_TOW + i]; + } + memcpy(&gpstime, gpstime_bytes, 4); + + // Sat Status + for (i = 0; i < 12; i++) { + gpx->gps.sat_status[i] = gpx->frame[posGPS_STATUS + i]; + } + + // PRN-Nummern + for (i = 0; i < 4; i++) { + for (j = 0; j < 2; j++) { + bytes[j] = gpx->frame[posGPS_PRN+2*i+j]; + } + memcpy(&byte16, bytes, 2); + prnbits_le(byte16, prn_le, i); + } + prn12(&gpx->gps, prn_le, prns); + + + // GPS Sat Pos (& Vel) + if (gpx->gps.almanac) calc_satpos_alm( gpx, gpstime/1000.0, gpx->gps.sat); + if (gpx->gps.ephem) calc_satpos_rnx2(gpx, gpstime/1000.0, gpx->gps.sat); + + // GPS Sat Pos t -= 1s + if (gpx->gps.opt_vel == 1) { + if (gpx->gps.almanac) calc_satpos_alm( gpx, gpstime/1000.0-1, gpx->gps.sat1s); + if (gpx->gps.ephem) calc_satpos_rnx2(gpx, gpstime/1000.0-1, gpx->gps.sat1s); + } + + k = 0; + for (j = 0; j < 12; j++) { + + // Pseudorange/chips + for (i = 0; i < 4; i++) { + pseudobytes[i] = gpx->frame[posGPS_DATA+8*j+i]; + } + memcpy(&chipbytes, pseudobytes, 4); + + // delta_pseudochips / 385 + for (i = 0; i < 3; i++) { + pseudobytes[i] = gpx->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) && (gpx->gps.sat_status[j] & 0x0F) ) prns[j] = 32; + range[prns[j]].tow = gpstime; + range[prns[j]].status = gpx->gps.sat_status[j]; + + if ( chipbytes == 0x7FFFFFFF || chipbytes == 0x55555555 ) { + range[prns[j]].chips = 0; + continue; + } + if (gpx->gps.opt_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) && ((gpx->gps.sat_status[j] & 0x0F) == 0xF) + && (dist(gpx->gps.sat[prns[j]].X, gpx->gps.sat[prns[j]].Y, gpx->gps.sat[prns[j]].Z, 0, 0, 0) > 6700000) ) + { + for (i = 0; i < k; i++) { if (gpx->gps.prn[i] == prns[j]) break; } + if (i == k && prns[j] != gpx->gps.exSat) { + //if ( range[prns[j]].status & 0xF0 ) // Signalstaerke > 0 ? + { + gpx->gps.prn[k] = prns[j]; + k++; + } + } + } + + } + + + for (j = 0; j < 12; j++) { // 0x013FB0A4 + gpx->gps.sat[prns[j]].pseudorange = /*0x01400000*/ - range[prns[j]].chips * df; + gpx->gps.sat1s[prns[j]].pseudorange = -(range[prns[j]].chips - range[prns[j]].deltachips/dl)*df; + //+ sat[prns[j]].clock_corr - gpx->gps.sat1s[prns[j]].clock_corr + gpx->gps.sat[prns[j]].pseudorate = - range[prns[j]].deltachips * df / dl; + + gpx->gps.sat[prns[j]].prn = prns[j]; + gpx->gps.sat1s[prns[j]].prn = prns[j]; + } + + + pr0 = (double)0x01400000; + for (j = 0; j < k; j++) { + prj = gpx->gps.sat[gpx->gps.prn[j]].pseudorange + gpx->gps.sat[gpx->gps.prn[j]].clock_corr; + if (prj < pr0) pr0 = prj; + } + for (j = 0; j < k; j++) gpx->gps.sat[gpx->gps.prn[j]].PR = gpx->gps.sat[gpx->gps.prn[j]].pseudorange + + gpx->gps.sat[gpx->gps.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 + gpx->gps.sat1s[gpx->gps.prn[j]].PR = gpx->gps.sat1s[gpx->gps.prn[j]].pseudorange + + gpx->gps.sat[gpx->gps.prn[j]].clock_corr - pr0 + 20e6; + } + + return k; +} + +static 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; +} + +static int get_GPSkoord(gpx_t *gpx, 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; + double DOP[4]; + 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 (gpx->gps.opt_vergps == 8) { + fprintf(stdout, " sats: "); + for (j = 0; j < N; j++) fprintf(stdout, "%02d ", gpx->gps.prn[j]); + fprintf(stdout, "\n"); + } + + gpx->lat = gpx->lon = gpx->alt = 0; + DOP[0] = DOP[1] = DOP[2] = DOP[3] = 0.0; + + if (gpx->gps.opt_vergps != 2) { + for (i0=0;i0gps.sat[gpx->gps.prn[i0]]; + Sat_A[1] = gpx->gps.sat[gpx->gps.prn[i1]]; + Sat_A[2] = gpx->gps.sat[gpx->gps.prn[i2]]; + Sat_A[3] = gpx->gps.sat[gpx->gps.prn[i3]]; + nav_ret = NAV_ClosedFormSolution_FromPseudorange( Sat_A, &lat, &lon, &alt, &rx_cl_bias, pos_ecef ); + + if (nav_ret == 0) { + num += 1; + if (calc_DOPn(4, Sat_A, pos_ecef, DOP) == 0) { + gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]); + //fprintf(stdout, " DOP : %.1f ", gdop); + + NAV_LinP(4, Sat_A, pos_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); + diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1], dpos_ecef[2]); + 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); + if ( gpx->gps.opt_vel == 4 ) { + vel_ecef[0] = vel_ecef[1] = vel_ecef[2] = 0; + NAV_LinV(4, Sat_A, pos_ecef, vel_ecef, 0.0, dvel_ecef, &rx_cl_bias); + for (j=0; j<3; j++) vel_ecef[j] += dvel_ecef[j]; + NAV_LinV(4, Sat_A, pos_ecef, vel_ecef, rx_cl_bias, dvel_ecef, &rx_cl_bias); + for (j=0; j<3; j++) vel_ecef[j] += dvel_ecef[j]; + get_GPSvel(lat, lon, vel_ecef, &vH, &vD, &vU); + } + if (gpx->gps.opt_vergps == 8) { + // gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]); // s.o. + //hdop = sqrt(DOP[0]+DOP[1]); + //vdop = sqrt(DOP[2]); + //pdop = sqrt(DOP[0]+DOP[1]+DOP[2]); + if (gdop < gpx->gps.dop_limit) { + fprintf(stdout, " "); + fprintf(stdout, "lat: %.5f , lon: %.5f , alt: %.1f ", lat, lon, alt); + fprintf(stdout, " (d:%.1f)", diter); + if ( gpx->gps.opt_vel == 4 ) { + fprintf(stdout, " vH: %4.1f D: %5.1f vV: %3.1f ", vH, vD, vU); + } + fprintf(stdout, " sats: "); + fprintf(stdout, "%02d %02d %02d %02d ", gpx->gps.prn[i0], gpx->gps.prn[i1], gpx->gps.prn[i2], gpx->gps.prn[i3]); + fprintf(stdout, " GDOP : %.1f ", gdop); + //fprintf(stdout, " HDOP: %.1f VDOP: %.1f ", hdop, vdop); + //fprintf(stdout, " PDOP: %.1f ", pdop); + fprintf(stdout, "\n"); + } + } + } + else gdop = -1; + + if (gdop > 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] = gpx->gps.prn[i0]; gpx->sats[1] = gpx->gps.prn[i1]; gpx->sats[2] = gpx->gps.prn[i2]; gpx->sats[3] = gpx->gps.prn[i3]; + gdop0 = gdop; + + if (gpx->gps.opt_vel == 4) { + gpx->vH = vH; + gpx->vD = vD; + gpx->vU = vU; + } + } + } + + }}}} + } + + if (gpx->gps.opt_vergps == 8 || gpx->gps.opt_vergps == 2) { + + for (j = 0; j < N; j++) Sat_B[j] = gpx->gps.sat[gpx->gps.prn[j]]; + for (j = 0; j < N; j++) Sat_B1s[j] = gpx->gps.sat1s[gpx->gps.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 (gpx->gps.opt_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 > gpx->gps.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 (gpx->gps.prn[exN] == gpx->gps.prn32next) gpx->gps.prn32toggle ^= 0x1; + for (k = exN; k < N-1; k++) { + Sat_B[k] = Sat_B[k+1]; + gpx->gps.prn[k] = gpx->gps.prn[k+1]; + if (gpx->gps.opt_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 && gpx->gps.prn32next > 0) { + //prn32next used in pre-fix? prn32toggle ^= 0x1; + } +*/ + } + + if (gpx->gps.opt_vel == 1) { + NAV_bancroft1(N, Sat_B1s, pos1s_ecef, &rx_cl_bias); + if (gpx->gps.opt_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 (gpx->gps.opt_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 (gpx->gps.opt_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 (gpx->gps.opt_vergps == 8) { + fprintf(stdout, "bancroft[%2d] lat: %.6f , lon: %.6f , alt: %.2f ", N, lat, lon, alt); + fprintf(stdout, " (d:%.1f)", gpx->diter); + if (gpx->gps.opt_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", gpx->gps.prn[j]); + if (j < N-1) fprintf(stdout, ","); else fprintf(stdout, "] %.1f ", gdop); + } + fprintf(stdout, "\n"); + } + + if (gpx->gps.opt_vergps == 2) { + gpx->lat = lat; + gpx->lon = lon; + gpx->alt = alt; + gpx->dop = gdop; + num = N; + + if (gpx->gps.opt_vel) { + gpx->vH = vH; + gpx->vD = vD; + gpx->vU = vU; + } + } + + } + + return num; +} + + +/* ------------------------------------------------------------------------------------ */ + +#define rs_N 255 +#define rs_R 24 +#define rs_K (rs_N-rs_R) + +static int rs92_ecc(gpx_t *gpx, int msglen) { + + int i, ret = 0; + int errors; + ui8_t cw[rs_N]; + ui8_t err_pos[rs_R], err_val[rs_R]; + + memset(cw, 0, rs_N); + + if (msglen > FRAME_LEN) msglen = FRAME_LEN; + for (i = msglen; i < FRAME_LEN; i++) gpx->frame[i] = 0;//xFF; + + + for (i = 0; i < rs_R; i++) cw[i] = gpx->frame[cfg_rs92.parpos+i]; + for (i = 0; i < cfg_rs92.msglen; i++) cw[rs_R+i] = gpx->frame[cfg_rs92.msgpos+i]; + + errors = rs_decode(&gpx->RS, cw, err_pos, err_val); + + //for (i = 0; i < cfg_rs92.hdrlen; i++) gpx->frame[i] = data[i]; + for (i = 0; i < rs_R; i++) gpx->frame[cfg_rs92.parpos+i] = cw[i]; + for (i = 0; i < cfg_rs92.msglen; i++) gpx->frame[cfg_rs92.msgpos+i] = cw[rs_R+i]; + + ret = errors; + + return ret; +} + +/* ------------------------------------------------------------------------------------ */ + +static int print_position(gpx_t *gpx, int ec) { // GPS-Hoehe ueber Ellipsoid + int j, k, n = 0; + int err1, err2, err3, err4; + + err1 = 0; + err1 |= get_FrameNb(gpx); + err1 |= get_SondeID(gpx); + err2 = get_PTU(gpx); + err3 = 0; + //err3 |= get_GPSweek(); + err3 |= get_GPStime(gpx); + err4 = get_Aux(gpx); + + if (!err3 && (gpx->gps.almanac || gpx->gps.ephem)) { + k = get_pseudorange(gpx); + if (k >= 4) { + n = get_GPSkoord(gpx, k); + } + } + + if (!err1) + { + fprintf(stdout, "[%5d] ", gpx->frnr); + fprintf(stdout, "(%s) ", gpx->id); + + if (!err3) { + if (gpx->gps.almanac || gpx->gps.ephem) + { + Gps2Date(gpx); + //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 (gpx->gps.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 (gpx->option.vbs && gpx->gps.opt_vergps != 8) { + fprintf(stdout, " (d:%.1f)", gpx->diter); + } + if (gpx->gps.opt_vel /*&& gpx->gps.opt_vergps >= 2*/) { + fprintf(stdout," vH: %4.1f D: %5.1f vV: %3.1f ", gpx->vH, gpx->vD, gpx->vU); + } + if (gpx->option.vbs) { + if (gpx->gps.opt_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 gpx->gps.opt_vergps=2, dann n=N=k(-1) + fprintf(stdout, " DOP["); + for (j = 0; j < n; j++) { + fprintf(stdout, "%d", gpx->gps.prn[j]); + if (j < n-1) fprintf(stdout, ","); else fprintf(stdout, "] %.1f ", gpx->dop); + } + } + } + } + } + + if (gpx->option.aux) { + if (gpx->option.vbs != 4 && (gpx->crc & crc_AUX)==0 || !gpx->option.crc) { + 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]); + } + } + } + + fprintf(stdout, " # "); + fprintf(stdout, "["); + for (j=0; j<4; j++) fprintf(stdout, "%d", (gpx->crc>>j)&1); + fprintf(stdout, "]"); + if (gpx->option.ecc == 2) { + if (ec > 0) fprintf(stdout, " (%d)", ec); + if (ec < 0) fprintf(stdout, " (-)"); + } + + get_Cal(gpx); +/* + if (!err3) { + if (gpx->gps.opt_vergps == 8) + { + 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 ", gpx->gps.sat_status[j]); //range[prns[j]].status + fprintf(stdout, "\n"); + } + } +*/ + + if (gpx->option.jsn) { + // Print out telemetry data as JSON //even if we don't have a valid GPS lock + if ((gpx->crc & (crc_FRAME | crc_GPS))==0 && (gpx->gps.almanac || gpx->gps.ephem)) //(!err1 && !err3) + { // eigentlich GPS, d.h. UTC = GPS - UTC_OFS (UTC_OFS=18sec ab 1.1.2017) + fprintf(stdout, "\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", + 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); + if ((gpx->crc & crc_AUX)==0 && (gpx->aux[0] != 0 || gpx->aux[1] != 0 || gpx->aux[2] != 0 || gpx->aux[3] != 0)) { + fprintf(stdout, ", \"aux\": \"%04x%04x%04x%04x\"", gpx->aux[0], gpx->aux[1], gpx->aux[2], gpx->aux[3]); + } + fprintf(stdout, " }\n"); + } + } + + fprintf(stdout, "\n"); + //if (gpx->gps.opt_vergps == 8) fprintf(stdout, "\n"); + } + + return err3; +} + +static void print_frame(gpx_t *gpx, int len) { + int i, ec = 0; + ui8_t byte; + + gpx->crc = 0; + + if (gpx->option.ecc) { + ec = rs92_ecc(gpx, len); + } + + for (i = len; i < FRAME_LEN; i++) { + gpx->frame[i] = 0; + } + + if (gpx->option.raw) { + for (i = 0; i < len; i++) { + byte = gpx->frame[i]; + fprintf(stdout, "%02x", byte); + } + if (gpx->option.ecc && gpx->option.vbs) { + fprintf(stdout, " "); + if (ec >= 0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); + if (ec > 0) fprintf(stdout, " (%d)", ec); + if (ec < 0) fprintf(stdout, " (-)"); + } + fprintf(stdout, "\n"); +// fprintf(stdout, "\n"); + } + else print_position(gpx, ec); +} + + +int main(int argc, char *argv[]) { + + FILE *fp, *fp_alm = NULL, *fp_eph = NULL; + char *fpname = NULL; + + int option_der = 0; // linErr + int option_iq = 0; + int sel_wavch = 0; // audio channel: left + int spike = 0; + int fileloaded = 0; + + char bitbuf[BITS]; + int bitpos = 0, + b8pos = 0, + byte_count = FRAMESTART; + int bit, byte; + int bitQ; + int herrs, herr1; + int headerlen = 0; + + int k; + + int header_found = 0; + + float thres = 0.7; + float _mv = 0.0; + + int symlen = 2; + int bitofs = 2; // +0 .. +3 + int shift = 0; + + pcm_t pcm = {0}; + dsp_t dsp = {0}; //memset(&dsp, 0, sizeof(dsp)); + + gpx_t gpx = {0}; + + gpx.gps.prn32toggle = 0x1; + gpx.gps.dop_limit = 9.9; + gpx.gps.d_err = 10000; + gpx.gps.exSat = -1; + + +#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) ) { + gpx.gps.opt_vel = 4; + } + else if ( (strcmp(*argv, "--vel1") == 0) ) { + gpx.gps.opt_vel = 1; + if (gpx.gps.opt_vergps < 1) gpx.gps.opt_vergps = 2; + } + else if ( (strcmp(*argv, "--vel2") == 0) ) { + gpx.gps.opt_vel = 2; + if (gpx.gps.opt_vergps < 1) gpx.gps.opt_vergps = 2; + } + else if ( (strcmp(*argv, "--iter") == 0) ) { + gpx.gps.opt_iter = 1; + } + else if ( (strcmp(*argv, "-v") == 0) ) { gpx.option.vbs = 1; } + else if ( (strcmp(*argv, "-vv") == 0) ) { gpx.option.vbs = 4; } + else if ( (strcmp(*argv, "-vx") == 0) ) { gpx.option.aux = 1; } + else if (strcmp(*argv, "--crc") == 0) { gpx.option.crc = 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, "-r") == 0) || (strcmp(*argv, "--raw") == 0) ) { + gpx.option.raw = 1; + } + else if ( (strcmp(*argv, "-i") == 0) || (strcmp(*argv, "--invert") == 0) ) { + gpx.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) { + gpx.gps.dop_limit = atof(*argv); + if (gpx.gps.dop_limit <= 0 || gpx.gps.dop_limit >= 100) gpx.gps.dop_limit = 9.9; + } + else return -1; + } + else if ( (strcmp(*argv, "--der") == 0) ) { + ++argv; + if (*argv) { + gpx.gps.d_err = atof(*argv); + if (gpx.gps.d_err <= 0 || gpx.gps.d_err >= 100000) gpx.gps.d_err = 10000; + else option_der = 1; + } + else return -1; + } + else if ( (strcmp(*argv, "--exsat") == 0) ) { + ++argv; + if (*argv) { + gpx.gps.exSat = atoi(*argv); + if (gpx.gps.exSat < 1 || gpx.gps.exSat > 32) gpx.gps.exSat = -1; + } + else return -1; + } + else if (strcmp(*argv, "-g1") == 0) { gpx.gps.opt_vergps = 1; } // verbose1 GPS + else if (strcmp(*argv, "-g2") == 0) { gpx.gps.opt_vergps = 2; } // verbose2 GPS (bancroft) + else if (strcmp(*argv, "-gg") == 0) { gpx.gps.opt_vergps = 8; } // vverbose GPS + else if (strcmp(*argv, "--json") == 0) { + gpx.option.jsn = 1; + gpx.option.ecc = 2; + gpx.option.crc = 1; + gpx.gps.opt_vel = 4; + } + else if (strcmp(*argv, "--spike") == 0) { spike = 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, "--ngp") == 0) { gpx.option.ngp = 1; } // RS92-NGP, RS92-D: 1680 MHz + else { + fp = fopen(*argv, "rb"); + 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, gpx.gps.alm) == 0) { + gpx.gps.almanac = 1; + } + fclose(fp_alm); + if (!option_der) gpx.gps.d_err = 4000; + } + if (fp_eph) { + /* i = read_RNXephemeris(fp_eph, eph); + if (i == 0) { + gpx.gps.ephem = 1; + gpx.gps.almanac = 0; + } + fclose(fp_eph); */ + gpx.gps.ephs = read_RNXpephs(fp_eph); + if (gpx.gps.ephs) { + gpx.gps.ephem = 1; + gpx.gps.almanac = 0; + } + fclose(fp_eph); + if (!option_der) gpx.gps.d_err = 1000; + } + + + if (gpx.option.ecc) { + rs_init_RS255(&gpx.RS); + } + + // init gpx + memcpy(gpx.frame, rs92_header_bytes, sizeof(rs92_header_bytes)); // 6 header bytes + + pcm.sel_ch = sel_wavch; + k = read_wav_header(&pcm, fp); + if ( k < 0 ) { + fclose(fp); + fprintf(stderr, "error: wav header\n"); + return -1; + } + + // rs92-sgp: BT=0.5, h=1.0 ? + symlen = 2; + + // init dsp + // + dsp.fp = fp; + dsp.sr = pcm.sr; + dsp.bps = pcm.bps; + dsp.nch = pcm.nch; + dsp.ch = pcm.sel_ch; + dsp.br = (float)BAUD_RATE; + dsp.sps = (float)dsp.sr/dsp.br; + dsp.symlen = symlen; + dsp.symhd = symlen; + dsp._spb = dsp.sps*symlen; + dsp.hdr = rs92_rawheader; + dsp.hdrlen = strlen(rs92_rawheader); + dsp.BT = 0.5; // bw/time (ISI) // 0.3..0.5 + dsp.h = 0.8; // 1.0? modulation index abzgl. BT + if (gpx.option.ngp) dsp.h *= 4.5; // L-band rs92-ngp + 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; + }; + + bitofs += shift; + + while ( 1 ) { + + header_found = find_header(&dsp, thres, 3, bitofs, 0); + _mv = dsp.mv; + + if (header_found == EOF) break; + + // mv == correlation score + if (_mv *(0.5-gpx.option.inv) < 0) { + if (gpx.option.aut == 0) header_found = 0; + else gpx.option.inv ^= 0x1; + } + + if (header_found) { + + byte_count = FRAMESTART; + bitpos = 0; + b8pos = 0; + + while ( byte_count < FRAME_LEN ) { + float bl = -1; + if (option_iq > 2) bl = 4.0; + bitQ = read_slbit(&dsp, &bit, 0/*gpx.option.inv*/, bitofs, bitpos, bl, spike); + if ( bitQ == EOF) break; + + if (gpx.option.inv) bit ^= 1; + + bitpos += 1; + bitbuf[b8pos] = bit; + b8pos++; + if (b8pos >= BITS) { + b8pos = 0; + byte = bits2byte(bitbuf); + gpx.frame[byte_count] = byte; + byte_count++; + } + } + header_found = 0; + print_frame(&gpx, byte_count); + byte_count = FRAMESTART; + + } + + } + + + free_buffers(&dsp); + + if (gpx.gps.ephs) free(gpx.gps.ephs); + fclose(fp); + + return 0; +} + +