diff --git a/demod/multi/bch_ecc_mod.c b/demod/multi/bch_ecc_mod.c new file mode 100644 index 0000000..812fc9e --- /dev/null +++ b/demod/multi/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/multi/bch_ecc_mod.h b/demod/multi/bch_ecc_mod.h new file mode 100644 index 0000000..d52a423 --- /dev/null +++ b/demod/multi/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/multi/demod_base.c b/demod/multi/demod_base.c new file mode 100644 index 0000000..c32fb77 --- /dev/null +++ b/demod/multi/demod_base.c @@ -0,0 +1,1050 @@ + +/* + * sync header: correlation/matched filter + * compile: + * gcc -c demod_base.c + * + * author: zilog80 + */ + +/* ------------------------------------------------------------------------------------ */ + +#include +#include +#include + +#include "demod_base.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 = pcm->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; +} + + +static volatile int bufeof = 0; // threads exit +static volatile int rbf; +extern int rbf1; + + +static int __f32read_cblock_nocond(dsp_t *dsp) { // blk + // faster; however if #{fqs} > #{cores}, very very slow, quasi-lock + int n; + int BL = dsp->decM * blk_sz; + int len = BL; + + if (bufeof) return 0; + + pthread_mutex_lock( dsp->thd.mutex ); + + if (rbf == 0) + { + if (dsp->bps == 8) { + ui8_t u[2*BL]; + len = fread( u, dsp->bps/8, 2*BL, dsp->fp) / 2; + for (n = 0; n < len; n++) dsp->thd.blk[n] = (u[2*n]-128)/128.0 + I*(u[2*n+1]-128)/128.0; + } + else { // dsp->bps == 16 + short b[2*BL]; + len = fread( b, dsp->bps/8, 2*BL, dsp->fp) / 2; + for (n = 0; n < len; n++) dsp->thd.blk[n] = b[2*n]/32768.0 + I*b[2*n+1]/32768.0; + } + if (len < BL) bufeof = 1; + + rbf = rbf1; // set all bits + } + pthread_mutex_unlock( dsp->thd.mutex ); + + while ((rbf & dsp->thd.tn_bit) == 0) ; // only if #{fqs} leq #{cores} ... + + for (n = 0; n < dsp->decM; n++) dsp->decMbuf[n] = dsp->thd.blk[dsp->decM*dsp->blk_cnt + n]; + + dsp->blk_cnt += 1; + if (dsp->blk_cnt == blk_sz) { + pthread_mutex_lock( dsp->thd.mutex ); + rbf &= ~(dsp->thd.tn_bit); // clear bit(tn) + dsp->blk_cnt = 0; + pthread_mutex_unlock( dsp->thd.mutex ); + } + + return len; +} + +static int f32read_cblock(dsp_t *dsp) { // blk_cond + + int n; + int BL = dsp->decM * blk_sz; + int len = BL; + + if (bufeof) return 0; + + pthread_mutex_lock( dsp->thd.mutex ); + + if (rbf == 0) + { + if (dsp->bps == 8) { + ui8_t u[2*BL]; + len = fread( u, dsp->bps/8, 2*BL, dsp->fp) / 2; + for (n = 0; n < len; n++) dsp->thd.blk[n] = (u[2*n]-128)/128.0 + I*(u[2*n+1]-128)/128.0; + } + else { // dsp->bps == 16 + short b[2*BL]; + len = fread( b, dsp->bps/8, 2*BL, dsp->fp) / 2; + for (n = 0; n < len; n++) dsp->thd.blk[n] = b[2*n]/32768.0 + I*b[2*n+1]/32768.0; + } + if (len < BL) bufeof = 1; + + rbf = rbf1; // set all bits + pthread_cond_broadcast( dsp->thd.cond ); + } + + while ((rbf & dsp->thd.tn_bit) == 0) pthread_cond_wait( dsp->thd.cond, dsp->thd.mutex ); + + for (n = 0; n < dsp->decM; n++) dsp->decMbuf[n] = dsp->thd.blk[dsp->decM*dsp->blk_cnt + n]; + + dsp->blk_cnt += 1; + if (dsp->blk_cnt == blk_sz) { + rbf &= ~(dsp->thd.tn_bit); // clear bit(tn) + dsp->blk_cnt = 0; + } + + pthread_mutex_unlock( dsp->thd.mutex ); + + return len; +} + + +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; +} + +static int res = 1; // 1..10 Hz, exp_lut resolution + +static double sinc(double x) { + double y; + if (x == 0) y = 1; + else y = sin(M_PI*x)/(M_PI*x); + return y; +} + +static double *ws_dec; + +int decimate_init(int taps, float f) { + //int M = 4 / (tbw/sr) + 1; + int M = taps; + if (M % 2 == 0) M++; // odd/symmetric + //float f = 0.25; + + double *h, *w; + double norm = 0; + int n; + + if ( M < 1 ) M = 1; + + h = (double*)calloc( M+1, sizeof(double)); + w = (double*)calloc( M+1, sizeof(double)); + ws_dec = (double*)calloc( M+1, sizeof(double)); + + for (n = 0; n < M; n++) { + w[n] = 7938/18608.0 - 9240/18608.0*cos(2*M_PI*n/(M-1)) + 1430/18608.0*cos(4*M_PI*n/(M-1)); // Blackmann + h[n] = 2*f*sinc(2*f*(n-(M-1)/2)); + ws_dec[n] = w[n]*h[n]; + norm += ws_dec[n]; + } + for (n = 0; n < M; n++) { + ws_dec[n] /= norm; + //fprintf(stderr, "%d %f %f %f\n", n, h[n], w[n], ws_dec[n]); + } + + free(h); h = NULL; + free(w); w = NULL; + + return M; +} + +int decimate_free() { + + if (ws_dec) { free(ws_dec); ws_dec = NULL; } + + return 0; +} + + +static float complex lowpass(float complex buffer[], int sample, int M) { + int n; + double complex w = 0; + for (n = 0; n < M; n++) { + w += buffer[(sample+n+1)%M]*ws_dec[M-1-n]; + } + return (float complex)w; +} + +int f32buf_sample(dsp_t *dsp, int inv) { + float s = 0.0; + float xneu, xalt; + + float complex z, w, z0; + double gain = 0.8; + + double t = dsp->sample_in / (double)dsp->sr; + + + if (dsp->opt_iq) { + + if (dsp->opt_iq == 5) { + int j; + if ( f32read_cblock(dsp) < dsp->decM ) return EOF; + for (j = 0; j < dsp->decM; j++) { + dsp->decXbuffer[dsp->sample_dec % dsp->dectaps] = dsp->decMbuf[j] * dsp->ex[dsp->sample_dec % (dsp->sr_base/res)]; + dsp->sample_dec += 1; + } + z = lowpass(dsp->decXbuffer, dsp->sample_dec, dsp->dectaps); + } + else 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; + dsp->rot_iqbuf[dsp->sample_in % dsp->N_IQBUF] = z; + + /* //if (rs_type==rs41) get_SNR(dsp); + // rs41, constant amplitude, avg/filter + int n; + double r = 0.0; + for (n = 0; n < dsp->sps; n++) r += cabs(dsp->rot_iqbuf[(dsp->sample_in - n + dsp->N_IQBUF) % dsp->N_IQBUF]); + r /= (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 X0 = 0; + float complex X = 0; + + int n = dsp->sps; + double tn = (dsp->sample_in-n) / (double)dsp->sr; + //t = dsp->sample_in / (double)dsp->sr; + //z = dsp->rot_iqbuf[dsp->sample_in % dsp->N_IQBUF]; + z0 = dsp->rot_iqbuf[(dsp->sample_in-n + dsp->N_IQBUF) % dsp->N_IQBUF]; + + // f1 + X0 = z0 * cexp(-tn*2*M_PI*f1*I); // alt + X = z * cexp(-t *2*M_PI*f1*I); // neu + dsp->F1sum += X - X0; + + // f2 + X0 = z0 * cexp(-tn*2*M_PI*f2*I); // alt + X = z * cexp(-t *2*M_PI*f2*I); // neu + dsp->F2sum += X - X0; + + xbit = cabs(dsp->F2sum) - cabs(dsp->F1sum); + + s = xbit / dsp->sps; + } + else if (0 && dsp->opt_iq >= 4) + { + double xbit = 0.0; + //float complex xi = cexp(+I*M_PI*dsp->h/dsp->sps); + double f1 = -dsp->h*dsp->sr/(2*dsp->sps); + double f2 = -f1; + + float complex X1 = 0; + float complex X2 = 0; + + int n = dsp->sps; + + while (n > 0) { + n--; + t = -n / (double)dsp->sr; + z = dsp->rot_iqbuf[(dsp->sample_in - n + dsp->N_IQBUF) % dsp->N_IQBUF]; // +1 + X1 += z*cexp(-t*2*M_PI*f1*I); + X2 += z*cexp(-t*2*M_PI*f2*I); + } + + xbit = cabs(X2) - cabs(X1); + + s = xbit / dsp->sps; + } + } + else { + if (f32read_sample(dsp, &s) == EOF) return EOF; + } + + if (inv) s = -s; + 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; + + if (dsp->opt_iq == 5) + { + dsp->decXbuffer = calloc( dsp->dectaps+1, sizeof(float complex)); + if (dsp->decXbuffer == NULL) return -1; + + dsp->decMbuf = calloc( dsp->decM+1, sizeof(float complex)); + if (dsp->decMbuf == NULL) return -1; + + dsp->ex = calloc(dsp->sr_base/res+1, sizeof(float complex)); + if (dsp->ex == NULL) return -1; + for (n = 0; n < dsp->sr_base/res; n++) { + t = (double)n*dsp->thd.xlt_fq; // xlt_fq=xltFq/sample_rate , integer xltFq frequency + dsp->ex[n] = cexp(t*2*M_PI*I); + } + } + + + 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; } + } + + + // decimate + if (dsp->opt_iq == 5) + { + if (dsp->decXbuffer) { free(dsp->decXbuffer); dsp->decXbuffer = NULL; } + if (dsp->decMbuf) { free(dsp->decMbuf); dsp->decMbuf = NULL; } + if (dsp->ex) { free(dsp->ex); dsp->ex = 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/multi/demod_base.h b/demod/multi/demod_base.h new file mode 100644 index 0000000..476148c --- /dev/null +++ b/demod/multi/demod_base.h @@ -0,0 +1,165 @@ + + +#include +#include +#include +#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; + + +#define MAX_FQ 5 +static int blk_sz = 32; // const + + +typedef struct { + int tn; + int tn_bit; + pthread_t tid; + pthread_mutex_t *mutex; + pthread_cond_t *cond; + //pthread_rwlock_t *lock; + int max_fq; + double xlt_fq; + float complex *blk; +} thd_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; + float complex F1sum; + float complex F2sum; + + // + 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; + + int sr_base; + int decM; + int dectaps; + int blk_cnt; + ui32_t sample_dec; + float complex *decXbuffer; + float complex *decMbuf; + float complex *ex; // exp_lut + thd_t thd; +} dsp_t; + + +typedef struct { + int sr; // sample_rate + int bps; // bits_sample bits/sample + int nch; // channels + int sel_ch; // select wav channel +// + int sr_base; + int decM; + int dectaps; + FILE *fp; +} pcm_t; + + + +typedef struct { + pcm_t pcm; + thd_t thd; +} thargs_t; + + + +float read_wav_header(pcm_t *); +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); + +int decimate_init(int taps, float f); +int decimate_free(void); + diff --git a/demod/multi/dfm09base.c b/demod/multi/dfm09base.c new file mode 100644 index 0000000..ec22fc5 --- /dev/null +++ b/demod/multi/dfm09base.c @@ -0,0 +1,1034 @@ + +/* + * dfm09 (dfm06) + * sync header: correlation/matched filter + * compile: + * gcc -c dfm09mod.c + * + * author: zilog80 + */ + +#include +#include +#include +#include + +/* +#ifdef CYGWIN + #include // cygwin: _setmode() + #include +#endif +*/ + +#include "demod_base.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 && ec == 0) { // mind. 5 Kanaele + if (bits2val(conf_bits+4, 4) == 0xC) { // 0xsCaaaab + gpx->snc.max_ch = conf_id; // reset? + } + } + + if (conf_id > 4 && (conf_id == (gpx->snc.nul_ch>>4)+1 || conf_id == gpx->snc.max_ch)) + { + 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 int print_gpx(gpx_t *gpx) { + int i, j; + int contgps = 0; + int output = 0; + int jsonout = 0; + int start = 0; + int ret = 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"); + } + + ret = 1; + } + + for (i = 0; i < 9; i++) gpx->pck[i].ec = -1; + + return ret; +} + +static int print_frame(gpx_t *gpx, dsp_t *dsp) { + 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) { + pthread_mutex_lock( dsp->thd.mutex ); + fprintf(stdout, "<%d> ", dsp->thd.tn); + ret1 = print_gpx(gpx); + if (ret1==0) fprintf(stdout, "\n"); + pthread_mutex_unlock( dsp->thd.mutex ); + } + } + if (ret2 == 0 || ret2 > 0) { + frid = dat_out(gpx, block_dat2, ret2); + if (frid == 8) { + pthread_mutex_lock( dsp->thd.mutex ); + fprintf(stdout, "<%d> ", dsp->thd.tn); + ret2 = print_gpx(gpx); + if (ret2==0) fprintf(stdout, "\n"); + pthread_mutex_unlock( dsp->thd.mutex ); + } + } + + } + 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; +} + + +void *thd_dfm09(void *targs) { + + + thargs_t *tharg = targs; + pcm_t *pcm = &(tharg->pcm); + + int option_iq = 5; + int option_bin = 0; + int spike = 0; + + FILE *fp = NULL; + char *fpname = NULL; + + int ret = 0; + int k; + + int bit; + int bitpos = 0; + int bitQ = 0; + 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; + + 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); +*/ + + // 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.vbs = 2; + gpx.option.ptu = 1; + gpx.option.ecc = 1; + gpx.option.aut = 1; + gpx.option.dst = 0; + gpx.option.jsn = 0; + + + headerlen = strlen(dfm_rawheader); + + + pcm->sel_ch = 0; + + // dfm: BT=1?, h=2.4? + symlen = 2; + + // init dsp + // + dsp.fp = pcm->fp; + dsp.sr = pcm->sr; + dsp.sr_base = pcm->sr_base; + dsp.dectaps = pcm->dectaps; + dsp.decM = pcm->decM; + + dsp.thd = tharg->thd; + + 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"); + } + + + k = init_buffers(&dsp); + if ( k < 0 ) { + fprintf(stderr, "error: init buffers\n"); + return NULL; + }; + + + bitofs += shift; + + + while ( 1 && bitQ != EOF ) + { + 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, &dsp); + if (pos < BITFRAME_LEN) break; + pos = 0; + frm += 1; + //if (ret < 0) frms += 1; + } + } + + header_found = 0; + pos = headerlen; + } + + free_buffers(&dsp); + + + return NULL; +} + diff --git a/demod/multi/rs41base.c b/demod/multi/rs41base.c new file mode 100644 index 0000000..ceba9ca --- /dev/null +++ b/demod/multi/rs41base.c @@ -0,0 +1,1705 @@ + +/* + * rs41 + * sync header: correlation/matched filter + * compile, either (a) or (b): + * (a) + * gcc -DINCLUDESTATIC -c rs41base.c + * (b) + * gcc -c demod_mod.c + * gcc -c rs41base.c + * + * author: zilog80 + */ + +#include +#include +#include +#include + +/* +#ifdef CYGWIN + #include // cygwin: _setmode() + #include +#endif +*/ + +#include "demod_base.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) + i8_t slt; // silent +} 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 vH; double vD; double vV; + 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 + ui32_t freq; // freq/kHz + float batt; // battery voltage (V) + ui16_t conf_fw; // firmware + ui16_t conf_kt; // kill timer (sec) + ui16_t conf_bt; // burst timer (sec) + ui16_t conf_cd; // kill countdown (sec) (kt or bt) + ui8_t conf_bk; // burst kill + 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; + // check only pck_type (variable len pcks 0x76, 0x7E) + 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_BattVolts 0x045 // 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_SGM_xTU 0x7F1B // temperature/humidity +#define pck_SGM_CRYPT 0x80A7 // 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; +} + +static int get_FrameNb(gpx_t *gpx, int ofs) { + int i; + unsigned byte; + ui8_t frnr_bytes[2]; + int frnr; + + for (i = 0; i < 2; i++) { + byte = gpx->frame[pos_FrameNb+ofs + i]; + frnr_bytes[i] = byte; + } + + frnr = frnr_bytes[0] + (frnr_bytes[1] << 8); + gpx->frnr = frnr; + + return 0; +} + +static int get_BattVolts(gpx_t *gpx, int ofs) { + int i; + unsigned byte; + ui8_t batt_bytes[2]; + float batt_volts; + + for (i = 0; i < 2; i++) { + byte = gpx->frame[pos_BattVolts+ofs + i]; + batt_bytes[i] = byte; + } + + batt_volts = (float)(batt_bytes[0] + (batt_bytes[1] << 8)); + gpx->batt = batt_volts/10.0; + + return 0; +} + +static int get_SondeID(gpx_t *gpx, int crc, int ofs) { + int i; + unsigned byte; + char sondeid_bytes[9]; + + if (crc == 0) { + for (i = 0; i < 8; i++) { + byte = gpx->frame[pos_SondeID+ofs + 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); // 0x00..0x32 + // reset conf data + memset(gpx->rstyp, 0, 9); + gpx->freq = 0; + gpx->conf_fw = 0; + gpx->conf_bt = 0; + gpx->conf_bk = 0; + gpx->conf_cd = -1; + gpx->conf_kt = -1; + // don't reset gpx->frame[] ! + gpx->T = -273.15; + gpx->RH = -1.0; + // new ID: + memcpy(gpx->id, sondeid_bytes, 8); + gpx->id[8] = '\0'; + } + } + + return 0; +} + +static int get_FrameConf(gpx_t *gpx, int ofs) { + int crc, err; + ui8_t calfr; + int i; + + crc = check_CRC(gpx, pos_FRAME+ofs, pck_FRAME); + if (crc) gpx->crc |= crc_FRAME; + + err = crc; + err |= get_SondeID(gpx, crc, ofs); + err |= get_FrameNb(gpx, ofs); + err |= get_BattVolts(gpx, ofs); + + if (crc == 0) { + calfr = gpx->frame[pos_CalData+ofs]; + if (gpx->calfrchk[calfr] == 0) // const? + { // 0x32 not constant + for (i = 0; i < 16; i++) { + gpx->calibytes[calfr*16 + i] = gpx->frame[pos_CalData+ofs+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/climate_environment/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 a0 = 7.5; // empirical + float a1 = 350.0/gpx->ptu_calH[0]; // empirical + float fh = (f-f1)/(float)(f2-f1); + float rh = 100.0 * ( a1*fh - a0 ); + float T0 = 0.0, T1 = -25.0; // T/C + rh += T0 - T/5.5; // empir. temperature compensation + if (T < T1) rh *= 1.0 + (T1-T)/90.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 ofs, int pck) { + 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+ofs, pck); + if (err) gpx->crc |= crc_PTU; + + if (err == 0) + { + // 0x7A2A: 16 byte (P)TU + // 0x7F1B: 12 byte _TU + for (i = 0; i < 12; i++) { + meas[i] = u3(gpx->frame+pos_PTU+ofs+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 ofs) { + int i; + unsigned byte; + ui8_t gpsweek_bytes[2]; + int gpsweek; + + for (i = 0; i < 2; i++) { + byte = gpx->frame[pos_GPSweek+ofs + 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 ofs) { + 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+ofs + 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 ofs) { + int err=0; + + // gpx->frame[pos_GPS1+1] != (pck_GPS1 & 0xFF) ? + err = check_CRC(gpx, pos_GPS1+ofs, pck_GPS1); + if (err) { + gpx->crc |= crc_GPS1; + // reset GPS1-data (json) + gpx->jahr = 0; gpx->monat = 0; gpx->tag = 0; + gpx->std = 0; gpx->min = 0; gpx->sek = 0.0; + return -1; + } + + err |= get_GPSweek(gpx, ofs); // no plausibility-check + err |= get_GPStime(gpx, ofs); // no plausibility-check + + return err; +} + +static int get_GPS2(gpx_t *gpx, int ofs) { + int err=0; + + // gpx->frame[pos_GPS2+1] != (pck_GPS2 & 0xFF) ? + err = check_CRC(gpx, pos_GPS2+ofs, 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 ofs) { + 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]; + double phi, lam, dir; + double vN; double vE; double vU; + + + for (k = 0; k < 3; k++) { + + for (i = 0; i < 4; i++) { + byte = gpx->frame[pos_GPSecefX+ofs + 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+ofs + 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; + vN = -V[0]*sin(phi)*cos(lam) - V[1]*sin(phi)*sin(lam) + V[2]*cos(phi); + vE = -V[0]*sin(lam) + V[1]*cos(lam); + vU = V[0]*cos(phi)*cos(lam) + V[1]*cos(phi)*sin(lam) + V[2]*sin(phi); + + // NEU -> HorDirVer + gpx->vH = sqrt(vN*vN+vE*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(vE, vN) * 180 / M_PI; + if (dir < 0) dir += 360; + gpx->vD = dir; + + gpx->vV = vU; + + gpx->numSV = gpx->frame[pos_numSats+ofs]; + + return 0; +} + +static int get_GPS3(gpx_t *gpx, int ofs) { + int err=0; + + // gpx->frame[pos_GPS3+1] != (pck_GPS3 & 0xFF) ? + err = check_CRC(gpx, pos_GPS3+ofs, pck_GPS3); + if (err) { + gpx->crc |= crc_GPS3; + // reset GPS3-data (json) + gpx->lat = 0.0; gpx->lon = 0.0; gpx->alt = 0.0; + gpx->vH = 0.0; gpx->vD = 0.0; gpx->vV = 0.0; + gpx->numSV = 0; + return -1; + } + + err |= get_GPSkoord(gpx, ofs); // plausibility-check: altitude, if ecef=(0,0,0) + + return err; +} + +static int get_Aux(gpx_t *gpx, int out, int pos) { +// +// "Ozone Sounding with Vaisala Radiosonde RS41" user's guide +// + int auxlen, auxcrc, count7E, pos7E; + int i, n; + + n = 0; + count7E = 0; + pos7E = 0; + //if (pos != pos_AUX) ; + gpx->xdata[0] = '\0'; + + if (frametype(gpx) <= 0) // pos7E == pos7611, 0x7E^0x76=0x08 ... + { + // 7Exx: xdata + while ( pos < FRAME_LEN && gpx->frame[pos] == 0x7E ) { + + auxlen = gpx->frame[pos+1]; + auxcrc = gpx->frame[pos+2+auxlen] | (gpx->frame[pos+2+auxlen+1]<<8); + + if ( auxcrc == crc16(gpx, pos+2, auxlen) ) { + if (count7E == 0) { + if (out) fprintf(stdout, "\n # xdata = "); + } + else { + if (out) 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[pos+2+i]; // (char) or better < 0x7F + if (c > 0x1E && c < 0x7F) { // ASCII-only + if (out) fprintf(stdout, "%c", c); + gpx->xdata[n++] = c; + } + } + count7E++; + pos7E = pos; + pos += 2+auxlen+2; + } + else { + pos = FRAME_LEN; + gpx->crc |= crc_AUX; + } + } + } + gpx->xdata[n] = '\0'; + + i = check_CRC(gpx, pos, pck_ZERO); // 0x76xx: 00-padding block + if (i) gpx->crc |= crc_ZERO; + + return pos7E; // count7E +} + +static int get_Calconf(gpx_t *gpx, int out, int ofs) { + 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+ofs]; + calfr = byte; + err = check_CRC(gpx, pos_FRAME+ofs, pck_FRAME); + + if (out && 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+ofs+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+ofs] & 0xC0; // erstmal nur oberste beiden bits + f0 = (byte * 10) / 64; // 0x80 -> 1/2, 0x40 -> 1/4 ; dann mal 40 + byte = gpx->frame[pos_Calfreq+ofs+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+ofs+6] | (gpx->frame[pos_CalData+ofs+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+ofs]; // fw >= 0x4ef5, burst-killtimer in 0x31 relevant + ui16_t kt = gpx->frame[pos_CalData+ofs+8] + (gpx->frame[pos_CalData+ofs+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+ofs+7] + (gpx->frame[pos_CalData+ofs+8] << 8); // burst timer (short?) + // fw >= 0x4ef5: default=[88 77]=0x7788sec=510min + if (out && bt != 0x0000 && + (gpx->option.vbs == 3 || gpx->option.vbs && gpx->conf_bk) + ) fprintf(stdout, ": bt %.1fmin ", bt/60.0); + gpx->conf_bt = bt; + } + + if (calfr == 0x32) { + ui16_t cd = gpx->frame[pos_CalData+ofs+1] + (gpx->frame[pos_CalData+ofs+2] << 8); // countdown (bt or kt) (short?) + if (out && cd != 0xFFFF && + (gpx->option.vbs == 3 || gpx->option.vbs && (gpx->conf_bk || gpx->conf_kt != 0xFFFF)) + ) fprintf(stdout, ": cd %.1fmin ", cd/60.0); + gpx->conf_cd = cd; // (short/i16_t) ? + } + + 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+ofs + 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); + if (out && gpx->option.vbs == 3) { // Stationsdruck QFE + float qfe1 = 0.0, qfe2 = 0.0; + memcpy(&qfe1, gpx->frame+pos_CalData+1, 4); + memcpy(&qfe2, gpx->frame+pos_CalData+5, 4); + if (qfe1 > 0.0 || qfe2 > 0.0) { + fprintf(stdout, " "); + if (qfe1 > 0.0) fprintf(stdout, "QFE1:%.1fhPa ", qfe1); + if (qfe2 > 0.0) fprintf(stdout, "QFE2:%.1fhPa ", qfe2); + } + } + } + } + + 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 prn_frm(gpx_t *gpx) { + fprintf(stdout, "[%5d] ", gpx->frnr); + fprintf(stdout, "(%s) ", gpx->id); + if (gpx->option.vbs == 3) fprintf(stdout, "(%.1f V) ", gpx->batt); + fprintf(stdout, " "); + return 0; +} + +static int prn_ptu(gpx_t *gpx) { + fprintf(stdout, " "); + if (gpx->T > -273.0) fprintf(stdout, " T=%.1fC ", gpx->T); + if (gpx->RH > -0.5) fprintf(stdout, " RH=%.0f%% ", gpx->RH); + return 0; +} + +static int prn_gpstime(gpx_t *gpx) { + 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); + fprintf(stdout, " "); + return 0; +} + +static int prn_gpspos(gpx_t *gpx) { + //fprintf(stdout, " "); + fprintf(stdout, " lat: %.5f ", gpx->lat); + fprintf(stdout, " lon: %.5f ", gpx->lon); + fprintf(stdout, " alt: %.2f ", gpx->alt); + fprintf(stdout, " vH: %4.1f D: %5.1f vV: %3.1f ", gpx->vH, gpx->vD, gpx->vV); + if (gpx->option.vbs == 3) fprintf(stdout, " sats: %02d ", gpx->numSV); + return 0; +} + +static int prn_sat1(gpx_t *gpx, int ofs) { + + fprintf(stdout, "\n"); + + fprintf(stdout, "iTOW: 0x%08X", u4(gpx->frame+pos_GPSiTOW+ofs)); + fprintf(stdout, " week: 0x%04X", u2(gpx->frame+pos_GPSweek+ofs)); + + return 0; +} +const double c = 299.792458e6; +const double L1 = 1575.42e6; +static int prn_sat2(gpx_t *gpx, int ofs) { + int i, n; + int sv; + ui32_t minPR; + + fprintf(stdout, "\n"); + + minPR = u4(gpx->frame+pos_minPR+ofs); + fprintf(stdout, "minPR: %d", minPR); + fprintf(stdout, "\n"); + + for (i = 0; i < 12; i++) { + n = i*7; + sv = gpx->frame[pos_satsN+ofs+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+ofs+n)/100.0 + minPR); + fprintf(stdout, " "); + fprintf(stdout, "doMes: %.1f", -i3(gpx->frame+pos_dataSats+ofs+n+4)/100.0*L1/c); + fprintf(stdout, "\n"); + } + + return 0; +} +static int prn_sat3(gpx_t *gpx, int ofs) { + int numSV; + double pDOP, sAcc; + + fprintf(stdout, "\n"); + + fprintf(stdout, "ECEF-POS: (%d,%d,%d)\n", + (i32_t)u4(gpx->frame+pos_GPSecefX+ofs), + (i32_t)u4(gpx->frame+pos_GPSecefY+ofs), + (i32_t)u4(gpx->frame+pos_GPSecefZ+ofs)); + fprintf(stdout, "ECEF-VEL: (%d,%d,%d)\n", + (i16_t)u2(gpx->frame+pos_GPSecefV+ofs+0), + (i16_t)u2(gpx->frame+pos_GPSecefV+ofs+2), + (i16_t)u2(gpx->frame+pos_GPSecefV+ofs+4)); + + numSV = gpx->frame[pos_numSats+ofs]; + sAcc = gpx->frame[pos_sAcc+ofs]/10.0; if (gpx->frame[pos_sAcc+ofs] == 0xFF) sAcc = -1.0; + pDOP = gpx->frame[pos_pDOP+ofs]/10.0; if (gpx->frame[pos_pDOP+ofs] == 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+ofs, 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+ofs, 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+ofs, pck_GPS3)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); + //fprintf(stdout, "[%+d]", check_CRC(gpx, pos_GPS3, pck_GPS3)); + + fprintf(stdout, "\n"); +*/ + return 0; +} + +static int print_position(gpx_t *gpx, int ec) { + int i, j; + int err, err0, err1, err2, err3; + //int output, out_mask; + int encrypted = 0; + int unexp = 0; + int out = 1; + int sat = 0; + int pos_aux = 0, cnt_aux = 0; + int ret = 0; + + //gpx->out = 0; + gpx->aux = 0; + + if (gpx->option.sat) sat = 1; + if (gpx->option.slt) out = 0; else out = 1; + + if ( ec >= 0 ) + { + int pos, blk, len, crc, pck; + int flen = NDATA_LEN; + + int ofs_cal = 0; + int frm_end = NDATA_LEN-2; + + if (frametype(gpx) < 0) flen += XDATA_LEN; + + switch (gpx->frame[pos_PTU]) { + case 0x7A: // 0x7A2A + frm_end = flen-2; + break; + case 0x7F: // 0x7F1B + frm_end = pos_ZEROstd + 0x1B-0x2A - 2; + break; + case 0x80: // 0x80A7 + frm_end = pos_PTU + 2 + 0xA7; + break; + } + + pos = pos_FRAME; + gpx->crc = 0; + + while (pos < flen-1) { + blk = gpx->frame[pos]; + len = gpx->frame[pos+1]; + crc = check_CRC(gpx, pos, blk<<8); + pck = (blk<<8) | len; + + if ( crc == 0 ) // ecc-OK -> crc-OK + { + int ofs = 0; + switch (pck) + { + case pck_FRAME: // 0x7928 + ofs = pos - pos_FRAME; + ofs_cal = ofs; + err = get_FrameConf(gpx, ofs); + if ( !err ) { + if (out || sat) prn_frm(gpx); + } + break; + + case pck_PTU: // 0x7A2A + ofs = pos - pos_PTU; + err0 = get_PTU(gpx, ofs, pck_PTU); + if ( 0 && !err0 && gpx->option.ptu ) { + prn_ptu(gpx); + } + break; + + case pck_GPS1: // 0x7C1E + ofs = pos - pos_GPS1; + err1 = get_GPS1(gpx, ofs); + if ( !err1 ) { + if (out) prn_gpstime(gpx); + if (sat) prn_sat1(gpx, ofs); + } + break; + + case pck_GPS2: // 0x7D59 + ofs = pos - pos_GPS2; + err2 = get_GPS2(gpx, ofs); + if ( !err2 ) { + if (sat) prn_sat2(gpx, ofs); + } + break; + + case pck_GPS3: // 0x7B15 + ofs = pos - pos_GPS3; + err3 = get_GPS3(gpx, ofs); + if ( !err3 ) { + if (out) prn_gpspos(gpx); + if (sat) prn_sat3(gpx, ofs); + } + break; + + case pck_SGM_xTU: // 0x7F1B + ofs = pos - pos_PTU; + err0 = get_PTU(gpx, ofs, pck); + break; + + case pck_SGM_CRYPT: // 0x80A7 + encrypted = 1; + if (out) fprintf(stdout, " [%04X] (RS41-SGM) ", pck_SGM_CRYPT); + break; + + default: + if (blk == 0x7E) { + if (pos_aux == 0) pos_aux = pos; // pos == pos_AUX ? + cnt_aux += 1; + } + if (blk == 0x76) { + // ZERO-Padding pck + } + + if (blk != 0x76 && blk != 0x7E) { + if (out) fprintf(stdout, " [%04X] ", pck); + unexp = 1; + } + } + } + else { // CRC-ERROR (ECC-OK) + fprintf(stdout, " [ERROR]\n"); + break; + } + + pos += 2+len+2; // next pck + + if ( pos > frm_end ) // end of (sub)frame + { + if (gpx->option.ptu && out && !sat && !err0 && !encrypted) { + prn_ptu(gpx); + } + + get_Calconf(gpx, out, ofs_cal); + + if (out && ec > 0 && pos > flen-1) fprintf(stdout, " (%d)", ec); + + if (pos_aux) gpx->aux = get_Aux(gpx, out && gpx->option.vbs > 1, pos_aux); + + gpx->crc = 0; + frm_end = FRAME_LEN-2; + + + if (out || sat) fprintf(stdout, "\n"); + + + 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, \"bt\": %d, \"batt\": %.2f", + 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->vV, gpx->numSV, gpx->conf_cd, gpx->batt ); + 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) { + fprintf(stdout, ", \"subtype\": \"RS41-SGM\", \"encrypted\": true"); + } else { + fprintf(stdout, ", \"subtype\": \"%s\"", *gpx->rstyp ? gpx->rstyp : "RS41" ); // RS41-SG(P/M) + if (strncmp(gpx->rstyp, "RS41-SGM", 8) == 0) { + fprintf(stdout, ", \"encrypted\": false"); + } + } + fprintf(stdout, " }\n"); + fprintf(stdout, "\n"); + } + } + } + } + ret = 1; + } + // else + if (ec < 0 && (out || sat /*|| gpx->option.jsn*/)) { + // + // crc-OK pcks ? + // + int pck, ofs; + int output = 0, out_mask; + + gpx->crc = 0; + out_mask = crc_FRAME|crc_GPS1|crc_GPS3; + if (gpx->option.ptu) out_mask |= crc_PTU; + + err = get_FrameConf(gpx, 0); + if (out && !err) prn_frm(gpx); + + pck = (gpx->frame[pos_PTU]<<8) | gpx->frame[pos_PTU+1]; + ofs = 0; + + if (pck < 0x8000) { + err0 = get_PTU(gpx, 0, pck); + if (pck == pck_PTU) ofs = 0; + else if (pck == pck_SGM_xTU) ofs = 0x1B-0x2A; + + err1 = get_GPS1(gpx, ofs); + err2 = get_GPS2(gpx, ofs); + err3 = get_GPS3(gpx, ofs); + + if (out) { + + if (!err1) prn_gpstime(gpx); + if (!err3) prn_gpspos(gpx); + if (!err0 && gpx->option.ptu) prn_ptu(gpx); + if (0 && !err) get_Calconf(gpx, out, 0); // only if ecc-OK + + output = ((gpx->crc & out_mask) != out_mask); + + if (output) { + fprintf(stdout, " "); + fprintf(stdout, "["); + for (i=0; i<5; i++) fprintf(stdout, "%d", (gpx->crc>>i)&1); + fprintf(stdout, "]"); + } + } + } + else if (pck == pck_SGM_CRYPT) { + if (out && !err) { + fprintf(stdout, " [%04X] (RS41-SGM) ", pck_SGM_CRYPT); + //fprintf(stdout, "[%d] ", check_CRC(gpx, pos_PTU, pck_SGM_CRYPT)); + output = 1; + } + } + + if (out && output) + { + if (ec == -1) fprintf(stdout, " (-+)"); + else if (ec == -2) fprintf(stdout, " (+-)"); + else /*ec == -3*/ fprintf(stdout, " (--)"); + + fprintf(stdout, "\n"); // fflush(stdout); + } + + ret = output; + } + + + return ret; +} + +static void print_frame(gpx_t *gpx, int len, dsp_t *dsp) { + int i, ec = 0, ft; + int ret = 0; + + 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 { + pthread_mutex_lock( dsp->thd.mutex ); + fprintf(stdout, "<%d> ", dsp->thd.tn); + ret = print_position(gpx, ec); + if (ret==0) fprintf(stdout, "\n"); + pthread_mutex_unlock( dsp->thd.mutex ); + } +} + +/* -------------------------------------------------------------------------- */ + + +// 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; +} + + +void *thd_rs41(void *targs) { // pcm_t *pcm, double xlt_fq + + thargs_t *tharg = targs; + pcm_t *pcm = &(tharg->pcm); + + + //int option_inv = 0; // invertiert Signal + int option_iq = 5; // baseband, decimate + int option_ofs = 0; + int option_bin = 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 = 0; + + 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; + + 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); +*/ + + gpx.option.vbs = 1; + gpx.option.ptu = 1; + gpx.option.aut = 1; + gpx.option.jsn = 0; + + 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 (option_iq) sel_wavch = 0; + + pcm->sel_ch = 0; //sel_wavch; + + // rs41: BT=0.5, h=0.8,1.0 ? + symlen = 1; + + // init dsp + // + dsp.fp = pcm->fp; + dsp.sr = pcm->sr; + dsp.sr_base = pcm->sr_base; + dsp.dectaps = pcm->dectaps; + dsp.decM = pcm->decM; + + dsp.thd = tharg->thd; +/* + thread_struct->tn; + dsp.mutex = thread_struct->mutex; + dsp.cond = thread_struct->cond; + dsp.xlt_fq = thread_struct->xlt_fq; + dsp.max_fq = thread_struct->max_fq; + dsp.blk = thread_struct->blk; +*/ + 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.6; //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); + } + + + k = init_buffers(&dsp); // BT=0.5 (IQ-Int: BT > 0.5 ?) + if ( k < 0 ) { + fprintf(stderr, "error: init buffers\n"); + return NULL; + }; + + //if (option_iq >= 2) bitofs += 1; // FM: +1 , IQ: +2 + bitofs += shift; + +//fprintf(stderr, "%d\n", dsp.tn); + + while ( 1 && bitQ != EOF ) + { + 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_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, wenn nicht nochmal 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 ^ mask[byte_count % MASK_LEN]; + byte_count++; + } + } + + print_frame(&gpx, byte_count, &dsp); + byte_count = FRAMESTART; + header_found = 0; + } + } + + free_buffers(&dsp); + + + return NULL; +} + diff --git a/demod/multi/rs_multi.c b/demod/multi/rs_multi.c new file mode 100644 index 0000000..0affbe7 --- /dev/null +++ b/demod/multi/rs_multi.c @@ -0,0 +1,206 @@ + +/* +gcc -O2 -c demod_base.c +gcc -O2 -c bch_ecc_mod.c +gcc -O2 -c rs41base.c +gcc -O2 -c dfm09base.c +gcc -O2 rs_multi.c demod_base.o bch_ecc_mod.o rs41base.o dfm09base.o -lm -pthread + +./a.out --rs41 [--dfm ..] baseband_IQ.wav +-0.5 < fq < 0.5 +*/ + + +#include + +#ifdef CYGWIN + #include // cygwin: _setmode() + #include +#endif + +#include "demod_base.h" + + +static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_cond_t cond = PTHREAD_COND_INITIALIZER; +//static pthread_rwlock_t lock = PTHREAD_RWLOCK_INITIALIZER; + +static float complex *block_decMB; + +int rbf1; // extern in demod_base.c + + +void *thd_rs41(void *); +void *thd_dfm09(void *); + + +#define IF_SAMPLE_RATE 48000 + +static int pcm_dec_init(pcm_t *p) { + + int IF_sr = IF_SAMPLE_RATE; // designated IF sample rate + int decM = 1; // decimate M:1 + int sr_base = p->sr; + float f_lp; // dec_lowpass: lowpass_bandwidth/2 + float tbw; // dec_lowpass: transition_bandwidth/Hz + int taps; // dec_lowpass: taps + + if (IF_sr > sr_base) IF_sr = sr_base; + if (IF_sr < sr_base) { + while (sr_base % IF_sr) IF_sr += 1; + decM = sr_base / IF_sr; + } + + f_lp = (IF_sr+20e3)/(4.0*sr_base); + tbw = (IF_sr-20e3)/*/2.0*/; if (tbw < 0) tbw = 8e3; + taps = sr_base*4.0/tbw; if (taps%2==0) taps++; + + taps = decimate_init(taps, f_lp); + + p->dectaps = taps; + p->sr_base = sr_base; + p->sr = IF_sr; // sr_base/decM + p->decM = decM; + + + fprintf(stderr, "IF: %d\n", IF_sr); + fprintf(stderr, "dec: %d\n", decM); + + fprintf(stderr, "taps: %d\n", taps); + fprintf(stderr, "transBW: %.4f = %.1f Hz\n", tbw/sr_base, tbw); + fprintf(stderr, "f: +/-%.4f = +/-%.1f Hz\n", f_lp, f_lp*sr_base); + + return 0; +} + + +int main(int argc, char **argv) { + + FILE *fp; + int wavloaded = 0; + int k; + int xlt_cnt = 0; + double base_fqs[MAX_FQ]; + void *rstype[MAX_FQ]; + int option_pcmraw = 0; + +#ifdef CYGWIN + _setmode(fileno(stdin), _O_BINARY); // _fileno(stdin) +#endif + setbuf(stdout, NULL); + + + pcm_t pcm = {0}; + + for (k = 0; k < MAX_FQ; k++) base_fqs[k] = 0.0; + + ++argv; + while ((*argv) && (!wavloaded)) { + if (strcmp(*argv, "--rs41") == 0) { + double fq = 0.0; + ++argv; + if (*argv) fq = atof(*argv); + else return -1; + if (fq < -0.5) fq = -0.5; + if (fq > 0.5) fq = 0.5; + if (xlt_cnt < MAX_FQ) { + base_fqs[xlt_cnt] = fq; + rstype[xlt_cnt] = thd_rs41; + xlt_cnt++; + } + } + else if (strcmp(*argv, "--dfm") == 0) { + double fq = 0.0; + ++argv; + if (*argv) fq = atof(*argv); + else return -1; + if (fq < -0.5) fq = -0.5; + if (fq > 0.5) fq = 0.5; + if (xlt_cnt < MAX_FQ) { + base_fqs[xlt_cnt] = fq; + rstype[xlt_cnt] = thd_dfm09; + xlt_cnt++; + } + } + else if (strcmp(*argv, "-") == 0) { + int sample_rate = 0, bits_sample = 0, channels = 0; + ++argv; + if (*argv) sample_rate = atoi(*argv); else return -1; + ++argv; + if (*argv) bits_sample = atoi(*argv); else return -1; + channels = 2; + if (sample_rate < 1 || (bits_sample != 8 && bits_sample != 16 /*&&bits_sample!=32*/)) { + fprintf(stderr, "- \n"); + return -1; + } + pcm.sr = sample_rate; + pcm.bps = bits_sample; + pcm.nch = channels; + option_pcmraw = 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; + + pcm.fp = fp; + if (option_pcmraw == 0) { + k = read_wav_header( &pcm ); + if ( k < 0 ) { + fclose(fp); + fprintf(stderr, "error: wav header\n"); + return -1; + } + } + if (pcm.nch < 2) { + fprintf(stderr, "error: iq channels < 2\n"); + return -50; + } + + pcm_dec_init( &pcm ); + + + block_decMB = calloc(pcm.decM*blk_sz+1, sizeof(float complex)); if (block_decMB == NULL) return -1; + + + thargs_t tharg[xlt_cnt]; + + for (k = 0; k < xlt_cnt; k++) { + tharg[k].thd.tn = k; + tharg[k].thd.tn_bit = (1< IF (rotate from and decimate) + tharg[k].pcm = pcm; + + rbf1 |= tharg[k].thd.tn_bit; + } + + for (k = 0; k < xlt_cnt; k++) { + pthread_create(&tharg[k].thd.tid, NULL, rstype[k], &tharg[k]); + } + + + for (k = 0; k < xlt_cnt; k++) { + pthread_join(tharg[k].thd.tid, NULL); + } + + + if (block_decMB) { free(block_decMB); block_decMB = NULL; } + decimate_free(); + + fclose(fp); + + return 0; +} +