diff --git a/rs_module/README.md b/rs_module/README.md new file mode 100755 index 0000000..577892a --- /dev/null +++ b/rs_module/README.md @@ -0,0 +1,27 @@ + +##### separate modules + +preliminary/test version (rs41, rs92) + +compile: + +``` + +gcc -c rs_datum.c +gcc -c rs_demod.c +gcc -c rs_bch_ecc.c + +gcc -c rs_rs41.c +gcc -c rs_rs92.c + + +gcc -c rs_main41.c +gcc rs_main41.o rs_rs41.o rs_bch_ecc.o rs_demod.o rs_datum.o -lm -o rs41mod + + +gcc -c rs_main92.c +gcc rs_main92.o rs_rs92.o rs_bch_ecc.o rs_demod.o rs_datum.o -lm -o rs92mod + +``` + + diff --git a/rs_module/rs_bch_ecc.c b/rs_module/rs_bch_ecc.c new file mode 100644 index 0000000..362e7fd --- /dev/null +++ b/rs_module/rs_bch_ecc.c @@ -0,0 +1,536 @@ + +/* + * BCH / Reed-Solomon + * encoder() + * decoder() (Euklid. Alg.) + * + * + * author: zilog80 + * + * cf. RS/ecc/bch_ecc.c + * + + Vaisala RS92, RS41: + RS(255, 231), t=12 + g(X) = (X-alpha^0)...(X-alpha^(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 "rs_data.h" +#include "rs_bch_ecc.h" + + +#define MAX_DEG 254 // max N-1 + + +typedef struct { + ui32_t f; + ui32_t ord; + ui8_t alpha; +} GF_t; + +static GF_t GF256RS = { 0x11D, // RS-GF(2^8): X^8 + X^4 + X^3 + X^2 + 1 : 0x11D + 256, // 2^8 + 0x02 }; // generator: alpha = X + +static GF_t GF64BCH = { 0x43, // BCH-GF(2^6): X^6 + X + 1 : 0x43 + 64, // 2^6 + 0x02 }; // generator: alpha = X +/* +static GF_t GF16RS = { 0x13, // RS-GF(2^4): X^4 + X + 1 : 0x13 + 16, // 2^4 + 0x02 }; // generator: alpha = X + +static GF_t GF256AES = { 0x11B, // AES-GF(2^8): X^8 + X^4 + X^3 + X + 1 : 0x11B + 256, // 2^8 + 0x03 }; // generator: alpha = X+1 +*/ + +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 g[MAX_DEG+1]; // ohne g[] eventuell als init_return +} RS_t; + + +static RS_t RS256 = { 255, 12, 24, 231, 0, {0}}; +static RS_t BCH64 = { 63, 2, 12, 51, 1, {0}}; + + +static GF_t GF; +static RS_t RS; + +static ui8_t exp_a[256], + log_a[256]; + + +/* --------------------------------------------------------------------------------------------- */ + +static +int GF_deg(ui32_t p) { + ui32_t d = 31; + if (p == 0) return -1; /* deg(0) = -infty */ + else { + while (d && !(p & (1<>= 1; + if (b & 1) ab ^= (ui8_t)aa; /* b_{i+1} > 0 ? */ + } + return ab; +} + +static +int GF_genTab(GF_t gf, ui8_t expa[], ui8_t loga[]) { + int i, j; + ui8_t b; + +// GF.f = f; +// GF.ord = 1 << GF_deg(GF.f); + + b = 0x01; + for (i = 0; i < gf.ord; i++) { + expa[i] = b; // b_i = a^i + b = GF2m_mul(gf.alpha, b); + } + + loga[0] = -00; // log(0) = -inf + for (i = 1; i < gf.ord; i++) { + b = 0x01; j = 0; + while (b != i) { + b = GF2m_mul(gf.alpha, b); + j++; + if (j > gf.ord-1) { + return -1; // a not primitive + } + } // j = log_a(i) + loga[i] = j; + } + + return 0; +} + + +static ui8_t GF_mul(ui8_t p, ui8_t q) { + ui32_t x; + if ((p == 0) || (q == 0)) return 0; + x = (ui32_t)log_a[p] + log_a[q]; + return exp_a[x % (GF.ord-1)]; // a^(ord-1) = 1 +} + +static ui8_t GF_inv(ui8_t p) { + if (p == 0) return 0; // DIV_BY_ZERO + return exp_a[GF.ord-1-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(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 = exp_a[(n*log_a[x]) % (GF.ord-1)]; + y ^= GF_mul(poly[n], xn); + } + } + 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(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; // 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( p[deg_p], GF_inv(q[deg_q])); + + if (deg_q == 0) { + for (i = 0; i <= deg_p; i++) d[i] = GF_mul(p[i], c); + for (i = 0; i <= MAX_DEG; i++) r[i] = 0; + } + else if (deg_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( 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( r[deg_p], GF_inv(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(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(a[i], b[j]); + } + } + + for (i = 0; i <= MAX_DEG; i++) { ab[i] = c[i]; } + + return 0; +} + + +static +int polyGF_lfsr(int deg, ui8_t S[], + ui8_t *Lambda, ui8_t *Omega ) { +// BCH/RS/LFSR: deg=t, +// 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[2*deg] = 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 ) { + poly_divmod(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(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(ui8_t x, ui8_t Omega[], ui8_t Lambda[]) { + 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(Omega, x); + z = poly_eval(DLam, x); + Y = GF_mul(w, GF_inv(z)); + if (RS.b == 0) Y = GF_mul(GF_inv(x), Y); + else if (RS.b > 1) { + ui8_t xb1 = exp_a[((RS.b-1)*log_a[x]) % (GF.ord-1)]; + Y = GF_mul(xb1, Y); + } + + return Y; +} + + +int rs_init_RS255() { + int i, check_gen; + ui8_t Xalp[MAX_DEG+1]; + + GF = GF256RS; + check_gen = GF_genTab( GF, exp_a, log_a); + + RS = RS256; // N=255, t=12, b=0 + 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] = exp_a[(RS.b+i) % (GF.ord-1)]; // Xalp[0..1]: X - alpha^(b+i) + poly_mul(RS.g, Xalp, RS.g); + } + + return check_gen; +} + +int rs_init_BCH64() { + int i, check_gen; + + GF = GF64BCH; + check_gen = GF_genTab( GF, exp_a, log_a); + + RS = BCH64; // N=63, t=2, b=1 + 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; +} + +static +int syndromes(ui8_t cw[], ui8_t *S) { + int i, errors = 0; + ui8_t a_i; + + // syndromes: e_j=S(alpha^(b+i)) + for (i = 0; i < 2*RS.t; i++) { + a_i = exp_a[(RS.b+i) % (GF.ord-1)]; // alpha^(b+i) + S[i] = poly_eval(cw, a_i); + if (S[i]) errors = 1; + } + return errors; +} + +int rs_encode(ui8_t cw[]) { + int j; + ui8_t parity[MAX_DEG+1], + d[MAX_DEG+1]; + for (j = 0; j < RS.R; j++) cw[j] = 0; + for (j = 0; j <=MAX_DEG; j++) parity[j] = 0; + poly_divmod(cw, RS.g, d, parity); + //if (poly_deg(parity) >= RS.R) return -1; + for (j = 0; j <= poly_deg(parity); j++) cw[j] = parity[j]; + return 0; +} + +int rs_decode(ui8_t cw[], ui8_t *err_pos, ui8_t *err_val) { + ui8_t x, gamma, + S[MAX_DEG+1], + 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(cw, S); + // wenn S(x)=0 , dann poly_divmod(cw, RS.g, d, rem): rem=0 + + if (errors) { + polyGF_lfsr(RS.t, S, Lambda, Omega); + gamma = Lambda[0]; + if (gamma) { + for (i = poly_deg(Lambda); i >= 0; i--) Lambda[i] = GF_mul(Lambda[i], GF_inv(gamma)); + for (i = poly_deg(Omega) ; i >= 0; i--) Omega[i] = GF_mul( Omega[i], GF_inv(gamma)); + } + else { + 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(Lambda, x) == 0) { + // error location index + err_pos[n] = log_a[GF_inv(x)]; + // error value; bin-BCH: err_val=1 + err_val[n] = 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; +} + + +int rs_decode_bch_gf2t2(ui8_t cw[], ui8_t *err_pos, ui8_t *err_val) { +// binary 2-error correcting BCH + + 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(cw, S); + // wenn S(x)=0 , dann poly_divmod(cw, RS.g, d, rem): rem=0 + + if (errors) { + polyGF_lfsr(RS.t, S, Lambda, Omega); + gamma = Lambda[0]; + if (gamma) { + for (i = poly_deg(Lambda); i >= 0; i--) Lambda[i] = GF_mul(Lambda[i], GF_inv(gamma)); + for (i = poly_deg(Omega) ; i >= 0; i--) Omega[i] = GF_mul( Omega[i], GF_inv(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(S[0], S[0]); L2 = GF_mul(L2, S[0]); L2 ^= S[2]; + L2 = GF_mul(L2, GF_inv(S[0])); + L[2] = L2; + + if (S[1] != GF_mul(S[0],S[0]) || S[3] != GF_mul(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(Lambda, x) == 0) { + // error location index + err_pos[n] = log_a[GF_inv(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/rs_module/rs_bch_ecc.h b/rs_module/rs_bch_ecc.h new file mode 100644 index 0000000..ad12826 --- /dev/null +++ b/rs_module/rs_bch_ecc.h @@ -0,0 +1,14 @@ + +#ifndef RS_BCH_ECC_H +#define RS_BCH_ECC_H + + +int rs_init_RS255(void); +int rs_init_BCH64(void); +int rs_encode(ui8_t *cw); +int rs_decode(ui8_t *cw, ui8_t *, ui8_t *); +int rs_decode_bch_gf2t2(ui8_t *cw, ui8_t *, ui8_t *); + + +#endif /* RS_BCH_ECC_H */ + diff --git a/rs_module/rs_data.h b/rs_module/rs_data.h new file mode 100644 index 0000000..40e4b9f --- /dev/null +++ b/rs_module/rs_data.h @@ -0,0 +1,102 @@ + +#ifndef RS_DATA_H +#define RS_DATA_H + + +typedef unsigned char ui8_t; +typedef unsigned short ui16_t; +typedef unsigned int ui32_t; +typedef short i16_t; +typedef int i32_t; + + +typedef struct { + int week; int msec; + double lat; double lon; double alt; + double vN; double vE; double vU; + double vH; double vD; +} GPS_t; + +typedef struct { + double P; + double T; + double H1; + double H2; +} PTU_t; + +typedef struct { + + char SN[12]; + int frnr; + int freq; + int year; int month; int day; + int wday; + int hr; int min; float sec; + + GPS_t GPS; + PTU_t PTU; + + ui32_t crc; + int ecc; + + int header_ofs; + int header_len; + int bufpos; + char *buf; + char *header; + + int baud; + int bits; + float samples_per_bit; + + char *frame_rawbits; + char *frame_bits; + ui8_t *frame_bytes; + ui32_t frame_start; + ui32_t pos; + ui32_t pos_min; + ui32_t frame_len; + + int (*bits2byte)(void *, char *); + int (*rs_process)(void *, int, int); + int input; + + void *addData; + +} rs_data_t; + +typedef struct { + ui32_t tow; + int prn[12]; + double pseudorange[12]; + double doppler[12]; + ui8_t status[12]; + double pos_ecef[3]; + double vel_ecef[3]; + ui8_t Nfix; + double pDOP; + double sAcc; +} sat_t; + +typedef struct { + char SN[12]; + ui8_t bytes[0x33][16+1]; + sat_t sat; +} addData_Vaisala_t; + +typedef struct { + int typ; + int msglen; + int msgpos; + int parpos; + int hdrlen; + int frmlen; +} rs_ecccfg_t; + + +#define ERROR_MALLOC -1 + + + +#endif /* RS_DATA_H */ + diff --git a/rs_module/rs_datum.c b/rs_module/rs_datum.c new file mode 100644 index 0000000..2f9c08f --- /dev/null +++ b/rs_module/rs_datum.c @@ -0,0 +1,35 @@ + +#include "rs_data.h" +#include "rs_datum.h" + +char weekday[7][4] = { "Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat"}; +//char weekday[7][3] = { "So", "Mo", "Di", "Mi", "Do", "Fr", "Sa"}; + +/* + * Convert GPS Week and Seconds to Modified Julian Day. + * - Adapted from sci.astro FAQ. + * - Ignores UTC leap seconds. + */ +//void Gps2Date(long GpsWeek, long GpsSeconds, int *Year, int *Month, int *Day) { +void Gps2Date(rs_data_t *rs_data) { + + long GpsSeconds, GpsDays, Mjd; + long J, C, Y, M; + + GpsSeconds = (rs_data->GPS).msec / 1000; + GpsDays = (rs_data->GPS).week * 7 + (GpsSeconds / 86400); + Mjd = 44244 + GpsDays; + + J = Mjd + 2468570; + C = 4 * J / 146097; + J = J - (146097 * C + 3) / 4; + Y = 4000 * (J + 1) / 1461001; + J = J - 1461 * Y / 4 + 31; + M = 80 * J / 2447; + rs_data->day = J - 2447 * M / 80; + J = M / 11; + rs_data->month = M + 2 - (12 * J); + rs_data->year = 100 * (C - 49) + Y + J; +} + + diff --git a/rs_module/rs_datum.h b/rs_module/rs_datum.h new file mode 100644 index 0000000..ce53e05 --- /dev/null +++ b/rs_module/rs_datum.h @@ -0,0 +1,12 @@ + +#ifndef RS_DATUM_H +#define RS_DATUM_H + + +char weekday[7][4]; + +void Gps2Date(rs_data_t *); + + +#endif /* RS_DATUM_H */ + diff --git a/rs_module/rs_demod.c b/rs_module/rs_demod.c new file mode 100644 index 0000000..dc4f72a --- /dev/null +++ b/rs_module/rs_demod.c @@ -0,0 +1,173 @@ + +#include +#include + +#include "rs_data.h" +#include "rs_demod.h" + + +static int sample_rate = 0, + bits_sample = 0, + channels = 0; +static float samples_per_bit = 0.0; + +static int findstr(char *buff, char *str, int pos) { + int i; + for (i = 0; i < 4; i++) { + if (buff[(pos+i)%4] != str[i]) break; + } + return i; +} + +int read_wav_header(FILE *fp, rs_data_t *rs_data) { + char txt[4+1] = "\0\0\0\0"; + unsigned char dat[4]; + int byte, p=0; + + if (fread(txt, 1, 4, fp) < 4) return -1; + if (strncmp(txt, "RIFF", 4)) return -1; + if (fread(txt, 1, 4, fp) < 4) return -1; + // pos_WAVE = 8L + if (fread(txt, 1, 4, fp) < 4) return -1; + if (strncmp(txt, "WAVE", 4)) return -1; + // pos_fmt = 12L + for ( ; ; ) { + if ( (byte=fgetc(fp)) == EOF ) return -1; + txt[p % 4] = byte; + p++; if (p==4) p=0; + if (findstr(txt, "fmt ", p) == 4) break; + } + if (fread(dat, 1, 4, fp) < 4) return -1; + if (fread(dat, 1, 2, fp) < 2) return -1; + + if (fread(dat, 1, 2, fp) < 2) return -1; + channels = dat[0] + (dat[1] << 8); + + if (fread(dat, 1, 4, fp) < 4) return -1; + // memcpy(&sample_rate, dat, 4); // string.h + sample_rate = dat[0]|(dat[1]<<8)|(dat[2]<<16)|(dat[3]<<24); + + if (fread(dat, 1, 4, fp) < 4) return -1; + if (fread(dat, 1, 2, fp) < 2) return -1; + //byte = dat[0] + (dat[1] << 8); + + if (fread(dat, 1, 2, fp) < 2) return -1; + bits_sample = dat[0] + (dat[1] << 8); + + // pos_dat = 36L + info + for ( ; ; ) { + if ( (byte=fgetc(fp)) == EOF ) return -1; + txt[p % 4] = byte; + p++; if (p==4) p=0; + if (findstr(txt, "data", p) == 4) break; + } + if (fread(dat, 1, 4, fp) < 4) return -1; + + + fprintf(stderr, "sample_rate: %d\n", sample_rate); + fprintf(stderr, "bits : %d\n", bits_sample); + fprintf(stderr, "channels : %d\n", channels); + + if ((bits_sample != 8) && (bits_sample != 16)) return -1; + + samples_per_bit = sample_rate/rs_data->baud; + fprintf(stderr, "samples/bit: %.2f\n", samples_per_bit); + rs_data->samples_per_bit = samples_per_bit; + + return 0; +} + + +#define EOF_INT 0x1000000 + +static unsigned long sample_count = 0; + +static int read_signed_sample(FILE *fp) { // int = i32_t + int byte, i, sample, s=0; // EOF -> 0x1000000 + + for (i = 0; i < channels; i++) { + // i = 0: left/mono + byte = fgetc(fp); + if (byte == EOF) return EOF_INT; + if (i == 0) sample = byte; + + if (bits_sample == 16) { + byte = fgetc(fp); + if (byte == EOF) return EOF_INT; + if (i == 0) sample += byte << 8; + } + + } + + if (bits_sample == 8) s = sample-128; // 8bit: 00..FF, centerpoint 0x80=128 + if (bits_sample == 16) s = (short)sample; + + sample_count++; + + return s; +} + +static int par=1, par_alt=1; + +int read_bits_fsk(FILE *fp, int *bit, int *len, int inv) { + +int option_res = 0; + + static int sample; + int n, y0; + float l, x1; + static float x0; + + n = 0; + do{ + y0 = sample; + sample = read_signed_sample(fp); + if (sample == EOF_INT) return EOF; + //sample_count++; // in read_signed_sample() + par_alt = par; + par = (sample >= 0) ? 1 : -1; // 8bit: 0..127,128..255 (-128..-1,0..127) + n++; + } while (par*par_alt > 0); + + if (!option_res) l = (float)n / samples_per_bit; + else { // genauere Bitlaengen-Messung + x1 = sample/(float)(sample-y0); // hilft bei niedriger sample rate + l = (n+x0-x1) / samples_per_bit; // meist mehr frames (nicht immer) + x0 = x1; + } + + *len = (int)(l+0.5); + + if (!inv) *bit = (1+par_alt)/2; // oben 1, unten -1 + else *bit = (1-par_alt)/2; // sdr#bufpos = (rs_data->bufpos+1) % rs_data->header_len; +} + +int compare(rs_data_t *rs_data) { + int i=0, j = rs_data->bufpos; + + while (i < rs_data->header_len) { + if (j < 0) j = rs_data->header_len-1; + if (rs_data->buf[j] != rs_data->header[rs_data->header_ofs+rs_data->header_len-1-i]) break; + j--; + i++; + } + return i; +} + + + +/* ------------------------------------------------------------------------------------ */ + + + diff --git a/rs_module/rs_demod.h b/rs_module/rs_demod.h new file mode 100644 index 0000000..b161ce0 --- /dev/null +++ b/rs_module/rs_demod.h @@ -0,0 +1,14 @@ + +#ifndef RS_DEMOD_H +#define RS_DEMOD_H + + +int read_wav_header(FILE *, rs_data_t *); +int read_bits_fsk(FILE *, int *, int *, int); + +void inc_bufpos(rs_data_t *); +int compare(rs_data_t *); + + +#endif /* RS_DEMOD_H */ + diff --git a/rs_module/rs_gps.c b/rs_module/rs_gps.c new file mode 100644 index 0000000..6efc708 --- /dev/null +++ b/rs_module/rs_gps.c @@ -0,0 +1,1469 @@ + +/* + Quellen: + - IS-GPS-200H (bis C: ICD-GPS-200): + http://www.gps.gov/technical/icwg/ + - Borre: http://kom.aau.dk/~borre + - Essential GNSS Project (hier und da etwas unklar) +*/ + + +#define PI (3.1415926535897932384626433832795) + +#define RELATIVISTIC_CLOCK_CORRECTION (-4.442807633e-10) // combined constant defined in IS-GPS-200 [s]/[sqrt(m)] +#define GRAVITY_CONSTANT (3.986005e14) // gravity constant defined on IS-GPS-200 [m^3/s^2] +#define EARTH_ROTATION_RATE (7.2921151467e-05) // IS-GPS-200 [rad/s] +#define SECONDS_IN_WEEK (604800.0) // [s] +#define LIGHTSPEED (299792458.0) // light speed constant defined in IS-GPS-200 [m/s] + +#define RANGE_ESTIMATE (0.072) // approx. GPS-Sat range 0.072s*299792458m/s=21585057m +#define RANGERATE_ESTIMATE (0.000) // + +#define EARTH_a (6378137.0) +#define EARTH_b (6356752.31424518) +#define EARTH_a2_b2 (EARTH_a*EARTH_a - EARTH_b*EARTH_b) + + +/* ---------------------------------------------------------------------------------------------------- */ + + +typedef struct { + ui16_t prn; + ui16_t week; + ui32_t toa; + char epoch[20]; + double toe; + double toc; + double e; + double delta_n; + double delta_i; + double i0; + double OmegaDot; + double sqrta; + double Omega0; + double w; + double M0; + double tgd; + double idot; + double cuc; + double cus; + double crc; + double crs; + double cic; + double cis; + double af0; + double af1; + double af2; + int gpsweek; + ui16_t svn; + ui8_t ura; + ui8_t health; + ui8_t conf; +} EPHEM_t; + +typedef struct { + ui32_t tow; + double pseudorange; + double pseudorate; + double clock_corr; + double clock_drift; + double X; + double Y; + double Z; + double vX; + double vY; + double vZ; + int ephhr; + double PR; + double ephtime; + int prn; + ui8_t status; +} SAT_t; + +typedef struct {double X; double Y; double Z;} LOC_t; + +typedef struct {double X; double Y; double Z; + double vX; double vY; double vZ;} VEL_t; + +static void GPS_SatellitePositionVelocity_Ephem( + const unsigned short , const double , EPHEM_t , + double* , double* , double* , double* , double* , + double* , double* , double* ); + + +/* ---------------------------------------------------------------------------------------------------- */ + + +static int ecef2elli(double X, double Y, double Z, double *lat, double *lon, double *alt) { + double ea2 = EARTH_a2_b2 / (EARTH_a*EARTH_a), + eb2 = EARTH_a2_b2 / (EARTH_b*EARTH_b); + double phi, lam, R, p, t; + double sint, cost; + + lam = atan2( Y , X ); + + p = sqrt( X*X + Y*Y ); + t = atan2( Z*EARTH_a , p*EARTH_b ); + + sint = sin(t); + cost = cos(t); + + phi = atan2( Z + eb2 * EARTH_b * sint*sint*sint , + p - ea2 * EARTH_a * cost*cost*cost ); + + R = EARTH_a / sqrt( 1 - ea2*sin(phi)*sin(phi) ); + *alt = p / cos(phi) - R; + + *lat = phi*180.0/PI; + *lon = lam*180.0/PI; + + return 0; +} + + +static double dist(double X1, double Y1, double Z1, double X2, double Y2, double Z2) { + return sqrt( (X2-X1)*(X2-X1) + (Y2-Y1)*(Y2-Y1) + (Z2-Z1)*(Z2-Z1) ); +} + + +static void rotZ(double x1, double y1, double z1, double angle, double *x2, double *y2, double *z2) { + double cosa = cos(angle); + double sina = sin(angle); + *x2 = cosa * x1 + sina * y1; + *y2 = -sina * x1 + cosa * y1; + *z2 = z1; +} + + +/* ---------------------------------------------------------------------------------------------------- */ + + +int read_SEMalmanac(FILE *fp, EPHEM_t *alm) { + int l, j; + char buf[64]; + unsigned n, week, toa, ui; + double dbl; + + l = fscanf(fp, "%u", &n); if (l != 1) return -1; + l = fscanf(fp, "%s", buf); if (l != 1) return -1; + l = fscanf(fp, "%u", &week); if (l != 1) return -1; + l = fscanf(fp, "%u", &toa); if (l != 1) return -1; + + for (j = 1; j <= n; j++) { + //memset(&ephem, 0, sizeof(ephem)); + + alm[j].week = (ui16_t)week; + alm[j].toa = (ui32_t)toa; + alm[j].toe = (double)toa; + alm[j].toc = alm[j].toe; + + l = fscanf(fp, "%u", &ui); if (l != 1) return -1; alm[j].prn = (ui16_t)ui; + l = fscanf(fp, "%u", &ui); if (l != 1) return -2; alm[j].svn = (ui16_t)ui; + l = fscanf(fp, "%u", &ui); if (l != 1) return -3; alm[j].ura = (ui8_t)ui; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -4; alm[j].e = dbl; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -5; alm[j].delta_i = dbl; + alm[j].i0 = (0.30 + alm[j].delta_i) * PI; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -6; alm[j].OmegaDot = dbl * PI; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -7; alm[j].sqrta = dbl; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -6; alm[j].Omega0 = dbl * PI; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -8; alm[j].w = dbl * PI; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -9; alm[j].M0 = dbl * PI; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -10; alm[j].af0 = dbl; + l = fscanf(fp, "%lf", &dbl); if (l != 1) return -11; alm[j].af1 = dbl; + alm[j].af2 = 0; + alm[j].crc = 0; + alm[j].crs = 0; + alm[j].cuc = 0; + alm[j].cus = 0; + alm[j].cic = 0; + alm[j].cis = 0; + alm[j].tgd = 0; + alm[j].idot = 0; + alm[j].delta_n = 0; + l = fscanf(fp, "%u", &ui); if (l != 1) return -12; alm[j].health = (ui8_t)ui; + l = fscanf(fp, "%u", &ui); if (l != 1) return -13; alm[j].conf = (ui8_t)ui; + } + + return 0; +} + +EPHEM_t *read_RNXpephs(FILE *fp) { + int l, i, n; + char buffer[82]; + char buf[64], str[20]; + char *pbuf; + unsigned ui; + double dbl; + int c; + EPHEM_t ephem = {}, *te = NULL; + int count = 0; + long fpos; + + do { // header-Zeilen: 80 Zeichen + pbuf = fgets(buffer, 82, fp); // max 82-1 Zeichen + '\0' + buffer[82] = '\0'; // doppelt haelt besser + } while ( pbuf && !strstr(buffer, "END OF HEADER") ); + + if (pbuf == NULL) return NULL; + fpos = ftell(fp); + + count = 0; + while (count >= 0) { // data-Zeilen: 79 Zeichen + pbuf = fgets(buffer, 82, fp); if (pbuf == 0) break; + strncpy(str, buffer, 3); + str[3] = '\0'; + sscanf(str, "%d", &ui); + if (ui < 33) count++; + for (i = 0; i < 7; i++) { + pbuf = fgets(buffer, 82, fp); if (pbuf == 0) break; + } + } + //printf("Ephemerides: %d\n", count); + + fseek(fp, fpos, SEEK_SET); + + te = calloc( count+1, sizeof(ephem) ); // calloc( 1, sizeof(ephem) ); + if (te == NULL) return NULL; + + n = 0; + + while (count > 0) { // brdc/hour-rinex sollte nur Daten von einem Tag enthalten + + //memset(&ephem, 0, sizeof(ephem)); + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; sscanf(buf, "%d", &ui); + ephem.prn = ui; + + for (i = 0; i < 16; i++) ephem.epoch[i] = '0'; + ephem.epoch[16] = '\0'; + + l = fread(buf, 19, 1, fp); if (l != 1) break; buf[19] = 0; + + for (i = 0; i < 6; i++) { + c = buf[3*i ]; if (c == ' ') c = '0'; str[2*i ] = c; + c = buf[3*i+1]; if (c == ' ') c = '0'; str[2*i+1] = c; + } + str[12] = buf[17]; + str[13] = buf[18]; + str[14] = '\0'; + + strncpy(ephem.epoch , "20", 2); // vorausgesetzt 21.Jhd; Datum steht auch im Header + strncpy(ephem.epoch+2, str, 15); + ephem.epoch[16] = '\0'; + + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af0 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af1 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.af2 = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iode = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.crs = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.delta_n = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.M0 = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cuc = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.e = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cus = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.sqrta = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.toe = dbl; + ephem.toc = ephem.toe; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cic = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.Omega0 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.cis = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.i0 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.crc = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.w = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.OmegaDot = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.idot = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.codeL2 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.gpsweek = (int)dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iodc = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.sva = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.svh = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); ephem.tgd = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.iodc = dbl; + if ((c=fgetc(fp)) == EOF) break; + + l = fread(buf, 3, 1, fp); if (l != 1) break; buf[ 3] = 0; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.ttom = dbl; + pbuf = fgets(buffer, 82, fp); + /* // die letzten beiden Felder (spare) sind manchmal leer (statt 0.00); manchmal fehlt sogar das drittletzte Feld + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.fit = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.spare1 = dbl; + l = fread(buf, 19, 1, fp); if (l != 1) break; if (buf[15] == 'D') buf[15] = 'E'; buf[19] = 0; sscanf(buf, "%lf", &dbl); //ephem.spare2 = dbl; + if ((c=fgetc(fp)) == EOF) break; */ + + ephem.week = 1; // ephem.gpsweek + + te[n] = ephem; + n += 1; + + //tmp = realloc( te, (count+1) * sizeof(ephem) ); + //if (tmp == NULL) break; + //te = tmp; + + if (pbuf == NULL) break; + } + + te[n].prn = 0; + + + return te; +} + + +/* ---------------------------------------------------------------------------------------------------- */ + + +static int trace_invert(double mat[4][4], double trinv[4]) // trace-invert +/* selected elements from 4x4 matrix inversion */ +{ + // Find all NECESSARY 2x2 subdeterminants + double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]; + double Det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]; + //double Det2_12_03 = mat[1][0]*mat[2][3] - mat[1][3]*mat[2][0]; + double Det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]; + //double Det2_12_13 = mat[1][1]*mat[2][3] - mat[1][3]*mat[2][1]; + //double Det2_12_23 = mat[1][2]*mat[2][3] - mat[1][3]*mat[2][2]; + double Det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0]; + //double Det2_13_02 = mat[1][0]*mat[3][2] - mat[1][2]*mat[3][0]; + double Det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0]; + //double Det2_13_12 = mat[1][1]*mat[3][2] - mat[1][2]*mat[3][1]; + double Det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1]; + //double Det2_13_23 = mat[1][2]*mat[3][3] - mat[1][3]*mat[3][2]; + double Det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0]; + double Det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0]; + double Det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0]; + double Det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1]; + double Det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1]; + double Det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2]; + + // Find all NECESSARY 3x3 subdeterminants + double Det3_012_012 = mat[0][0] * Det2_12_12 - mat[0][1] * Det2_12_02 + mat[0][2] * Det2_12_01; + //double Det3_012_013 = mat[0][0]*Det2_12_13 - mat[0][1]*Det2_12_03 + mat[0][3]*Det2_12_01; + //double Det3_012_023 = mat[0][0]*Det2_12_23 - mat[0][2]*Det2_12_03 + mat[0][3]*Det2_12_02; + //double Det3_012_123 = mat[0][1]*Det2_12_23 - mat[0][2]*Det2_12_13 + mat[0][3]*Det2_12_12; + //double Det3_013_012 = mat[0][0]*Det2_13_12 - mat[0][1]*Det2_13_02 + mat[0][2]*Det2_13_01; + double Det3_013_013 = mat[0][0] * Det2_13_13 - mat[0][1] * Det2_13_03 + mat[0][3] * Det2_13_01; + //double Det3_013_023 = mat[0][0]*Det2_13_23 - mat[0][2]*Det2_13_03 + mat[0][3]*Det2_13_02; + //double Det3_013_123 = mat[0][1]*Det2_13_23 - mat[0][2]*Det2_13_13 + mat[0][3]*Det2_13_12; + //double Det3_023_012 = mat[0][0]*Det2_23_12 - mat[0][1]*Det2_23_02 + mat[0][2]*Det2_23_01; + //double Det3_023_013 = mat[0][0]*Det2_23_13 - mat[0][1]*Det2_23_03 + mat[0][3]*Det2_23_01; + double Det3_023_023 = mat[0][0] * Det2_23_23 - mat[0][2] * Det2_23_03 + mat[0][3] * Det2_23_02; + //double Det3_023_123 = mat[0][1]*Det2_23_23 - mat[0][2]*Det2_23_13 + mat[0][3]*Det2_23_12; + double Det3_123_012 = mat[1][0] * Det2_23_12 - mat[1][1] * Det2_23_02 + mat[1][2] * Det2_23_01; + double Det3_123_013 = mat[1][0] * Det2_23_13 - mat[1][1] * Det2_23_03 + mat[1][3] * Det2_23_01; + double Det3_123_023 = mat[1][0] * Det2_23_23 - mat[1][2] * Det2_23_03 + mat[1][3] * Det2_23_02; + double Det3_123_123 = mat[1][1] * Det2_23_23 - mat[1][2] * Det2_23_13 + mat[1][3] * Det2_23_12; + + // Find the 4x4 determinant + static double det; + det = mat[0][0] * Det3_123_123 + - mat[0][1] * Det3_123_023 + + mat[0][2] * Det3_123_013 + - mat[0][3] * Det3_123_012; + + // Very small determinants probably reflect floating-point fuzz near zero + if (fabs(det) < 0.0001) + return -1; + + //inv[0][0] = Det3_123_123 / det; + //inv[0][1] = -Det3_023_123 / det; + //inv[0][2] = Det3_013_123 / det; + //inv[0][3] = -Det3_012_123 / det; + + //inv[1][0] = -Det3_123_023 / det; + //inv[1][1] = Det3_023_023 / det; + //inv[1][2] = -Det3_013_023 / det; + //inv[1][3] = Det3_012_023 / det; + + //inv[2][0] = Det3_123_013 / det; + //inv[2][1] = -Det3_023_013 / det; + //inv[2][2] = Det3_013_013 / det; + //inv[2][3] = -Det3_012_013 / det; + + //inv[3][0] = -Det3_123_012 / det; + //inv[3][1] = Det3_023_012 / det; + //inv[3][2] = -Det3_013_012 / det; + //inv[3][3] = Det3_012_012 / det; + + trinv[0] = Det3_123_123 / det; + + trinv[1] = Det3_023_023 / det; + trinv[2] = Det3_013_013 / det; + trinv[3] = Det3_012_012 / det; + + return 0; +} + +int calc_DOPn(int n, SAT_t satss[], double pos_ecef[3], double DOP[4]) { + int i, j, k; + double norm[n], satpos[n][3]; + double SATS[n][4], AtA[4][4]; + + for (i = 0; i < n; i++) { + satpos[i][0] = satss[i].X-pos_ecef[0]; + satpos[i][1] = satss[i].Y-pos_ecef[1]; + satpos[i][2] = satss[i].Z-pos_ecef[2]; + } + + + for (i = 0; i < n; i++) { + norm[i] = sqrt( satpos[i][0]*satpos[i][0] + satpos[i][1]*satpos[i][1] + satpos[i][2]*satpos[i][2] ); + for (j = 0; j < 3; j++) { + SATS[i][j] = satpos[i][j] / norm[i]; + } + SATS[i][3] = 1; + } + + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + AtA[i][j] = 0.0; + for (k = 0; k < n; k++) { + AtA[i][j] += SATS[k][i]*SATS[k][j]; + } + } + } + + return trace_invert(AtA, DOP); +} + +/* ---------------------------------------------------------------------------------------------------- */ + +static int rotE(SAT_t sat, double *x, double *y, double *z) { + // Erdrotation ECEF-ECI, 0.070s*299792458m/s=20985472m, 0.072s*299792458m/s=21585057m + double range = sat.PR/LIGHTSPEED; + if (range < 19e6 || range > 30e6) range = 21e6; + rotZ(sat.X, sat.Y, sat.Z, EARTH_ROTATION_RATE*(range/LIGHTSPEED), x, y, z); + return 0; +} + + +static double lorentz(double a[4], double b[4]) { + return a[0]*b[0] + a[1]*b[1] +a[2]*b[2] - a[3]*b[3]; +} + +static int matrix_invert(double mat[4][4], double inv[4][4]) +{ + // 2x2 subdeterminants + double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]; + double Det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]; + double Det2_12_03 = mat[1][0] * mat[2][3] - mat[1][3] * mat[2][0]; + double Det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]; + double Det2_12_13 = mat[1][1] * mat[2][3] - mat[1][3] * mat[2][1]; + double Det2_12_23 = mat[1][2] * mat[2][3] - mat[1][3] * mat[2][2]; + double Det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0]; + double Det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0]; + double Det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0]; + double Det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1]; + double Det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1]; + double Det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2]; + double Det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0]; + double Det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0]; + double Det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0]; + double Det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1]; + double Det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1]; + double Det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2]; + + // 3x3 subdeterminants + double Det3_012_012 = mat[0][0] * Det2_12_12 - mat[0][1] * Det2_12_02 + mat[0][2] * Det2_12_01; + double Det3_012_013 = mat[0][0] * Det2_12_13 - mat[0][1] * Det2_12_03 + mat[0][3] * Det2_12_01; + double Det3_012_023 = mat[0][0] * Det2_12_23 - mat[0][2] * Det2_12_03 + mat[0][3] * Det2_12_02; + double Det3_012_123 = mat[0][1] * Det2_12_23 - mat[0][2] * Det2_12_13 + mat[0][3] * Det2_12_12; + double Det3_013_012 = mat[0][0] * Det2_13_12 - mat[0][1] * Det2_13_02 + mat[0][2] * Det2_13_01; + double Det3_013_013 = mat[0][0] * Det2_13_13 - mat[0][1] * Det2_13_03 + mat[0][3] * Det2_13_01; + double Det3_013_023 = mat[0][0] * Det2_13_23 - mat[0][2] * Det2_13_03 + mat[0][3] * Det2_13_02; + double Det3_013_123 = mat[0][1] * Det2_13_23 - mat[0][2] * Det2_13_13 + mat[0][3] * Det2_13_12; + double Det3_023_012 = mat[0][0] * Det2_23_12 - mat[0][1] * Det2_23_02 + mat[0][2] * Det2_23_01; + double Det3_023_013 = mat[0][0] * Det2_23_13 - mat[0][1] * Det2_23_03 + mat[0][3] * Det2_23_01; + double Det3_023_023 = mat[0][0] * Det2_23_23 - mat[0][2] * Det2_23_03 + mat[0][3] * Det2_23_02; + double Det3_023_123 = mat[0][1] * Det2_23_23 - mat[0][2] * Det2_23_13 + mat[0][3] * Det2_23_12; + double Det3_123_012 = mat[1][0] * Det2_23_12 - mat[1][1] * Det2_23_02 + mat[1][2] * Det2_23_01; + double Det3_123_013 = mat[1][0] * Det2_23_13 - mat[1][1] * Det2_23_03 + mat[1][3] * Det2_23_01; + double Det3_123_023 = mat[1][0] * Det2_23_23 - mat[1][2] * Det2_23_03 + mat[1][3] * Det2_23_02; + double Det3_123_123 = mat[1][1] * Det2_23_23 - mat[1][2] * Det2_23_13 + mat[1][3] * Det2_23_12; + + // 4x4 determinant + static double det; + det = mat[0][0] * Det3_123_123 + - mat[0][1] * Det3_123_023 + + mat[0][2] * Det3_123_013 + - mat[0][3] * Det3_123_012; + + // Very small determinants probably reflect floating-point fuzz near zero + if (fabs(det) < 0.0001) + return -1; + + inv[0][0] = Det3_123_123 / det; + inv[0][1] = -Det3_023_123 / det; + inv[0][2] = Det3_013_123 / det; + inv[0][3] = -Det3_012_123 / det; + + inv[1][0] = -Det3_123_023 / det; + inv[1][1] = Det3_023_023 / det; + inv[1][2] = -Det3_013_023 / det; + inv[1][3] = Det3_012_023 / det; + + inv[2][0] = Det3_123_013 / det; + inv[2][1] = -Det3_023_013 / det; + inv[2][2] = Det3_013_013 / det; + inv[2][3] = -Det3_012_013 / det; + + inv[3][0] = -Det3_123_012 / det; + inv[3][1] = Det3_023_012 / det; + inv[3][2] = -Det3_013_012 / det; + inv[3][3] = Det3_012_012 / det; + + return 0; +} + +int NAV_bancroft1(int N, SAT_t sats[], double pos_ecef[3], double *cc) { + + int i, j, k; + double B[N][4], BtB[4][4], BBinv[4][4], BBB[4][N]; + double a[N], Be[4], Ba[4]; + + double q0, q1, q2, p, q, sq, x1, x2; + double Lsg1[4], Lsg2[4]; + + double tmp1, tmp2; + double X, Y, Z; + + + if (N < 4 || N > 12) return -1; + + for (i = 0; i < N; i++) { + rotZ(sats[i].X, sats[i].Y, sats[i].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE, B[i], B[i]+1, B[i]+2); + B[i][3] = sats[i].pseudorange + sats[i].clock_corr; + } + + if (N == 4) { + matrix_invert(B, BBB); + } + else { + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + BtB[i][j] = 0.0; + for (k = 0; k < N; k++) { + BtB[i][j] += B[k][i]*B[k][j]; + } + } + } + matrix_invert(BtB, BBinv); + for (i = 0; i < 4; i++) { + for (j = 0; j < N; j++) { + BBB[i][j] = 0.0; + for (k = 0; k < 4; k++) { + BBB[i][j] += BBinv[i][k]*B[j][k]; + } + } + } + } + + for (i = 0; i < 4; i++) { + Be[i] = 0.0; + for (k = 0; k < N; k++) { + Be[i] += BBB[i][k]*1.0; + } + } + + for (i = 0; i < N; i++) { + a[i] = 0.5 * lorentz(B[i], B[i]); + } + + for (i = 0; i < 4; i++) { + Ba[i] = 0.0; + for (k = 0; k < N; k++) { + Ba[i] += BBB[i][k]*a[k]; + } + } + + q2 = lorentz(Be, Be); + q1 = lorentz(Ba, Be)-1; + q0 = lorentz(Ba, Ba); + + if (q2 == 0) return -2; + + p = q1/q2; + q = q0/q2; + + sq = p*p - q; + if (sq < 0) return -2; + + x1 = -p + sqrt(sq); + x2 = -p - sqrt(sq); + + for (i = 0; i < 4; i++) { + Lsg1[i] = x1*Be[i]+Ba[i]; + Lsg2[i] = x2*Be[i]+Ba[i]; + } + Lsg1[3] = -Lsg1[3]; + Lsg2[3] = -Lsg2[3]; + + + tmp1 = sqrt( Lsg1[0]*Lsg1[0] + Lsg1[1]*Lsg1[1] + Lsg1[2]*Lsg1[2] ); + tmp2 = sqrt( Lsg2[0]*Lsg2[0] + Lsg2[1]*Lsg2[1] + Lsg2[2]*Lsg2[2] ); + + tmp1 = fabs( tmp1 - 6371000.0 ); + tmp2 = fabs( tmp2 - 6371000.0 ); + + if (tmp1 < tmp2) { + X = Lsg1[0]; Y = Lsg1[1]; Z = Lsg1[2]; *cc = Lsg1[3]; + } else { + X = Lsg2[0]; Y = Lsg2[1]; Z = Lsg2[2]; *cc = Lsg2[3]; + } + pos_ecef[0] = X; + pos_ecef[1] = Y; + pos_ecef[2] = Z; + + return 0; +} + +int NAV_bancroft2(int N, SAT_t sats[], double pos_ecef[3], double *cc) { + + int i, j, k; + double B[N][4], BtB[4][4], BBinv[4][4], BBB[4][N]; + double a[N], Be[4], Ba[4]; + + double q0, q1, q2, p, q, sq, x1, x2; + double Lsg1[4], Lsg2[4]; + + double tmp1, tmp2; + double X, Y, Z; + + + if (N < 4 || N > 12) return -1; + + for (i = 0; i < N; i++) { + rotE(sats[i], B[i], B[i]+1, B[i]+2); + B[i][3] = sats[i].PR; + } + + if (N == 4) { + matrix_invert(B, BBB); + } + else { + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + BtB[i][j] = 0.0; + for (k = 0; k < N; k++) { + BtB[i][j] += B[k][i]*B[k][j]; + } + } + } + matrix_invert(BtB, BBinv); + for (i = 0; i < 4; i++) { + for (j = 0; j < N; j++) { + BBB[i][j] = 0.0; + for (k = 0; k < 4; k++) { + BBB[i][j] += BBinv[i][k]*B[j][k]; + } + } + } + } + + for (i = 0; i < 4; i++) { + Be[i] = 0.0; + for (k = 0; k < N; k++) { + Be[i] += BBB[i][k]*1.0; + } + } + + for (i = 0; i < N; i++) { + a[i] = 0.5 * lorentz(B[i], B[i]); + } + + for (i = 0; i < 4; i++) { + Ba[i] = 0.0; + for (k = 0; k < N; k++) { + Ba[i] += BBB[i][k]*a[k]; + } + } + + q2 = lorentz(Be, Be); + q1 = lorentz(Ba, Be)-1; + q0 = lorentz(Ba, Ba); + + if (q2 == 0) return -2; + + p = q1/q2; + q = q0/q2; + + sq = p*p - q; + if (sq < 0) return -2; + + x1 = -p + sqrt(sq); + x2 = -p - sqrt(sq); + + for (i = 0; i < 4; i++) { + Lsg1[i] = x1*Be[i]+Ba[i]; + Lsg2[i] = x2*Be[i]+Ba[i]; + } + Lsg1[3] = -Lsg1[3]; + Lsg2[3] = -Lsg2[3]; + + + tmp1 = sqrt( Lsg1[0]*Lsg1[0] + Lsg1[1]*Lsg1[1] + Lsg1[2]*Lsg1[2] ); + tmp2 = sqrt( Lsg2[0]*Lsg2[0] + Lsg2[1]*Lsg2[1] + Lsg2[2]*Lsg2[2] ); + + tmp1 = fabs( tmp1 - 6371000.0 ); + tmp2 = fabs( tmp2 - 6371000.0 ); + + if (tmp1 < tmp2) { + X = Lsg1[0]; Y = Lsg1[1]; Z = Lsg1[2]; *cc = Lsg1[3]; + } else { + X = Lsg2[0]; Y = Lsg2[1]; Z = Lsg2[2]; *cc = Lsg2[3]; + } + pos_ecef[0] = X; + pos_ecef[1] = Y; + pos_ecef[2] = Z; + + return 0; +} + +/* ---------------------------------------------------------------------------------------------------- */ + +int NAV_bancroft3(int N, SAT_t sats[], double pos_ecef1[3], double *cc1 , double pos_ecef2[3], double *cc2) { + + int i, j, k; + double B[N][4], BtB[4][4], BBinv[4][4], BBB[4][N]; + double a[N], Be[4], Ba[4]; + + double q0, q1, q2, p, q, sq, x1, x2; + double Lsg1[4], Lsg2[4]; + + double tmp1, tmp2; + double X1, Y1, Z1; + double X2, Y2, Z2; + + + if (N < 4 || N > 12) return -1; + + for (i = 0; i < N; i++) { // Test: nicht hier rotieren, sondern spaeter Lsg rotieren... + rotZ(sats[i].X, sats[i].Y, sats[i].Z, 0.0, B[i], B[i]+1, B[i]+2); + //B[i][3] = sats[i].PR; + B[i][3] = sats[i].pseudorange + sats[i].clock_corr; + } + + if (N == 4) { + matrix_invert(B, BBB); + } + else { + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + BtB[i][j] = 0.0; + for (k = 0; k < N; k++) { + BtB[i][j] += B[k][i]*B[k][j]; + } + } + } + matrix_invert(BtB, BBinv); + for (i = 0; i < 4; i++) { + for (j = 0; j < N; j++) { + BBB[i][j] = 0.0; + for (k = 0; k < 4; k++) { + BBB[i][j] += BBinv[i][k]*B[j][k]; + } + } + } + } + + for (i = 0; i < 4; i++) { + Be[i] = 0.0; + for (k = 0; k < N; k++) { + Be[i] += BBB[i][k]*1.0; + } + } + + for (i = 0; i < N; i++) { + a[i] = 0.5 * lorentz(B[i], B[i]); + } + + for (i = 0; i < 4; i++) { + Ba[i] = 0.0; + for (k = 0; k < N; k++) { + Ba[i] += BBB[i][k]*a[k]; + } + } + + q2 = lorentz(Be, Be); + q1 = lorentz(Ba, Be)-1; + q0 = lorentz(Ba, Ba); + + if (q2 == 0) return -2; + + p = q1/q2; + q = q0/q2; + + sq = p*p - q; + if (sq < 0) return -2; + + x1 = -p + sqrt(sq); + x2 = -p - sqrt(sq); + + for (i = 0; i < 4; i++) { + Lsg1[i] = x1*Be[i]+Ba[i]; + Lsg2[i] = x2*Be[i]+Ba[i]; + } + Lsg1[3] = -Lsg1[3]; + Lsg2[3] = -Lsg2[3]; + + + tmp1 = sqrt( Lsg1[0]*Lsg1[0] + Lsg1[1]*Lsg1[1] + Lsg1[2]*Lsg1[2] ); + tmp2 = sqrt( Lsg2[0]*Lsg2[0] + Lsg2[1]*Lsg2[1] + Lsg2[2]*Lsg2[2] ); + + tmp1 = tmp1 - 6371000.0; + tmp2 = tmp2 - 6371000.0; + + if ( fabs(tmp1) < fabs(tmp2) ) { + X1 = Lsg1[0]; Y1 = Lsg1[1]; Z1 = Lsg1[2]; *cc1 = Lsg1[3]; + X2 = Lsg2[0]; Y2 = Lsg2[1]; Z2 = Lsg2[2]; *cc2 = Lsg2[3]; + } else { + X1 = Lsg2[0]; Y1 = Lsg2[1]; Z1 = Lsg2[2]; *cc1 = Lsg2[3]; + X2 = Lsg1[0]; Y2 = Lsg1[1]; Z2 = Lsg1[2]; *cc2 = Lsg1[3]; + } + + rotZ(X1, Y1, Z1, EARTH_ROTATION_RATE*RANGE_ESTIMATE, pos_ecef1, pos_ecef1+1, pos_ecef1+2); + rotZ(X2, Y2, Z2, EARTH_ROTATION_RATE*RANGE_ESTIMATE, pos_ecef2, pos_ecef2+1, pos_ecef2+2); + + return 0; +} + + + +/* ---------------------------------------------------------------------------------------------------- */ + + +static double NAV_relVel(LOC_t loc, VEL_t vel) { + double d; + double x,y,z; + double norm; + + x = vel.X-loc.X; + y = vel.Y-loc.Y; + z = vel.Z-loc.Z; + norm = sqrt(x*x+y*y+z*z); + x /= norm; + y /= norm; + z /= norm; + + d = vel.vX*x + vel.vY*y + vel.vZ*z; + + return d; +} + +int NAV_LinP(int N, SAT_t satv[], double pos_ecef[3], double dt, + double dpos_ecef[3], double *cc) { + + int i, j, k; + double B[N][4], Binv[4][N], BtB[4][4], BBinv[4][4]; + double a[N], Ba[N]; + + double X, Y, Z; + double norm[N]; + double range, obs_range, prox_range; + + if (N < 4 || N > 12) return -1; + + for (i = 0; i < N; i++) { + + range = dist( pos_ecef[0], pos_ecef[1], pos_ecef[2], satv[i].X, satv[i].Y, satv[i].Z ); + range /= LIGHTSPEED; + if (range < 0.06 || range > 0.1) range = RANGE_ESTIMATE; + rotZ(satv[i].X, satv[i].Y, satv[i].Z, EARTH_ROTATION_RATE*range, B[i], B[i]+1, B[i]+2); + //rotZ(satv[i].X, satv[i].Y, satv[i].Z, 0.0, B[i], B[i]+1, B[i]+2); // est. RANGE_RATE = 0.0 + + X = B[i][0]-pos_ecef[0]; + Y = B[i][1]-pos_ecef[1]; + Z = B[i][2]-pos_ecef[2]; + norm[i] = sqrt(X*X+Y*Y+Z*Z); + + B[i][0] = X/norm[i]; + B[i][1] = Y/norm[i]; + B[i][2] = Z/norm[i]; + + B[i][3] = 1; + } + + if (N == 4) { + matrix_invert(B, Binv); + } + else { + + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + BtB[i][j] = 0.0; + for (k = 0; k < N; k++) { + BtB[i][j] += B[k][i]*B[k][j]; + } + } + } + matrix_invert(BtB, BBinv); + for (i = 0; i < 4; i++) { + for (j = 0; j < N; j++) { + Binv[i][j] = 0.0; + for (k = 0; k < 4; k++) { + Binv[i][j] += BBinv[i][k]*B[j][k]; + } + } + } + + } + + + for (i = 0; i < N; i++) { + obs_range = satv[i].pseudorange + satv[i].clock_corr; //satv[i].PR; + prox_range = norm[i] - dt; + a[i] = prox_range - obs_range; + } + + for (i = 0; i < 4; i++) { + Ba[i] = 0.0; + for (k = 0; k < N; k++) { + Ba[i] += Binv[i][k]*a[k]; + } + } + + dpos_ecef[0] = Ba[0]; + dpos_ecef[1] = Ba[1]; + dpos_ecef[2] = Ba[2]; + + *cc = Ba[3]; + + return 0; +} + +int NAV_LinV(int N, SAT_t satv[], double pos_ecef[3], + double vel_ecef[3], double dt, + double dvel_ecef[3], double *cc) { + + int i, j, k; + double B[N][4], Binv[4][N], BtB[4][4], BBinv[4][4]; + double a[N], Ba[N]; + + double X, Y, Z; + double norm[N]; + double v_proj; + double obs_rate, prox_rate; + LOC_t loc; + VEL_t vel; + + if (N < 4 || N > 12) return -1; + + loc.X = pos_ecef[0]; + loc.Y = pos_ecef[1]; + loc.Z = pos_ecef[2]; + + if (N < 4 || N > 12) return -1; + + for (i = 0; i < N; i++) { + rotZ(satv[i].X, satv[i].Y, satv[i].Z, EARTH_ROTATION_RATE*RANGE_ESTIMATE, B[i], B[i]+1, B[i]+2); + //rotZ(satv[i].X, satv[i].Y, satv[i].Z, 0.0, B[i], B[i]+1, B[i]+2); // est. RANGE_RATE = 0.0 + + X = B[i][0]-pos_ecef[0]; + Y = B[i][1]-pos_ecef[1]; + Z = B[i][2]-pos_ecef[2]; + norm[i] = sqrt(X*X+Y*Y+Z*Z); + B[i][0] = X/norm[i]; + B[i][1] = Y/norm[i]; + B[i][2] = Z/norm[i]; + + // SatSpeed = sqrt( satv[i].vX*satv[i].vX + satv[i].vY*satv[i].vY + satv[i].vZ*satv[i].vZ ); + + B[i][3] = 1; + } + + if (N == 4) { + matrix_invert(B, Binv); + } + else { + + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + BtB[i][j] = 0.0; + for (k = 0; k < N; k++) { + BtB[i][j] += B[k][i]*B[k][j]; + } + } + } + matrix_invert(BtB, BBinv); + for (i = 0; i < 4; i++) { + for (j = 0; j < N; j++) { + Binv[i][j] = 0.0; + for (k = 0; k < 4; k++) { + Binv[i][j] += BBinv[i][k]*B[j][k]; + } + } + } + + } + + + for (i = 0; i < N; i++) { + obs_rate = satv[i].pseudorate; // + satv[i].clock_drift; + vel.X = satv[i].X; + vel.Y = satv[i].Y; + vel.Z = satv[i].Z; + vel.vX = satv[i].vX - vel_ecef[0]; + vel.vY = satv[i].vY - vel_ecef[1]; + vel.vZ = satv[i].vZ - vel_ecef[2]; + v_proj = NAV_relVel(loc, vel); + prox_rate = v_proj - dt; + a[i] = prox_rate - obs_rate; + } + + for (i = 0; i < 4; i++) { + Ba[i] = 0.0; + for (k = 0; k < N; k++) { + Ba[i] += Binv[i][k]*a[k]; + } + } + + dvel_ecef[0] = Ba[0]; + dvel_ecef[1] = Ba[1]; + dvel_ecef[2] = Ba[2]; + + *cc = Ba[3]; + + return 0; +} + + + + +// ------------------------------------------------------------- + +#define GPS_WEEK1024 1 +#define WEEKSEC 604800 + + +int gps_satpos_alm(rs_data_t *rs_data, EPHEM_t alm[], double t, SAT_t *sat) { + double X, Y, Z, vX, vY, vZ; + int i, j; + int week, + rollover = 0; + double cl_corr, cl_drift; + + for (i = 0; i < 12; i++) { + if (sat[i].prn == 0) continue; + for (j = 1; j < 33; j++) { + if (alm[j].prn == sat[i].prn) break; + } + if (j == 33) { + // Sat not found + printf("[PRN %02d not found]\n", sat[i].prn); + } + + // Woche hat 604800 sec + if (t-alm[j].toa > WEEKSEC/2) rollover = +1; + else if (t-alm[j].toa < -WEEKSEC/2) rollover = -1; + else rollover = 0; + week = alm[j].week - rollover; + /*if (j == 1)*/ (rs_data->GPS).week = week + GPS_WEEK1024*1024; + + GPS_SatellitePositionVelocity_Ephem( + week, t, alm[j], + &cl_corr, &cl_drift, &X, &Y, &Z, &vX, &vY, &vZ + ); + + sat[i].clock_drift = cl_drift; + sat[i].vX = vX; + sat[i].vY = vY; + sat[i].vZ = vZ; + + sat[i].X = X; + sat[i].Y = Y; + sat[i].Z = Z; + sat[i].clock_corr = cl_corr; + + } + + return 0; +} + +int gps_satpos_rnx(rs_data_t *rs_data, EPHEM_t *eph, double t, SAT_t *sat) { + double X, Y, Z, vX, vY, vZ; + int i; + int week, + rollover = 0; + double cl_corr, cl_drift; + double tdiff, td; + int count, count0, satfound; + + for (i = 0; i < 12; i++) { + if (sat[i].prn == 0) continue; + + count = count0 = 0; + satfound = 0; + + // Woche hat 604800 sec + tdiff = WEEKSEC; + + while (eph[count].prn > 0) { + + if (eph[count].prn == sat[i].prn) { + + satfound += 1; + + if (t - eph[count].toe > WEEKSEC/2) rollover = +1; + else if (t - eph[count].toe < -WEEKSEC/2) rollover = -1; + else rollover = 0; + td = fabs( t - eph[count].toe - rollover*WEEKSEC); + + if ( td < tdiff ) { + tdiff = td; + week = eph[count].week - rollover; + (rs_data->GPS).week = eph[count].gpsweek - rollover; + count0 = count; + } + } + count += 1; + } + + if ( satfound ) + { + GPS_SatellitePositionVelocity_Ephem( + week, t, eph[count0], + &cl_corr, &cl_drift, &X, &Y, &Z, &vX, &vY, &vZ + ); + sat[i].clock_drift = cl_drift; + sat[i].vX = vX; + sat[i].vY = vY; + sat[i].vZ = vZ; + + sat[i].X = X; + sat[i].Y = Y; + sat[i].Z = Z; + sat[i].clock_corr = cl_corr; + sat[i].ephtime = eph[count0].toe; + } + + } + + return 0; +} + + + +/* ---------------------------------------------------------------------------------------------------- */ + +// - IS-GPS-200H (bis C: ICD-GPS-200): +// http://www.gps.gov/technical/icwg/ +// - Borre: http://kom.aau.dk/~borre +// - Essential GNSS Project (hier und da etwas unklar) + +// ------------------------------------------------------------- +// +// Satellite Position and Velocity +// + +static void GPS_SatelliteClockDriftCorrection( + const unsigned short transmission_gpsweek, // GPS week when signal was transmit (0-1024+) [weeks] + const double transmission_gpstow, // GPS time of week when signal was transmit [s] + const unsigned short ephem_week, // ephemeris: GPS week (0-1024+) [weeks] + const double toe, // ephemeris: time of week [s] + const double toc, // ephemeris: clock reference time of week [s] + const double af0, // ephemeris: polynomial clock correction coefficient [s], + const double af1, // ephemeris: polynomial clock correction coefficient [s/s], + const double af2, // ephemeris: polynomial clock correction coefficient [s/s^2] + const double ecc, // ephemeris: eccentricity of satellite orbit [] + const double sqrta, // ephemeris: square root of the semi-major axis of orbit [m^(1/2)] + const double delta_n, // ephemeris: mean motion difference from computed value [rad] + const double m0, // ephemeris: mean anomaly at reference time [rad] + const double tgd, // ephemeris: group delay differential between L1 and L2 [s] + double* clock_correction, // ephemeris: satellite clock correction [m] + double* clock_drift ) // ephemeris: satellite clock drift correction [m/s] +{ + int j; + + double tot; // time of transmission (including gps week) [s] + double tk; // time from ephemeris reference epoch [s] + double tc; // time from clock reference epoch [s] + double d_tr; // relativistic correction term [s] + double d_tsv; // SV PRN code phase time offset [s] + double a; // semi-major axis of orbit [m] + double n; // corrected mean motion [rad/s] + double M; // mean anomaly, [rad] + double E; // eccentric anomaly [rad] + + // compute the times from the reference epochs + tot = transmission_gpsweek*SECONDS_IN_WEEK + transmission_gpstow; + tk = tot - (ephem_week*SECONDS_IN_WEEK + toe); + tc = tot - (ephem_week*SECONDS_IN_WEEK + toc); + + // compute the corrected mean motion term + a = sqrta*sqrta; + n = sqrt( GRAVITY_CONSTANT / (a*a*a) ); // mean motion + n += delta_n; // corrected mean motion + + // Kepler-Gleichung M = E - e sin(E) + M = m0 + n*tk; // mean anomaly + E = M; // f(E) = M + e sin(E) hat Fixpunkt fuer e < 1, + for( j = 0; j < 7; j++ ) { // da |f'(E)|=|e cos(E)|<1. + E = M + ecc * sin(E); // waehle Startwert E_1 = M, E_{k+1} = f(E_k) + } // konvergiert langsam gegen E_0 = f(E_0) + + // relativistic correction + d_tr = RELATIVISTIC_CLOCK_CORRECTION * ecc * sqrta * sin(E); // [s] + d_tr *= LIGHTSPEED; + + // clock correction + d_tsv = af0 + af1*tc + af2*tc*tc; // [s] + + // L1 only + d_tsv -= tgd; // [s] + + // clock correction + *clock_correction = d_tsv*LIGHTSPEED + d_tr; // [m] + + // clock drift + *clock_drift = (af1 + 2.0*af2*tc) * LIGHTSPEED; // [m/s] + +} + +static void GPS_ComputeSatellitePositionVelocity( + const unsigned short transmission_gpsweek, // GPS week when signal was transmit (0-1024+) [weeks] + const double transmission_gpstow, // GPS time of week when signal was transmit [s] + const unsigned short ephem_week, // ephemeris: GPS week (0-1024+) [weeks] + const double toe, // ephemeris: time of week [s] + const double m0, // ephemeris: mean anomaly at reference time [rad] + const double delta_n, // ephemeris: mean motion difference from computed value [rad] + const double ecc, // ephemeris: eccentricity [] + const double sqrta, // ephemeris: square root of the semi-major axis [m^(1/2)] + const double omega0, // ephemeris: longitude of ascending node of orbit plane at weekly epoch [rad] + const double i0, // ephemeris: inclination angle at reference time [rad] + const double w, // ephemeris: argument of perigee [rad] + const double omegadot, // ephemeris: rate of right ascension [rad/s] + const double idot, // ephemeris: rate of inclination angle [rad/s] + const double cuc, // ephemeris: cos harmonic correction term to the argument of latitude [rad] + const double cus, // ephemeris: sin harmonic correction term to the argument of latitude [rad] + const double crc, // ephemeris: cos harmonic correction term to the orbit radius [m] + const double crs, // ephemeris: sin harmonic correction term to the orbit radius [m] + const double cic, // ephemeris: cos harmonic correction term to the angle of inclination [rad] + const double cis, // ephemeris: sin harmonic correction term to the angle of inclination [rad] + double* x, // satellite x [m] + double* y, // satellite y [m] + double* z, // satellite z [m] + double* vx, // satellite velocity x [m/s] + double* vy, // satellite velocity y [m/s] + double* vz ) // satellite velocity z [m/s] +{ + int j; + + double tot; // time of transmission (including gps week) [s] + double tk; // time from ephemeris reference epoch [s] + double a; // semi-major axis of orbit [m] + double n; // corrected mean motion [rad/s] + double M; // mean anomaly, [rad] + double E; // eccentric anomaly [rad] + double v; // true anomaly [rad] + double u; // argument of latitude, corrected [rad] + double r; // radius in the orbital plane [m] + double i; // orbital inclination [rad] + double cos2u; // cos(2*u) [] + double sin2u; // sin(2*u) [] + double d_u; // argument of latitude correction [rad] + double d_r; // radius correction [m] + double d_i; // inclination correction [rad] + double x_op; // x position in the orbital plane [m] + double y_op; // y position in the orbital plane [m] + double omegak; // corrected longitude of the ascending node [rad] + double cos_omegak; // cos(omegak) + double sin_omegak; // sin(omegak) + double cosu; // cos(u) + double sinu; // sin(u) + double cosi; // cos(i) + double sini; // sin(i) + double cosE; // cos(E) + double sinE; // sin(E) + + + // compute the times from the reference epochs + tot = transmission_gpsweek*SECONDS_IN_WEEK + transmission_gpstow; + tk = tot - (ephem_week*SECONDS_IN_WEEK + toe); + + // compute the corrected mean motion term + a = sqrta*sqrta; + n = sqrt( GRAVITY_CONSTANT / (a*a*a) ); // computed mean motion + n += delta_n; // corrected mean motion + + // Kepler-Gleichung M = E - e sin(E) + M = m0 + n*tk; // mean anomaly + E = M; // f(E) = M + e sin(E) hat Fixpunkt fuer e < 1, + for( j = 0; j < 7; j++ ) { // da |f'(E)|=|e cos(E)|<1. + E = M + ecc * sin(E); // waehle Startwert E_1 = M, E_{k+1} = f(E_k) + } // konvergiert langsam gegen E_0 = f(E_0) + + cosE = cos(E); + sinE = sin(E); + + // true anomaly + v = atan2( (sqrt(1.0 - ecc*ecc)*sinE), (cosE - ecc) ); + // argument of latitude + u = v + w; + // radius in orbital plane + r = a * (1.0 - ecc * cos(E)); + // orbital inclination + i = i0; + + // second harmonic perturbations + // + cos2u = cos(2.0*u); + sin2u = sin(2.0*u); + // argument of latitude correction + d_u = cuc * cos2u + cus * sin2u; + // radius correction + d_r = crc * cos2u + crs * sin2u; + // correction to inclination + d_i = cic * cos2u + cis * sin2u; + + // corrected argument of latitude + u += d_u; + // corrected radius + r += d_r; + // corrected inclination + i += d_i + idot * tk; + + // positions in orbital plane + cosu = cos(u); + sinu = sin(u); + x_op = r * cosu; + y_op = r * sinu; + + + omegak = omega0 + omegadot*tk - EARTH_ROTATION_RATE*(tk + toe); + // fuer Bestimmung der Satellitenposition in ECEF, range=0; + // fuer GPS-Loesung (Sats in ECI) Erdrotation hinzuziehen: + // rotZ(EARTH_ROTATION_RATE*0.072), 0.072s*299792458m/s=21585057m + + // compute the WGS84 ECEF coordinates, + // vector r with components x & y is now rotated using, R3(-omegak)*R1(-i) + cos_omegak = cos(omegak); + sin_omegak = sin(omegak); + cosi = cos(i); + sini = sin(i); + + *x = x_op * cos_omegak - y_op * sin_omegak * cosi; + *y = x_op * sin_omegak + y_op * cos_omegak * cosi; + *z = y_op * sini; + + + // Satellite Velocity Computations are below + // see Reference + // Remodi, B. M (2004). GPS Tool Box: Computing satellite velocities using the broadcast ephemeris. + // GPS Solutions. Volume 8(3), 2004. pp. 181-183 + // + // example source code was available at [http://www.ngs.noaa.gov/gps-toolbox/bc_velo/bc_velo.c] + + // recomputed the cos and sin of the corrected argument of latitude + + double omegadotk; // corrected rate of right ascension [rad/s] + double edot; // edot = n/(1.0 - ecc*cos(E)), [rad/s] + double vdot; // d/dt of true anomaly [rad/s] + double udot; // d/dt of argument of latitude [rad/s] + double idotdot; // d/dt of the rate of the inclination angle [rad/s^2] + double rdot; // d/dt of the radius in the orbital plane [m/s] + double tmpa; // temp + double tmpb; // temp + double vx_op; // x velocity in the orbital plane [m/s] + double vy_op; // y velocity in the orbital plane [m/s] + + cos2u = cos(2.0*u); + sin2u = sin(2.0*u); + + edot = n / (1.0 - ecc*cosE); + vdot = sinE*edot*(1.0 + ecc*cos(v)) / ( sin(v)*(1.0-ecc*cosE) ); + udot = vdot + 2.0*(cus*cos2u - cuc*sin2u)*vdot; + rdot = a*ecc*sinE*n/(1.0-ecc*cosE) + 2.0*(crs*cos2u - crc*sin2u)*vdot; + idotdot = idot + (cis*cos2u - cic*sin2u)*2.0*vdot; + + vx_op = rdot*cosu - y_op*udot; + vy_op = rdot*sinu + x_op*udot; + + // corrected rate of right ascension including similarily as above, + // for omegak, compensation for the Sagnac effect + omegadotk = omegadot - EARTH_ROTATION_RATE /* - EARTH_ROTATION_RATE*RANGERATE_ESTIMATE/LIGHTSPEED */ ; + + tmpa = vx_op - y_op*cosi*omegadotk; + tmpb = x_op*omegadotk + vy_op*cosi - y_op*sini*idotdot; + + *vx = tmpa * cos_omegak - tmpb * sin_omegak; + *vy = tmpa * sin_omegak + tmpb * cos_omegak; + *vz = vy_op*sini + y_op*cosi*idotdot; +} + +static void GPS_SatellitePositionVelocity_Ephem( + const unsigned short gpsweek, // gps week of signal transmission (0-1024+) [week] + const double gpstow, // time of week of signal transmission (gpstow-psr/c) [s] + EPHEM_t ephem, + double* clock_correction, // clock correction for this satellite for this epoch [m] + double* clock_drift, // clock correction for this satellite for this epoch [m] + double* satX, // satellite X position WGS84 ECEF [m] + double* satY, // satellite Y position WGS84 ECEF [m] + double* satZ, // satellite Z position WGS84 ECEF [m] + double* satvX, // satellite X velocity WGS84 ECEF [m] + double* satvY, // satellite Y velocity WGS84 ECEF [m] + double* satvZ // satellite Z velocity WGS84 ECEF [m] + ) +{ + double tow; // user time of week adjusted with the clock corrections [s] + double x; // sat X position [m] + double y; // sat Y position [m] + double z; // sat Z position [m] + double vx; // sat vX velocity [m] + double vy; // sat VY velocity [m] + double vz; // sat VZ velocity [m] + unsigned short week; // user week adjusted with the clock correction if needed [week] + + + x = y = z = 0.0; + + GPS_SatelliteClockDriftCorrection( gpsweek, gpstow, + ephem.week, ephem.toe, ephem.toc, ephem.af0, + ephem.af1, ephem.af2, ephem.e, ephem.sqrta, + ephem.delta_n, ephem.M0, ephem.tgd, clock_correction, clock_drift ); + + + // adjust for week rollover + week = gpsweek; + tow = gpstow + (*clock_correction)/LIGHTSPEED; + if ( tow < 0.0 ) { + tow += SECONDS_IN_WEEK; + week--; + } + if ( tow > SECONDS_IN_WEEK ) { + tow -= SECONDS_IN_WEEK; + week++; + } + + //range = 0.072s*299792458m/s=21585057m + GPS_ComputeSatellitePositionVelocity( week, tow, + ephem.week, ephem.toe, ephem.M0, ephem.delta_n, ephem.e, ephem.sqrta, + ephem.Omega0, ephem.i0, ephem.w, ephem.OmegaDot, ephem.idot, + ephem.cuc, ephem.cus, ephem.crc, ephem.crs, ephem.cic, ephem.cis, + &x, &y, &z, &vx, &vy, &vz ); + + *satX = x; + *satY = y; + *satZ = z; + *satvX = vx; + *satvY = vy; + *satvZ = vz; + +} + diff --git a/rs_module/rs_main41.c b/rs_module/rs_main41.c new file mode 100644 index 0000000..b0a62c3 --- /dev/null +++ b/rs_module/rs_main41.c @@ -0,0 +1,213 @@ + + +#include +#include +#include +#include + +#include "rs_data.h" +#include "rs_demod.h" +#include "rs_datum.h" +#include "rs_rs41.h" + + +int option_verbose = 0, // ausfuehrliche Anzeige + option_raw = 0, // rohe Frames + option_inv = 0, // invertiert Signal + option_crc = 0, // check CRC + option_sat = 0, // GPS sat data + wavloaded = 0; + + +ui8_t *frame = NULL; + +int print_position(rs_data_t *rs_data) { + +// option_crc: check block-crc + + fprintf(stdout, "[%5d] ", rs_data->frnr); + fprintf(stdout, "(%s) ", rs_data->SN); + + fprintf(stdout, "%s ", weekday[rs_data->wday]); + fprintf(stdout, "%04d-%02d-%02d %02d:%02d:%06.3f", + rs_data->year, rs_data->month, rs_data->day, + rs_data->hr, rs_data->min, rs_data->sec); + if (option_verbose == 3) fprintf(stdout, " (W %d)", (rs_data->GPS).week); + + fprintf(stdout, " "); + fprintf(stdout, " lat: %.5f ", (rs_data->GPS).lat); + fprintf(stdout, " lon: %.5f ", (rs_data->GPS).lon); + fprintf(stdout, " alt: %.2f ", (rs_data->GPS).alt); + fprintf(stdout," vH: %4.1f D: %5.1f° vV: %3.1f ", (rs_data->GPS).vH, (rs_data->GPS).vD, (rs_data->GPS).vU); + + int i; + fprintf(stdout, " # ["); + for (i=0; i<5; i++) fprintf(stdout, "%d", (rs_data->crc>>i)&1); + fprintf(stdout, "]"); + + if (rs_data->ecc >= 0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); + if (rs_data->ecc > 0) fprintf(stdout, " (%d)", rs_data->ecc); + + fprintf(stdout, "\n"); + + return 0; +} + +void print_frame(rs_data_t *rs_data) { + int i; + + for (i = rs_data->pos; i < rs_data->frame_len; i++) { + rs_data->frame_bytes[i] = 0; + } + + if (option_verbose) fprintf(stdout, "\n"); // fflush(stdout); + + (rs_data->rs_process)(rs_data, option_raw, option_verbose); + + if (option_raw) { + for (i = 0; i < rs_data->pos; i++) { + fprintf(stdout, "%02x", rs_data->frame_bytes[i]); + } + + if (rs_data->ecc >= 0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); + if (rs_data->ecc > 0) fprintf(stdout, " (%d)", rs_data->ecc); + + fprintf(stdout, "\n"); + } + else { + print_position(rs_data); + } +} + +int main(int argc, char *argv[]) { + + FILE *fp = NULL; + char *fpname = NULL; + char *bitbuf = NULL; + int bit_count = 0, + header_found = 0, + frmlen = 0; + int i, bit, len; + int err = 0; + + + setbuf(stdout, NULL); + + fpname = argv[0]; + ++argv; + while ((*argv) && (!wavloaded)) { + if ( (strcmp(*argv, "-h") == 0) || (strcmp(*argv, "--help") == 0) ) { + fprintf(stderr, "%s [options] audio.wav\n", fpname); + fprintf(stderr, " options:\n"); + fprintf(stderr, " -v, -vx, -vv (info, aux, info/conf)\n"); + fprintf(stderr, " -r, --raw\n"); + fprintf(stderr, " -i, --invert\n"); + fprintf(stderr, " --crc (check CRC)\n"); + fprintf(stderr, " --std (std framelen)\n"); + return 0; + } + else if ( (strcmp(*argv, "-v") == 0) || (strcmp(*argv, "--verbose") == 0) ) { + option_verbose |= 0x1; + } + else if (strcmp(*argv, "-vx") == 0) { option_verbose |= 0x2; } + else if (strcmp(*argv, "-vv") == 0) { option_verbose |= 0x3; } + else if (strcmp(*argv, "--crc") == 0) { option_crc = 1; } + else if ( (strcmp(*argv, "-r") == 0) || (strcmp(*argv, "--raw") == 0) ) { + option_raw = 1; + } + else if ( (strcmp(*argv, "-i") == 0) || (strcmp(*argv, "--invert") == 0) ) { + option_inv = 1; + } + else if (strcmp(*argv, "--std" ) == 0) { frmlen = 320; } // NDATA_LEN + else if (strcmp(*argv, "--std2") == 0) { frmlen = 518; } // FRAME_LEN + else if (strcmp(*argv, "--sat") == 0) { option_sat = 1; option_verbose |= 0x100; } + 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; + + + rs_data_t rs41data = {{0}}; + rs_data_t *rs_data = &rs41data; + rs_data->input = 8; + err = init_rs41data(rs_data); + if (err) goto error_tag; + frame = rs_data->frame_bytes; + if (frmlen == 0) frmlen = rs_data->frame_len; + + err = read_wav_header(fp, rs_data); + if (err) goto error_tag; + + bitbuf = calloc(rs_data->bits, 1); + if (bitbuf == NULL) { + err = -1; + goto error_tag; + } + + rs_data->pos = rs_data->frame_start; + + while (!read_bits_fsk(fp, &bit, &len, option_inv)) { + + if (len == 0) { // reset_frame(); + if (rs_data->pos > rs_data->pos_min) { + print_frame(rs_data); + bit_count = 0; + rs_data->pos = rs_data->frame_start; + header_found = 0; + } + //inc_bufpos(); + //buf[bufpos] = 'x'; + continue; // ... + } + + for (i = 0; i < len; i++) { + + inc_bufpos(rs_data); + rs_data->buf[rs_data->bufpos] = 0x30 + bit; // Ascii + + if (!header_found) { + if (compare(rs_data) >= rs_data->header_len) header_found = 1; + } + else { + + if (rs_data->input < 8) { + rs_data->frame_rawbits[rs_data->bits*rs_data->pos + bit_count] = 0x30 + bit; + } + + bitbuf[bit_count] = bit; + bit_count++; + + if (bit_count == rs_data->bits) { + bit_count = 0; + if (rs_data->input == 8) { + frame[rs_data->pos] = rs_data->bits2byte(rs_data, bitbuf); + } + rs_data->pos++; + if (rs_data->pos == frmlen) { + print_frame(rs_data); + rs_data->pos = rs_data->frame_start; + header_found = 0; + } + } + } + + } + } + + free(bitbuf); + bitbuf = NULL; + +error_tag: + fclose(fp); + free_rs41data(rs_data); + + return err; +} + diff --git a/rs_module/rs_main92.c b/rs_module/rs_main92.c new file mode 100644 index 0000000..32669ce --- /dev/null +++ b/rs_module/rs_main92.c @@ -0,0 +1,233 @@ + + +#include +#include +#include +#include + +#include "rs_data.h" +#include "rs_demod.h" +#include "rs_datum.h" +#include "rs_rs92.h" + + +int option_verbose = 0, // ausfuehrliche Anzeige + option_raw = 0, // rohe Frames + option_inv = 0, // invertiert Signal + option_crc = 0, // check CRC + option_sat = 0, // GPS sat data + wavloaded = 0; + + +ui8_t *frame = NULL; + +int print_position(rs_data_t *rs_data) { + +// option_crc: check block-crc + + fprintf(stdout, "[%5d] ", rs_data->frnr); + +if ( option_crc==0 || ( option_crc && (rs_data->crc & 0x7)==0 ) ) +{ + fprintf(stdout, "(%s) ", rs_data->SN); + + fprintf(stdout, "%s ", weekday[rs_data->wday]); + fprintf(stdout, "%04d-%02d-%02d %02d:%02d:%06.3f", + rs_data->year, rs_data->month, rs_data->day, + rs_data->hr, rs_data->min, rs_data->sec); + if (option_verbose) fprintf(stdout, " (W %d)", (rs_data->GPS).week); + + fprintf(stdout, " "); + fprintf(stdout, " lat: %.5f ", (rs_data->GPS).lat); + fprintf(stdout, " lon: %.5f ", (rs_data->GPS).lon); + fprintf(stdout, " alt: %.2f ", (rs_data->GPS).alt); + fprintf(stdout," vH: %4.1f D: %5.1f° vV: %3.1f ", (rs_data->GPS).vH, (rs_data->GPS).vD, (rs_data->GPS).vU); +} + int i; + fprintf(stdout, " # ["); + for (i=0; i<4; i++) fprintf(stdout, "%d", (rs_data->crc>>i)&1); + fprintf(stdout, "]"); + + if (rs_data->ecc >= 0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); + if (rs_data->ecc > 0) fprintf(stdout, " (%d)", rs_data->ecc); + + fprintf(stdout, "\n"); + + return 0; +} + +void print_frame(rs_data_t *rs_data) { + int i; + + for (i = rs_data->pos; i < rs_data->frame_len; i++) { + rs_data->frame_bytes[i] = 0; + } + + if (option_verbose) fprintf(stdout, "\n"); // fflush(stdout); + + (rs_data->rs_process)(rs_data, option_raw, option_verbose); + + if (option_raw) { + for (i = 0; i < rs_data->pos; i++) { + fprintf(stdout, "%02x", rs_data->frame_bytes[i]); + } + + if (rs_data->ecc >= 0) fprintf(stdout, " [OK]"); else fprintf(stdout, " [NO]"); + if (rs_data->ecc > 0) fprintf(stdout, " (%d)", rs_data->ecc); + + fprintf(stdout, "\n"); + } + else { + print_position(rs_data); + } +} + +int main(int argc, char *argv[]) { + + FILE *fp = NULL; + char *fpname = NULL; + char *bitbuf = NULL; + int bit_count = 0, + header_found = 0, + frmlen = 0; + int i, bit, len; + int err = 0; + + int orbdata = 0; + char *eph_file = NULL; + + setbuf(stdout, NULL); + + fpname = argv[0]; + ++argv; + while ((*argv) && (!wavloaded)) { + if ( (strcmp(*argv, "-h") == 0) || (strcmp(*argv, "--help") == 0) ) { + fprintf(stderr, "%s [options] audio.wav\n", fpname); + fprintf(stderr, " options:\n"); + fprintf(stderr, " -v, -vx, -vv (info, aux, info/conf)\n"); + fprintf(stderr, " -r, --raw\n"); + fprintf(stderr, " -i, --invert\n"); + fprintf(stderr, " --crc (check CRC)\n"); + fprintf(stderr, " --std (std framelen)\n"); + return 0; + } + else if ( (strcmp(*argv, "-v") == 0) || (strcmp(*argv, "--verbose") == 0) ) { + option_verbose |= 0x1; + } + else if (strcmp(*argv, "-vx") == 0) { option_verbose |= 0x2; } + else if (strcmp(*argv, "-vv") == 0) { option_verbose |= 0x3; } + else if (strcmp(*argv, "--crc") == 0) { option_crc = 1; } + else if ( (strcmp(*argv, "-r") == 0) || (strcmp(*argv, "--raw") == 0) ) { + option_raw = 1; + } + else if ( (strcmp(*argv, "-i") == 0) || (strcmp(*argv, "--invert") == 0) ) { + option_inv = 1; + } + else if (strcmp(*argv, "--sat") == 0) { option_sat = 1; option_verbose |= 0x100; } + else if ( (strcmp(*argv, "-a") == 0) || (strcmp(*argv, "--almanac") == 0) ) { + ++argv; + eph_file = *argv; + if (eph_file) orbdata = 1; + else return -1; + } + else if ( (strcmp(*argv, "-e") == 0) || (strncmp(*argv, "--ephem", 7) == 0) ) { + ++argv; + eph_file = *argv; + if (eph_file) orbdata = 2; + else return -1; + } + else { + fp = fopen(*argv, "rb"); + if (fp == NULL) { + fprintf(stderr, "%s konnte nicht geoeffnet werden\n", *argv); + return -1; + } + wavloaded = 1; + } + ++argv; + } + if (!wavloaded) fp = stdin; + + + rs_data_t rs92data = {{0}}; + rs_data_t *rs_data = &rs92data; + rs_data->input = 8; + err = init_rs92data(rs_data, orbdata, eph_file); + if (err) goto error_tag; + frame = rs_data->frame_bytes; + if (frmlen == 0) frmlen = rs_data->frame_len; + + err = read_wav_header(fp, rs_data); + if (err) goto error_tag; + + bitbuf = calloc(rs_data->bits, 1); + if (bitbuf == NULL) { + err = -1; + goto error_tag; + } + + rs_data->pos = rs_data->frame_start; + + while (!read_bits_fsk(fp, &bit, &len, option_inv)) { + + if (len == 0) { // reset_frame(); + if (rs_data->pos > rs_data->pos_min) { + print_frame(rs_data); +/* + bit_count = 0; + rs_data->pos = rs_data->frame_start; + header_found = 0; +*/ + } + bit_count = 0; + rs_data->pos = rs_data->frame_start; + header_found = 0; + //inc_bufpos(rs_data); + //buf[bufpos] = 'x'; + continue; // ... + } + + for (i = 0; i < len; i++) { + + inc_bufpos(rs_data); + rs_data->buf[rs_data->bufpos] = 0x30 + bit; // Ascii + + if (!header_found) { + if (compare(rs_data) >= rs_data->header_len) header_found = 1; + } + else { + + if (rs_data->input < 8) { + rs_data->frame_rawbits[rs_data->bits*rs_data->pos + bit_count] = 0x30 + bit; + } + + bitbuf[bit_count] = bit; + bit_count++; + + if (bit_count == rs_data->bits) { + bit_count = 0; + if (rs_data->input == 8) { + frame[rs_data->pos] = rs_data->bits2byte(rs_data, bitbuf); + } + rs_data->pos++; + if (rs_data->pos == frmlen) { + print_frame(rs_data); + rs_data->pos = rs_data->frame_start; + header_found = 0; + } + } + } + + } + } + + free(bitbuf); + bitbuf = NULL; + +error_tag: + fclose(fp); + free_rs92data(rs_data); + + return err; +} + diff --git a/rs_module/rs_rs41.c b/rs_module/rs_rs41.c new file mode 100644 index 0000000..84e1491 --- /dev/null +++ b/rs_module/rs_rs41.c @@ -0,0 +1,893 @@ + +#include +#include +#include +#include + +#include "rs_data.h" +#include "rs_datum.h" +#include "rs_bch_ecc.h" + + +/* + Vaisala data whitening + + LFSR: ab i=8 (mod 64): + m[16+i] = m[i] ^ m[i+2] ^ m[i+4] ^ m[i+6] + ________________3205590EF944C6262160C2EA795D6DA15469470CDCE85CF1 + F776827F0799A22C937C3063F5102E61D0BCB4B606AAF423786E3BAEBF7B4CC196833E51B1490898 + + uint16 y[]: + y[i+8] = y[i] ^ y[i+1] ^ y[i+2] ^ y[i+3] +*/ + +#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}; + +static // 10 B6 CA 11 22 96 12 F8 | +char headerbits_rs41[] = "0000100001101101010100111000100001000100011010010100100000011111"; + +static +ui8_t headerbytes_rs41[] = { 0x86, 0x35, 0xf4, 0x40, 0x93, 0xdf, 0x1a, 0x60}; // = xorbyte(xframe) + //xframe[] = { 0x10, 0xB6, 0xCA, 0x11, 0x22, 0x96, 0x12, 0xF8} = xorbyte( frame) + +#define NDATA_LEN 320 // std framelen 320 +#define XDATA_LEN 198 +#define FRAME_LEN (NDATA_LEN+XDATA_LEN) // max framelen 518 + +#define BAUD 4800 +#define BITS 8 +#define BITFRAME_LEN (FRAME_LEN*BITS) +#define RAWBITFRAME_LEN (BITFRAME_LEN) + + +// ------------------------------------------------------------- + +static ui32_t u4(ui8_t *bytes) { // 32bit unsigned int + ui32_t val = 0; + memcpy(&val, bytes, 4); + // val = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16) | (bytes[3]<<24); + 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 ui32_t u2(ui8_t *bytes) { // 16bit unsigned int + return bytes[0] | (bytes[1]<<8); +} + +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 -= 0x1000000; + return val24; +} + +static int crc16(ui8_t bytes[], int len) { + int crc16poly = 0x1021; + int rem = 0xFFFF, i, j; + int byte; + + //if (start+len >= FRAME_LEN) return -1; + + for (i = 0; i < len; i++) { + byte = bytes[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 ui32_t rs41_check_CRC(rs_data_t *rs_data, ui32_t pos, ui32_t pck) { + ui32_t crclen = 0, + crcdat = 0; + int ret = 0; + + // ((frame[pos]<<8) | frame[pos+1]) != pck ? // caution: variable block length + if ( rs_data->frame_bytes[pos] != ((pck>>8) & 0xFF) ) { + ret = 0x10000; + } + + crclen = rs_data->frame_bytes[pos+1]; + if (pos + crclen + 4 > rs_data->frame_len) ret |= 1; + else { + crcdat = u2((rs_data->frame_bytes)+pos+2+crclen); + if ( crcdat != crc16((rs_data->frame_bytes)+pos+2, crclen) ) { + ret |= 1; // CRC NO + } + //else { }; // CRC OK + } + + return ret; +} + +// ------------------------------------------------------------- + +/* + Pos: SubHeader, 1+1 byte (ID+LEN) +0x039: 7928 FrameNumber+SondeID + +(0x050: 0732 CalFrames 0x00..0x32) +0x065: 7A2A PTU +0x093: 7C1E GPS1: RXM-RAW (0x02 0x10) Week, TOW, Sats +0x0B5: 7D59 GPS2: RXM-RAW (0x02 0x10) pseudorange, doppler +0x112: 7B15 GPS3: NAV-SOL (0x01 0x06) ECEF-POS, ECEF-VEL +0x12B: 7611 00 +0x12B: 7Exx AUX-xdata +*/ + +#define crc_FRAME (1<<0) +#define xor_FRAME 0x1713 // ^0x6E3B=0x7928 +#define pck_FRAME 0x7928 +#define pos_FRAME 0x039 +#define pos_FrameNb 0x03B // 2 byte +#define pos_SondeID 0x03D // 8 byte +#define pos_CalData 0x052 // 1 byte, counter 0x00..0x32 +#define pos_Calfreq 0x055 // 2 byte, calfr 0x00 +#define pos_Calburst 0x05E // 1 byte, calfr 0x02 +// ? #define pos_Caltimer 0x05A // 2 byte, calfr 0x02 ? +#define pos_CalRSTyp 0x05B // 8 byte, calfr 0x21 (+2 byte in 0x22?) + // weitere chars in calfr 0x22/0x23; weitere ID + +#define crc_PTU (1<<1) +#define 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 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_GPSecefP 0x114 // 3*4 byte ecefX,ecefY,ecefZ +#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 + + +static addData_Vaisala_t rs41_addData; + + +static double c = 299.792458e6; +static double L1 = 1575.42e6; + +static int rs41_get_SatData(rs_data_t *rs_data, int verbose) { + int i, n; + int sv; + ui32_t minPR; + int Nfix; + double pDOP, sAcc; + ui32_t tow; + ui32_t ecefP[3]; + i16_t ecefV[3]; + + ui8_t *frame = rs_data->frame_bytes; + addData_Vaisala_t *rs41_add = rs_data->addData; + + tow = u4(frame+pos_GPSiTOW); + minPR = u4(frame+pos_minPR); + + (rs41_add->sat).tow = tow; + for (i = 0; i < 12; i++) { + n = i*7; + sv = frame[pos_satsN+2*i]; + if (sv == 0xFF) break; + (rs41_add->sat).prn[i] = sv; + (rs41_add->sat).pseudorange[i] = u4(frame+pos_dataSats+n)/100.0 + minPR; + (rs41_add->sat).doppler[i] = -i3(frame+pos_dataSats+n+4)/100.0*L1/c; + } + n = i; + for (i = n; i < 12; i++) { + (rs41_add->sat).prn[i] = 0; + (rs41_add->sat).pseudorange[i] = 0.0; + (rs41_add->sat).doppler[i] = 0.0; + i++; + } + + // ECEF-pos + for (i = 0; i < 3; i++) { + ecefP[i] = (i32_t)u4(frame+pos_GPSecefP+4*i); + (rs41_add->sat).pos_ecef[i] = ecefP[i] / 100.0; + } + // ECEF-vel + for (i = 0; i < 3; i++) { + ecefV[i] = (i16_t)u2(frame+pos_GPSecefV+2*i); + (rs41_add->sat).vel_ecef[i] = ecefV[i] / 100.0; + } + + Nfix = frame[pos_numSats]; + sAcc = frame[pos_sAcc]/10.0; + pDOP = frame[pos_pDOP]/10.0; + + (rs41_add->sat).Nfix = Nfix; + (rs41_add->sat).pDOP = pDOP; + (rs41_add->sat).sAcc = sAcc; + + + if (verbose) { + + fprintf(stdout, "[%5d]\n", u2(frame+pos_FrameNb)); + + fprintf(stdout, "iTOW: 0x%08X", tow); + fprintf(stdout, " week: 0x%04X", u2(frame+pos_GPSweek)); + fprintf(stdout, "\n"); + fprintf(stdout, "minPR: %d", minPR); + fprintf(stdout, "\n"); + + for (i = 0; i < n; i++) { + fprintf(stdout, " SV: %2d # ", (rs41_add->sat).prn[i]); + fprintf(stdout, "prMes: %.1f", (rs41_add->sat).pseudorange[i]); + fprintf(stdout, " "); + fprintf(stdout, "doMes: %.1f", (rs41_add->sat).doppler[i]); + fprintf(stdout, "\n"); + } + + fprintf(stdout, "ECEF-POS: (%d,%d,%d)\n", ecefP[0], ecefP[1], ecefP[2]); + fprintf(stdout, "ECEF-VEL: (%d,%d,%d)\n", ecefV[0], ecefV[1], ecefV[2]); + + fprintf(stdout, "numSatsFix: %2d sAcc: %.1f pDOP: %.1f\n", Nfix, sAcc, pDOP); + + fprintf(stdout, "CRC: "); + fprintf(stdout, " %04X", pck_GPS1); + if (rs41_check_CRC(rs_data, pos_GPS1, pck_GPS1)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); + fprintf(stdout, " %04X", pck_GPS2); + if (rs41_check_CRC(rs_data, pos_GPS2, pck_GPS2)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); + fprintf(stdout, " %04X", pck_GPS3); + if (rs41_check_CRC(rs_data, pos_GPS3, pck_GPS3)==0) fprintf(stdout, "[OK]"); else fprintf(stdout, "[NO]"); + + fprintf(stdout, "\n"); + } + + return 0; +} + + +static int rs41_get_FrameNb(rs_data_t *rs_data) { + ui8_t *frnr_bytes = NULL; + + frnr_bytes = (rs_data->frame_bytes)+pos_FrameNb; + rs_data->frnr = frnr_bytes[0] | (frnr_bytes[1] << 8); + + return 0; +} + +static int rs41_get_SondeID(rs_data_t *rs_data) { + int i; + ui8_t byte; + ui8_t sondeid_bytes[8]; + + for (i = 0; i < 8; i++) { + byte = rs_data->frame_bytes[pos_SondeID + i]; + if ((byte < 0x20) || (byte > 0x7E)) return -1; + sondeid_bytes[i] = byte; + } + + for (i = 0; i < 8; i++) { + rs_data->SN[i] = sondeid_bytes[i]; + } + rs_data->SN[8] = '\0'; + + return 0; +} + +#define LEN_CAL 16 +static int rs41_get_Cal(rs_data_t *rs_data, int verbose) { + int i; + unsigned byte; + ui8_t calfr = 0; + ui8_t burst = 0; + ui16_t fw = 0; + int freq = 0, f0 = 0, f1 = 0; + char sondetyp[9]; + int crc = 0; + + ui8_t *frame = rs_data->frame_bytes; + ui8_t *calbytes = rs_data->frame_bytes+pos_CalData+1; + addData_Vaisala_t *rs41_cal = rs_data->addData; + + calfr = frame[pos_CalData]; + + crc = rs41_check_CRC(rs_data, pos_FRAME, pck_FRAME); + + if (crc==0 && strncmp(rs41_cal->SN, rs_data->SN, 8)!=0) { + memset(rs41_cal, 0, sizeof(rs41_cal)); + strncpy(rs41_cal->SN, rs_data->SN, 9); + } + + if (crc == 0) { + if (rs41_cal->bytes[calfr][LEN_CAL] == 0) { + for (i = 0; i < LEN_CAL; i++) { + rs41_cal->bytes[calfr][i] = calbytes[i]; + } + rs41_cal->bytes[calfr][LEN_CAL] = 1; + } + } + + + if (calfr == 0x00) { + byte = frame[pos_Calfreq] & 0xC0; // erstmal nur oberste beiden bits + f0 = (byte * 10) / 64; // 0x80 -> 1/2, 0x40 -> 1/4 ; dann mal 40 + byte = frame[pos_Calfreq+1]; + f1 = 40 * byte; + freq = 400000 + f1+f0; // kHz; + if (crc == 0) rs_data->freq = freq; // crc == rs_data->crc & crc_FRAME + } + + if (calfr == 0x01) { + fw = frame[pos_CalData+6] | (frame[pos_CalData+7]<<8); + } + + if (calfr == 0x02) { + byte = frame[pos_Calburst]; + burst = byte; // fw >= 0x4ef5, BK irrelevant? (killtimer in 0x31?) + } + + if (calfr == 0x21) { // eventuell noch zwei bytes in 0x22 + for (i = 0; i < 9; i++) sondetyp[i] = 0; + for (i = 0; i < 8; i++) { + byte = frame[pos_CalRSTyp + i]; + if ((byte >= 0x20) && (byte < 0x7F)) sondetyp[i] = byte; + else if (byte == 0x00) sondetyp[i] = '\0'; + } + } + + if (verbose) { + fprintf(stdout, "[%5d] ", rs_data->frnr); + fprintf(stdout, "0x%02x: ", calfr); + for (i = 0; i < LEN_CAL; i++) { + fprintf(stdout, "%02x ", calbytes[i]); + } + if (crc == 0) fprintf(stdout, "[OK]"); + else fprintf(stdout, "[NO]"); + fprintf(stdout, " "); + switch (calfr) { + case 0x00: fprintf(stdout, ": fq %d ", freq); break; + case 0x01: fprintf(stdout, ": fw 0x%04x ", fw); break; + case 0x02: fprintf(stdout, ": BK %02X ", burst); break; + case 0x21: fprintf(stdout, ": %s ", sondetyp); break; + } + fprintf(stdout, "\n"); + } + + return 0; +} + + +static int rs41_get_PTUmeas(rs_data_t *rs_data) { + int i; + ui32_t measdata[12]; + ui8_t *frame = rs_data->frame_bytes; + + // 4*3 (u)int24 + for (i = 0; i < 12; i++) { + measdata[i] = u3(frame+pos_PTU+2+3*i); + } + + if (0) { + printf("\n"); + printf("1: %8d %8d %8d", measdata[ 0], measdata[ 1], measdata[ 2]); // T? + printf(" # "); + printf("2: %8d %8d %8d", measdata[ 3], measdata[ 4], measdata[ 5]); // H1? + printf(" # "); + printf("3: %8d %8d %8d", measdata[ 6], measdata[ 7], measdata[ 8]); // H2? + printf(" # "); + printf("4: %8d %8d %8d", measdata[ 9], measdata[10], measdata[11]); // P? + printf("\n"); + } + + + // calibration data: float32 poly-coeffs in cal/conf-blocks + + return 0; +} + + +static int rs41_get_GPSweek(rs_data_t *rs_data) { + ui8_t *gpsweek_bytes; + int gpsweek; + + gpsweek_bytes = (rs_data->frame_bytes)+pos_GPSweek; + + gpsweek = gpsweek_bytes[0] + (gpsweek_bytes[1] << 8); + //if (gpsweek < 0) { rs_data->week = -1; return -1; } // (short int) + (rs_data->GPS).week = gpsweek; + + return 0; +} + + +static int rs41_get_GPStime(rs_data_t *rs_data) { + ui8_t *gpstime_bytes; + ui32_t gpstime = 0, // 32bit + day, ms; + + gpstime_bytes = (rs_data->frame_bytes)+pos_GPSiTOW; + + memcpy(&gpstime, gpstime_bytes, 4); + (rs_data->GPS).msec = gpstime; + + ms = gpstime % 1000; + gpstime /= 1000; + + + day = (gpstime / (24 * 3600)) % 7; + rs_data->wday = day; + + gpstime %= (24*3600); + + rs_data->hr = gpstime / 3600; + rs_data->min = (gpstime % 3600) / 60; + rs_data->sec = gpstime % 60 + ms/1000.0; + + return 0; +} + + +#define EARTH_a 6378137.0 +#define EARTH_b 6356752.31424518 +#define EARTH_a2_b2 (EARTH_a*EARTH_a - EARTH_b*EARTH_b) + +static 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 int 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.0/M_PI; + *lon = lam*180.0/M_PI; + + return 0; +} + +static int rs41_get_GPSkoord(rs_data_t *rs_data) { + int k; + ui8_t *gpsPos = NULL; + int XYZ; // signed 32bit + double P[3], lat, lon, alt; + ui8_t *gpsVel = NULL; + short vel16; // signed 16bit + double V[3], phi, lam, dir; + int ret = 0; + + ui8_t *frame = rs_data->frame_bytes; + + for (k = 0; k < 3; k++) { + + gpsPos = frame + pos_GPSecefP + 4*k; + memcpy(&XYZ, gpsPos, 4); + P[k] = XYZ / 100.0; + + gpsVel = frame + pos_GPSecefV + 2*k; + vel16 = gpsVel[0] | gpsVel[1] << 8; + V[k] = vel16 / 100.0; + + } + + + // ECEF-Position + ecef2elli(P, &lat, &lon, &alt); + (rs_data->GPS).lat = lat; + (rs_data->GPS).lon = lon; + (rs_data->GPS).alt = alt; + if ((alt < -1000) || (alt > 80000)) ret = -3; + + + // ECEF-Velocities + // ECEF-Vel -> NorthEastUp + phi = lat*M_PI/180.0; + lam = lon*M_PI/180.0; + (rs_data->GPS).vN = -V[0]*sin(phi)*cos(lam) - V[1]*sin(phi)*sin(lam) + V[2]*cos(phi); + (rs_data->GPS).vE = -V[0]*sin(lam) + V[1]*cos(lam); + (rs_data->GPS).vU = V[0]*cos(phi)*cos(lam) + V[1]*cos(phi)*sin(lam) + V[2]*sin(phi); + + // NEU -> HorDirVer + (rs_data->GPS).vH = sqrt((rs_data->GPS).vN*(rs_data->GPS).vN + (rs_data->GPS).vE*(rs_data->GPS).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((rs_data->GPS).vE, (rs_data->GPS).vN) * 180.0 / M_PI; + if (dir < 0) dir += 360.0; + (rs_data->GPS).vD = dir; + + return ret; +} + +// ------------------------------------------------------------- + + +static int rs41_get_FrameConf(rs_data_t *rs_data, int verbose) { + int err; + + err = rs41_check_CRC(rs_data, pos_FRAME, pck_FRAME); + if (err) rs_data->crc |= crc_FRAME; + + rs41_get_FrameNb(rs_data); + rs41_get_SondeID(rs_data); + rs41_get_Cal(rs_data, verbose); + + return err; +} + +static int rs41_get_PTU(rs_data_t *rs_data) { + int err; + + err = rs41_check_CRC(rs_data, pos_PTU, pck_PTU); + if (err) rs_data->crc |= crc_PTU; + //else + { + rs41_get_PTUmeas(rs_data); + } + return err; +} + +static int rs41_get_GPS1(rs_data_t *rs_data) { + int err; + + err = rs41_check_CRC(rs_data, pos_GPS1, pck_GPS1); + if (err) rs_data->crc |= crc_GPS1; + //else + { + rs41_get_GPSweek(rs_data); + rs41_get_GPStime(rs_data); + } + + Gps2Date(rs_data); + + return err; +} + +static int rs41_get_GPS2(rs_data_t *rs_data, int verbose) { + int err; + + err = rs41_check_CRC(rs_data, pos_GPS2, pck_GPS2); + if (err) rs_data->crc |= crc_GPS2; + + rs41_get_SatData(rs_data, verbose); + + return err; +} + +static int rs41_get_GPS3(rs_data_t *rs_data) { + int err; + + err = rs41_check_CRC(rs_data, pos_GPS3, pck_GPS3); + if (err) rs_data->crc |= crc_GPS3; + + rs41_get_GPSkoord(rs_data); + + return err; +} + + +static int rs41_get_Aux(rs_data_t *rs_data) { +// +// "Ozone Sounding with Vaisala Radiosonde RS41" user's guide +// + int i, auxlen, auxcrc, count7E, pos7E, err; + ui8_t *frame = rs_data->frame_bytes; + + count7E = 0; + pos7E = pos_AUX; + + // 7Exx: xdata + while ( pos7E < rs_data->frame_len && frame[pos7E] == 0x7E ) { + + auxlen = frame[pos7E+1]; + auxcrc = frame[pos7E+2+auxlen] | (frame[pos7E+2+auxlen+1]<<8); + + if (count7E == 0) fprintf(stdout, "# xdata = "); + else fprintf(stdout, " # "); + + err = rs41_check_CRC(rs_data, pos7E, frame[pos7E]); + if (err) rs_data->crc |= crc_AUX; + + if ( auxcrc == crc16(frame+pos7E+2, auxlen) ) { + //fprintf(stdout, " # %02x : ", frame[pos_AUX+2]); + for (i = 1; i < auxlen; i++) { + fprintf(stdout, "%c", frame[pos7E+2+i]); + } + count7E++; + pos7E += 2+auxlen+2; + } + else pos7E = rs_data->frame_len; + } + if (count7E > 0) fprintf(stdout, "\n"); + + + err = rs41_check_CRC(rs_data, pos7E, 0x7600); + if (err) rs_data->crc |= crc_ZERO; + + + return count7E; +} + + +// ------------------------------------------------------------- + +// +// Reed-Solomon error correction ------------------------------- +// + +rs_ecccfg_t cfg_rs41ecc = { + .typ= 41, + .msglen= (320-56)/2, // 132..231 <= rs_K=231 + .msgpos= 56, + .parpos= 8, + .hdrlen= 8, + .frmlen= 320 // 320..518 + }; + +#define rs_N 255 +#define rs_R 24 +#define rs_K (rs_N-rs_R) + +static int rs41_ecc(rs_data_t *rs_data) { +// 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]; + + ui32_t frmlen = rs_data->pos; + ui8_t *frame = rs_data->frame_bytes; + + // frmlen <= cfg_rs41ecc.frmlen; // = 518 + + if (frmlen > rs_data->frame_len) frmlen = rs_data->frame_len; + cfg_rs41ecc.frmlen = frmlen; + cfg_rs41ecc.msglen = (frmlen-cfg_rs41ecc.msgpos)/2; // msgpos=56; + leak = frmlen % 2; + + for (i = frmlen; i < rs_data->frame_len; i++) frame[i] = 0; // FRAME_LEN-HDR = 510 = 2*255 + // memset(cw1/2, 0, rs_N); + + for (i = 0; i < rs_R; i++) cw1[i] = frame[cfg_rs41ecc.parpos+i ]; + for (i = 0; i < rs_R; i++) cw2[i] = frame[cfg_rs41ecc.parpos+i+rs_R]; + for (i = 0; i < rs_K; i++) cw1[rs_R+i] = frame[cfg_rs41ecc.msgpos+2*i ]; + for (i = 0; i < rs_K; i++) cw2[rs_R+i] = frame[cfg_rs41ecc.msgpos+2*i+1]; + + errors1 = rs_decode(cw1, err_pos1, err_val1); + errors2 = rs_decode(cw2, err_pos2, err_val2); + + // Wenn Fehler im 00-padding korrigiert wurden, + // war entweder der frame zu kurz, oder + // Fehler wurden falsch korrigiert; + // allerdings ist bei t=12 die Wahrscheinlichkeit, + // dass falsch korrigiert wurde mit 1/t! sehr gering. + + // check CRC32 + // CRC32 OK: + //for (i = 0; i < cfg_rs41ecc.hdrlen; i++) frame[i] = data[i]; + for (i = 0; i < rs_R; i++) { + frame[cfg_rs41ecc.parpos+ i] = cw1[i]; + frame[cfg_rs41ecc.parpos+rs_R+i] = cw2[i]; + } + for (i = 0; i < rs_K; i++) { // cfg_rs41ecc.msglen <= rs_K + frame[cfg_rs41ecc.msgpos+ 2*i] = cw1[rs_R+i]; + frame[cfg_rs41ecc.msgpos+1+2*i] = cw2[rs_R+i]; + } + if (leak) { + frame[cfg_rs41ecc.msgpos+2*i] = cw1[rs_R+i]; + } + + + ret = errors1 + errors2; + if (errors1 < 0 || errors2 < 0) ret = -1; + + return ret; +} + +// ------------------------------------------------------------- + +// +// process bits/bytes +// + +static int rs41_framebits2bytes(rs_data_t *rs_data) { + + char *rawframebits = rs_data->frame_rawbits; + ui8_t *frame = rs_data->frame_bytes; + + ui32_t endpos = rs_data->pos; + + for (rs_data->pos = 0; rs_data->pos < endpos; rs_data->pos++) { + frame[rs_data->pos] = rs_data->bits2byte(rs_data, rawframebits+(BITS*rs_data->pos)); + } + while (endpos < FRAME_LEN) frame[endpos++] = 0; + + return 0; +} + + +int rs41_process(void *data, int raw, int options) { + rs_data_t *rs_data = data; + int err=0, ret=0; + + if (rs_data->input < 8) { + rs41_framebits2bytes(rs_data); + } + + rs_data->ecc = rs41_ecc(rs_data); + rs_data->crc = 0; + + if ( !raw ) { + + err = 0; + ret = 0; + + ret = rs41_get_FrameConf(rs_data, options & 0x1); + err |= ret<<0; + + ret = rs41_get_PTU(rs_data); + err |= ret<<1; + + ret = rs41_get_GPS1(rs_data); + err |= ret<<2; + + ret = rs41_get_GPS2(rs_data, (options>>8) & 0xFF); + err |= ret<<3; + + ret = rs41_get_GPS3(rs_data); + err |= ret<<4; + + if (options & 0x2) { + ret = rs41_get_Aux(rs_data); + // ret = count7E; // bei crc: err |= ret<<4; + } + + } + + return err; +} + + +int rs41_xbits2byte(void *data, char bits[]) { +// 0 eq '0'=0x30 +// 1 eq '1'=0x31 + rs_data_t *rs_data = data; + int i, byte=0, d=1; + for (i = 0; i < 8; i++) { // little endian + /* for (i = 7; i >= 0; i--) { // big endian */ + if ((bits[i]&1) == 1) byte += d; + else if ((bits[i]&1) == 0) byte += 0; + //else return 0x100; + d <<= 1; + } + + return byte ^ mask[rs_data->pos % MASK_LEN]; +} + + +int init_rs41data(rs_data_t *rs_data) { + + rs_init_RS255(); + + // int in = rs_data->input; + // memset(rs_data, 0, sizeof(rs_data_t)); + // rs_data->input = in + + rs_data->baud = BAUD; + rs_data->bits = BITS; + + rs_data->header = calloc(sizeof(headerbits_rs41), 1); + if (rs_data->header == NULL) return ERROR_MALLOC; + strcpy(rs_data->header, headerbits_rs41); + rs_data->header_ofs = 24; + rs_data->header_len = 32; + + rs_data->bufpos = -1; + rs_data->buf = calloc((rs_data->header_len)+1, 1); + if (rs_data->buf == NULL) return ERROR_MALLOC; + + if (rs_data->input < 8) { + rs_data->frame_rawbits = calloc(RAWBITFRAME_LEN, 1); + if (rs_data->frame_rawbits == NULL) return ERROR_MALLOC; + strncpy(rs_data->frame_rawbits, headerbits_rs41, strlen(headerbits_rs41)); + + //rs_data->frame_bits = rs_data->frame_rawbits; + } + + rs_data->frame_bytes = calloc(FRAME_LEN, 1); + if (rs_data->frame_bytes == NULL) return ERROR_MALLOC; + memcpy(rs_data->frame_bytes, headerbytes_rs41, sizeof(headerbytes_rs41)); + + rs_data->frame_start = (rs_data->header_ofs + rs_data->header_len) / rs_data->bits; + rs_data->pos_min = pos_AUX; + rs_data->frame_len = FRAME_LEN; + + rs_data->bits2byte = rs41_xbits2byte; + rs_data->rs_process = rs41_process; + + rs_data->addData = &rs41_addData; + + return 0; +} + +int free_rs41data(rs_data_t *rs_data) { + + rs_data->header = NULL; + + free(rs_data->buf); + rs_data->buf = NULL; + + if (rs_data->input < 8) { + // memset(rs_data->frame_rawbits, 0, rs_data->RAWBITFRAME_LEN) ... + free(rs_data->frame_rawbits); + rs_data->frame_rawbits = NULL; + + //rs_data->frame_bits = NULL; + } + + // memset(rs_data->frame_bytes, 0, rs_data->FRAME_LEN) ... + free(rs_data->frame_bytes); + rs_data->frame_bytes = NULL; + + //memset(rs_data, 0, sizeof(rs_data_t)); + + return 0; +} + diff --git a/rs_module/rs_rs41.h b/rs_module/rs_rs41.h new file mode 100644 index 0000000..18bba94 --- /dev/null +++ b/rs_module/rs_rs41.h @@ -0,0 +1,14 @@ + +#ifndef RS_RS41_H +#define RS_RS41_H + + +int rs41_process(void *, int, int); +//int rs41_xbits2byte(void *, char *); + +int init_rs41data(rs_data_t *); +int free_rs41data(rs_data_t *); + + +#endif /* RS_RS41_H */ + diff --git a/rs_module/rs_rs92.c b/rs_module/rs_rs92.c new file mode 100644 index 0000000..c2cdaaa --- /dev/null +++ b/rs_module/rs_rs92.c @@ -0,0 +1,988 @@ + + +#include +#include +#include +#include + + +#include "rs_data.h" +#include "rs_datum.h" +#include "rs_bch_ecc.h" + +#include "rs_gps.c" + + + +// --- RS92-SGP: 8N1 manchester --- +static // 2A 10 +char headerbits_rs92sgp[] = "10100110011001101001" + "10100110011001101001" + "10100110011001101001" + "10100110011001101001" + "1010011001100110100110101010100110101001"; + +static +ui8_t headerbytes_rs92sgp[] = { 0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 0x10}; + + +#define FRAME_LEN 240 + +#define BAUD 4800 +#define BITS (2*(1+8+1)) // 20 (SGP/manchester 8N1) +#define BITFRAME_LEN (FRAME_LEN*BITS) +#define RAWBITFRAME_LEN (BITFRAME_LEN*2) + + +static addData_Vaisala_t rs92_addData; + + +// ------------------------------------------------------------- + +static ui32_t u4(ui8_t *bytes) { // 32bit unsigned int + ui32_t val = 0; + memcpy(&val, bytes, 4); + // val = bytes[0] | (bytes[1]<<8) | (bytes[2]<<16) | (bytes[3]<<24); + 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 ui32_t u2(ui8_t *bytes) { // 16bit unsigned int + return bytes[0] | (bytes[1]<<8); +} + +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 -= 0x1000000; + return val24; +} + +static int crc16(ui8_t bytes[], int len) { + int crc16poly = 0x1021; + int rem = 0xFFFF, i, j; + int byte; + + //if (start+len >= FRAME_LEN) return -1; + + for (i = 0; i < len; i++) { + byte = bytes[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 ui32_t rs92_check_CRC(rs_data_t *rs_data, ui32_t pos, ui32_t pck) { + ui32_t crclen = 0, + crcdat = 0; + int ret = 0; + + // ((frame[pos]<<8) | frame[pos+1]) != pck ? // caution: variable block length (rs92?) + if ( rs_data->frame_bytes[pos] != ((pck>>8) & 0xFF) ) { + ret = 0x10000; + } + + crclen = rs_data->frame_bytes[pos+1] * 2; + if (pos + crclen + 4 > rs_data->frame_len) ret |= 1; + else { + crcdat = u2((rs_data->frame_bytes)+pos+2+crclen); + if ( crcdat != crc16((rs_data->frame_bytes)+pos+2, crclen) ) { + ret |= 1; // CRC NO + } + //else { }; // CRC OK + } + + return ret; +} + +// ------------------------------------------------------------- + + +#define pck_CFG 0x6510 +#define pos_CFG 0x06 // 32 byte +#define pos_FrameNb 0x08 // 2 byte +#define pos_SondeID 0x0C // 8 byte // oder: 0x0A, 10 byte? +#define pos_CalData 0x17 // 1 byte, counter 0x00..0x1f +#define pos_Calfreq 0x1A // 2 byte, calfr 0x00 + +#define pck_PTU 0x690C +#define pos_PTU 0x2A // 24 byte + +#define pck_GPS 0x673D +#define pos_GPS 0x46 // 122 byte +#define pos_GPS_iTOW 0x48 // 4 byte +#define pos_GPS_PRN 0x4E // 12*5 bit in 8 byte +#define pos_GPS_STATUS 0x56 // 12 byte +#define pos_GPS_DATA 0x62 // 12*8 byte + +#define pck_AUX 0x6805 +#define pos_AUX 0xC4 // 2+8 byte + + +#define LEN_CFG (2*(pck_CFG & 0xFF)) +#define LEN_GPS (2*(pck_GPS & 0xFF)) +#define LEN_PTU (2*(pck_PTU & 0xFF)) + +#define crc_CFG (1<<0) +#define crc_PTU (1<<1) +#define crc_GPS (1<<2) +#define crc_AUX (1<<3) + + + +static int rs92_get_FrameNb(rs_data_t *rs_data) { + ui8_t *frnr_bytes = NULL; + + frnr_bytes = (rs_data->frame_bytes)+pos_FrameNb; + rs_data->frnr = frnr_bytes[0] | (frnr_bytes[1] << 8); + + return 0; +} + +static int rs92_get_SondeID(rs_data_t *rs_data) { + int i; + ui8_t byte; + ui8_t sondeid_bytes[10]; + + for (i = 0; i < 8; i++) { + byte = rs_data->frame_bytes[pos_SondeID + i]; + if ((byte < 0x20) || (byte > 0x7E)) return -1; + sondeid_bytes[i] = byte; + } + + for (i = 0; i < 8; i++) { + rs_data->SN[i] = sondeid_bytes[i]; + } + rs_data->SN[8] = '\0'; + + return 0; +} + + +static int rs92_get_GPStime(rs_data_t *rs_data) { + ui8_t *gpstime_bytes; + ui32_t gpstime = 0, // 32bit + day, ms; + + gpstime_bytes = (rs_data->frame_bytes)+pos_GPS_iTOW; + + memcpy(&gpstime, gpstime_bytes, 4); + (rs_data->GPS).msec = gpstime; + + ms = gpstime % 1000; + gpstime /= 1000; + + + day = (gpstime / (24 * 3600)) % 7; // besser CRC-check, da auch + //if ((day < 0) || (day > 6)) return -1; // gpssec=604800,604801 beobachtet + + if (day >=0 && day < 7) rs_data->wday = day; + gpstime %= (24*3600); + + rs_data->hr = gpstime / 3600; + rs_data->min = (gpstime % 3600) / 60; + rs_data->sec = gpstime % 60 + ms/1000.0; + + return 0; +} + + +#define LEN_CAL 16 +static int rs92_get_Cal(rs_data_t *rs_data, int verbose) { + int i; + unsigned byte; + ui8_t calfr = 0; + //ui8_t burst = 0; + int freq = 0; + ui8_t freq_bytes[2]; + int crc = 0; + + ui8_t *frame = rs_data->frame_bytes; + ui8_t *calbytes = rs_data->frame_bytes+pos_CalData+1; + addData_Vaisala_t *rs92_cal = rs_data->addData; + + calfr = frame[pos_CalData]; + + crc = rs92_check_CRC(rs_data, pos_CFG, pck_CFG); + // crc == rs_data->crc & crc_CFG ? + + if (crc==0 && strncmp(rs92_cal->SN, rs_data->SN, 8)!=0) { + memset(rs92_cal, 0, sizeof(rs92_cal)); + strncpy(rs92_cal->SN, rs_data->SN, 9); + } + + if (crc == 0) { + if (rs92_cal->bytes[calfr][LEN_CAL] == 0) { + for (i = 0; i < LEN_CAL; i++) { + rs92_cal->bytes[calfr][i] = calbytes[i]; + } + rs92_cal->bytes[calfr][LEN_CAL] = 1; + } + } + + if (calfr == 0x00) { + for (i = 0; i < 2; i++) { + freq_bytes[i] = frame[pos_Calfreq + i]; + } + byte = freq_bytes[0] + (freq_bytes[1] << 8); + freq = 400000 + 10*byte; // kHz; + rs_data->freq = freq; + } + + if (verbose) { + fprintf(stdout, "[%5d] ", rs_data->frnr); + fprintf(stdout, "0x%02x: ", calfr); + for (i = 0; i < LEN_CAL; i++) { + fprintf(stdout, "%02x ", calbytes[i]); + } + if (crc == 0) fprintf(stdout, "[OK]"); + else fprintf(stdout, "[NO]"); + fprintf(stdout, " "); + switch (calfr) { + case 0x00: fprintf(stdout, ": fq %d ", freq); break; + } + fprintf(stdout, "\n"); + } + + + return 0; +} + + +static int rs92_get_PTUmeas(rs_data_t *rs_data) { + //double T, P, H1, H2; + + int temp, pres, hum1, hum2, ref1, ref2, ref3, ref4; + + ui8_t *measdata = rs_data->frame_bytes+pos_PTU+2; + + // crc check + if ( (rs_data->crc & crc_PTU) != 0 ) return -1; + + temp = measdata[ 0] | (measdata[ 1]<<8) | (measdata[ 2]<<16); // ch1 + hum1 = measdata[ 3] | (measdata[ 4]<<8) | (measdata[ 5]<<16); // ch2 + hum2 = measdata[ 6] | (measdata[ 7]<<8) | (measdata[ 8]<<16); // ch3 + ref1 = measdata[ 9] | (measdata[10]<<8) | (measdata[11]<<16); // ch4 + ref2 = measdata[12] | (measdata[13]<<8) | (measdata[14]<<16); // ch5 + pres = measdata[15] | (measdata[16]<<8) | (measdata[17]<<16); // ch6 + ref3 = measdata[18] | (measdata[19]<<8) | (measdata[20]<<16); // ch7 + ref4 = measdata[21] | (measdata[22]<<8) | (measdata[23]<<16); // ch8 + + + // calibration data: float32 poly-coeffs in cal/conf-blocks + + return 0; +} + +// --------------------------------------------------------------------------------------------- + +// +// GPS --------------------------------------------------------- +// + +static double d_err = 10000; +static int option_iter = 0; +static int almanac = 0, + ephem = 0; + +static EPHEM_t alm[33]; +static EPHEM_t *ephs = NULL; + +static SAT_t Sat[12]; +static ui8_t prns[12]; // PRNs in data +static ui8_t sat_status[12]; + +static int prn32toggle = 0x1, ind_prn32, prn32next; + +static int prnbits_le(ui16_t byte16, ui8_t bits[64], int block) { + int i; /* letztes bit Ueberlauf, wenn 3. PRN = 32 */ + for (i = 0; i < 15; i++) { + bits[15*block+i] = byte16 & 1; + byte16 >>= 1; + } + bits[60+block] = byte16 & 1; + return byte16 & 1; +} + +static void rs92_prn12(ui8_t *prn_le, ui8_t prns[12]) { + int i, j, d; + for (i = 0; i < 12; i++) { + prns[i] = 0; + d = 1; + for (j = 0; j < 5; j++) { + if (prn_le[5*i+j]) prns[i] += d; + d <<= 1; + } + } + ind_prn32 = 32; + for (i = 0; i < 12; i++) { + // PRN-32 overflow + if ( (prns[i] == 0) && (sat_status[i] & 0x0F) ) { // 5 bit: 0..31 + if ( ((i % 3 == 2) && (prn_le[60+i/3] & 1)) // Spalte 2 + || ((i % 3 != 2) && (prn_le[5*(i+1)] & 1)) ) { // Spalte 0,1 + prns[i] = 32; ind_prn32 = i; + } + } + else if ((sat_status[i] & 0x0F) == 0) { // erste beiden bits: 0x03 ? + prns[i] = 0; + } + } + + prn32next = 0; + if (ind_prn32 < 12) { + // PRN-32 overflow + if (ind_prn32 % 3 != 2) { // -> ind_prn32<11 // vorausgesetzt im Block folgt auf PRN-32 + if ((sat_status[ind_prn32+1] & 0x0F) && prns[ind_prn32+1] > 1) { // entweder PRN-1 oder PRN-gerade + // && prns[ind_prn32+1] != 3 ? + for (j = 0; j < ind_prn32; j++) { + if (prns[j] == (prns[ind_prn32+1]^prn32toggle) && (sat_status[j] & 0x0F)) break; + } + if (j < ind_prn32) { prn32toggle ^= 0x1; } + else { + for (j = ind_prn32+2; j < 12; j++) { + if (prns[j] == (prns[ind_prn32+1]^prn32toggle) && (sat_status[j] & 0x0F)) break; + } + if (j < 12) { prn32toggle ^= 0x1; } + } + prns[ind_prn32+1] ^= prn32toggle; + /* + // nochmal testen + for (j = 0; j < ind_prn32; j++) { if (prns[j] == prns[ind_prn32+1]) break; } + if (j < ind_prn32) prns[ind_prn32+1] = 0; + else { + for (j = ind_prn32+2; j < 12; j++) { if (prns[j] == prns[ind_prn32+1]) break; } + if (j < 12) prns[ind_prn32+1] = 0; + } + if (prns[ind_prn32+1] == 0) { prn32toggle ^= 0x1; } + */ + } + } + if (ind_prn32 < 11) prn32next = prns[ind_prn32+1]; + } +} + + +// pseudo.range = -df*pseudo.chips +// df = lightspeed/(chips/sec)/2^10 +const double df = 299792.458/1023.0/1024.0; //0.286183844 // c=299792458m/s, 1023000chips/s +// dl = L1/(chips/sec)/4 +const double dl = 1575.42/1.023/4.0; //385.0 // GPS L1 1575.42MHz=154*10.23MHz, dl=154*10/4 + + +static int rs92_get_pseudorange(rs_data_t *rs_data) { + ui32_t gpstime; + ui8_t *gpstime_bytes; + ui8_t *pseudobytes; + int chipbytes, deltabytes; // signed int32 + int i, j, k; + ui8_t bytes[4]; + ui8_t prn_le[12*5+4]; + ui16_t byte16; + + ui8_t *frame = rs_data->frame_bytes; + + // GPS-TOW in ms + gpstime_bytes = frame+pos_GPS_iTOW; + memcpy(&gpstime, gpstime_bytes, 4); + + // Sat Status + for (i = 0; i < 12; i++) { + sat_status[i] = frame[pos_GPS_STATUS + i]; + } + + // PRN-Nummern + memset(prn_le, 0, sizeof(prn_le)); // size=60+4 + for (i = 0; i < 4; i++) { + for (j = 0; j < 2; j++) { + bytes[j] = frame[pos_GPS_PRN+2*i+j]; + } + memcpy(&byte16, bytes, 2); + prnbits_le(byte16, prn_le, i); + } + rs92_prn12(prn_le, prns); + + + for (j = 0; j < 12; j++) { + + Sat[j].tow = gpstime; + Sat[j].prn = prns[j]; + Sat[j].status = sat_status[j]; + + // Pseudorange/chips + pseudobytes = frame+pos_GPS_DATA+8*j; + memcpy(&chipbytes, pseudobytes, 4); + + // delta_pseudochips / 385 + pseudobytes = frame+pos_GPS_DATA+8*j+4; + deltabytes = 0; // bzw. pseudobytes[3]=0 (24 bit); + memcpy(&deltabytes, pseudobytes, 3); + + if ( chipbytes == 0x7FFFFFFF || chipbytes == 0x55555555 ) + //or ( chipbytes > 0x10000000 && chipbytes < 0xF0000000 ) + { + chipbytes = 0; + deltabytes = 0; + Sat[j].prn = 0; + } + //0x01400000 //0x013FB0A4 + Sat[j].pseudorange = - chipbytes * df; + Sat[j].pseudorate = - deltabytes * df / dl; + + if ((Sat[j].status & 0x0F) != 0xF) Sat[j].prn = 0; + } + + + // GPS Sat Pos & Vel + if (almanac) gps_satpos_alm(rs_data, alm, gpstime/1000.0, Sat); + if (ephem) gps_satpos_rnx(rs_data, ephs, gpstime/1000.0, Sat); + + k = 0; + for (j = 0; j < 12; j++) { + if (Sat[j].prn > 0) k++; + } + + return k; +} + +static int rs92_get_GPSvel(double lat, double lon, double vel_ecef[3], + double *vH, double *vD, double *vU) { + // ECEF-Velocities + // ECEF-Vel -> NorthEastUp + double phi = lat*M_PI/180.0; + double lam = lon*M_PI/180.0; + double vN = -vel_ecef[0]*sin(phi)*cos(lam) - vel_ecef[1]*sin(phi)*sin(lam) + vel_ecef[2]*cos(phi); + double vE = -vel_ecef[0]*sin(lam) + vel_ecef[1]*cos(lam); + *vU = vel_ecef[0]*cos(phi)*cos(lam) + vel_ecef[1]*cos(phi)*sin(lam) + vel_ecef[2]*sin(phi); + // NEU -> HorDirVer + *vH = sqrt(vN*vN+vE*vE); + *vD = atan2(vE, vN) * 180.0 / M_PI; + if (*vD < 0) *vD += 360.0; + + return 0; +} + +static int rs92_get_GPSkoord(rs_data_t *rs_data, int opt_gg2) { + double lat, lon, alt, rx_cl_bias; + double vH, vD, vU; + double lat0 , lon0 , alt0 , pos0_ecef[3]; + double vH0, vD0, vU0; + double pos_ecef[3], dpos_ecef[3], + vel_ecef[3], dvel_ecef[3]; + double DOP[4] = {0,0,0,0}; + double gdop, gdop0; + //double hdop, vdop, pdop; + int j, k, n; + SAT_t Sat_B[12]; // N <= 12 + SAT_t Sat_C[12]; // 11 + int i0, i1, i2, i3; + int j0, j1, j2, j3; + double diter, diter0; + int exN = -1; + int N = 0; + + (rs_data->GPS).lat = (rs_data->GPS).lon = (rs_data->GPS).alt = 0; + + k = 0; + for (j = 0; j < 12; j++) { + if (Sat[j].prn > 0) Sat_B[k++] = Sat[j]; + } + for (j = k; j < 12; j++) Sat_B[j].prn = 0; + N = k; + + // preliminary position + // + NAV_bancroft1(N, Sat_B, pos_ecef, &rx_cl_bias); + ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt); + gdop = -1; + if (calc_DOPn(N, Sat_B, pos_ecef, DOP) == 0) { + gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]); + } + + NAV_LinP(N, Sat_B, pos_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); + if (option_iter) { + for (j = 0; j < 3; j++) pos_ecef[j] += dpos_ecef[j]; + ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt); + } + diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1], dpos_ecef[2]); + + + // Sat mit schlechten Daten suchen + if (diter > d_err) + { + if (N > 4) { // N > 5 + for (n = 0; n < N; n++) { + k = 0; + for (j = 0; j < N; j++) { + if (j != n) { + Sat_C[k] = Sat_B[j]; + k++; + } + } + for (j = 0; j < 3; j++) pos0_ecef[j] = 0; + NAV_bancroft1(N-1, Sat_C, pos0_ecef, &rx_cl_bias); + + NAV_LinP(N-1, Sat_C, pos0_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); + diter0 = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1], dpos_ecef[2]); + ecef2elli(pos0_ecef[0], pos0_ecef[1], pos0_ecef[2], &lat0, &lon0, &alt0); + + if (diter0 < d_err && diter0 < diter) { + diter = diter0; + exN = n; + } + } + } + + if (exN >= 0) { + + if ( (Sat_B[exN].prn == prn32next) && (ind_prn32 % 3 != 2) ) { + prn32toggle ^= 0x1; // wenn zuvor mit prn32next valider Fix, dann eventuell nicht aendern;q + // eventuell gleich testen + } + + for (k = exN; k < N-1; k++) { + Sat_B[k] = Sat_B[k+1]; + } + Sat_B[N-1].prn = 0; + N = N-1; + + } + else { + // 4er-Kombinationen probieren + k = N; + j0 = j1 = j2 = j3 = 0; + for (i0=0;i0= 0) + && (alt0 > -200 && alt0 < 40000) // eventuell mit vorherigen + && (vH0 < 200.0 && vU0*vU0 < 200.0*200.0) // validen Positionen vergleichen + ) { + + if ( diter0 < diter ) { + diter = diter0; + j0 = i0; j1 = i1; j2 = i2; j3 = i3; + } + } + }}}} + + if (j1 > 0) { + Sat_C[0] = Sat_B[j0]; + Sat_C[1] = Sat_B[j1]; + Sat_C[2] = Sat_B[j2]; + Sat_C[3] = Sat_B[j3]; + N = 4; + Sat_B[0] = Sat_C[0]; + Sat_B[1] = Sat_C[1]; + Sat_B[2] = Sat_C[2]; + Sat_B[3] = Sat_C[3]; + } + } + } + + + // final solution + // + // position + NAV_bancroft1(N, Sat_B, pos_ecef, &rx_cl_bias); + ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt); + // velocity + vel_ecef[0] = vel_ecef[1] = vel_ecef[2] = 0; + NAV_LinV(N, Sat_B, pos_ecef, vel_ecef, 0.0, dvel_ecef, &rx_cl_bias); + for (j=0; j<3; j++) vel_ecef[j] += dvel_ecef[j]; + rs92_get_GPSvel(lat, lon, vel_ecef, &vH, &vD, &vU); + // DOP + gdop = -1; + if (calc_DOPn(N, Sat_B, pos_ecef, DOP) == 0) { + gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]); + } + + + (rs_data->GPS).lat = lat; + (rs_data->GPS).lon = lon; + (rs_data->GPS).alt = alt; + + (rs_data->GPS).vH = vH; + (rs_data->GPS).vD = vD; + (rs_data->GPS).vU = vU; + + + addData_Vaisala_t *rs92_add = rs_data->addData; + int pDOP = -1; + if (calc_DOPn(N, Sat_B, pos_ecef, DOP) == 0) { + pDOP = sqrt(DOP[0]+DOP[1]+DOP[2]); + } + (rs92_add->sat).pDOP = pDOP; + (rs92_add->sat).Nfix = N; + for (j = 0; j < N; j++) { + (rs92_add->sat).prn[j] = Sat_B[j].prn; + (rs92_add->sat).pseudorange[j] = Sat_B[j].pseudorange; + (rs92_add->sat).doppler[j] = Sat_B[j].pseudorate; + } + + + return N; +} + + +// --------------------------------------------------------------------------------------------- + + +static int rs92_get_FrameConf(rs_data_t *rs_data, int verbose) { + int err; + + err = rs92_check_CRC(rs_data, pos_CFG, pck_CFG); + if (err) rs_data->crc |= crc_CFG; + + rs92_get_FrameNb(rs_data); + rs92_get_SondeID(rs_data); + rs92_get_Cal(rs_data, verbose); + + return err; +} + +static int rs92_get_PTU(rs_data_t *rs_data) { + int err; + + err = rs92_check_CRC(rs_data, pos_PTU, pck_PTU); + if (err) rs_data->crc |= crc_PTU; + //else + { + rs92_get_PTUmeas(rs_data); + } + return err; +} + + +static int rs92_get_GPS(rs_data_t *rs_data, int verbose) { + int err; + int k, n; + + err = rs92_check_CRC(rs_data, pos_GPS, pck_GPS); + if (err) rs_data->crc |= crc_GPS; + + rs92_get_GPStime(rs_data); + // if alm||eph: + k = rs92_get_pseudorange(rs_data); + if (k >= 4) { + n = rs92_get_GPSkoord(rs_data, verbose); + } + Gps2Date(rs_data); + + return err; +} + + +static int rs92_get_Aux(rs_data_t *rs_data, int verbose) { +// + int err; + int i; + ui32_t aux; + ui8_t *frame = rs_data->frame_bytes; + + err = rs92_check_CRC(rs_data, pos_AUX, pck_AUX); + if (err) rs_data->crc |= crc_AUX; + + if (verbose) { + fprintf(stdout, "AUX #"); + for (i = 0; i < 4; i++) { + aux = frame[pos_AUX+4+2*i] | (frame[pos_AUX+4+2*i+1]<<8); + fprintf(stdout, " %04x", aux); + } + fprintf(stdout, "\n"); + } + + return err; +} + + +// ------------------------------------------------------------- + +// +// Reed-Solomon error correction ------------------------------- +// + +rs_ecccfg_t cfg_rs92ecc = { + .typ= 92, + .msglen= 240-6-24, // 210 <= rs_K=231 + .msgpos= 6, + .parpos= 240-24, + .hdrlen= 6, + .frmlen= 240 + }; + +#define rs_N 255 +#define rs_R 24 +#define rs_K (rs_N-rs_R) + +static int rs92_ecc(rs_data_t *rs_data) { +// richtige framelen wichtig fuer 0-padding + + int i, ret = 0; + int errors; + ui8_t cw[rs_N]; + ui8_t err_pos[rs_R], + err_val[rs_R]; + + ui32_t frmlen = rs_data->pos; + ui8_t *frame = rs_data->frame_bytes; + + // frmlen <= cfg_rs92ecc.frmlen; // = 240 + int msglen = cfg_rs92ecc.msglen; // = 240-6-24=231 // msgpos=6 rs_R=24; + int msgpos = cfg_rs92ecc.msgpos; // = 6 + int parpos = cfg_rs92ecc.parpos; // = 240-24 + + while (frmlen < rs_data->frame_len) frame[frmlen++] = 0; + if (frmlen > rs_data->frame_len) frmlen = rs_data->frame_len; + + memset(cw, 0, rs_N); + for (i = 0; i < rs_R; i++) cw[i] = frame[parpos+i]; + for (i = 0; i < msglen; i++) cw[rs_R+i] = frame[msgpos+i]; + // for (i = msglen; i < rs_K; i++) cw[rs_R+i] = 0; + + errors = rs_decode(cw, err_pos, err_val); + + // 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_rs92ecc.hdrlen; i++) frame[i] = data[i]; + for (i = 0; i < rs_R; i++) { + frame[parpos+i] = cw[i]; + } + for (i = 0; i < msglen; i++) { // msglen <= rs_K + frame[msgpos+i] = cw[rs_R+i]; + } + + ret = errors; + if (errors < 0) ret = -1; + else rs_data->pos = frmlen; + + return ret; +} + +// ------------------------------------------------------------- + +// +// process bits/bytes +// + +static int rs92_framebits2bytes(rs_data_t *rs_data) { + + char *rawframebits = rs_data->frame_rawbits; + ui8_t *frame = rs_data->frame_bytes; + + ui32_t endpos = rs_data->pos; + + for (rs_data->pos = 0; rs_data->pos < endpos; rs_data->pos++) { + frame[rs_data->pos] = rs_data->bits2byte(rs_data, rawframebits+(BITS*rs_data->pos)); + } + while (endpos < FRAME_LEN) frame[endpos++] = 0; + + return 0; +} + + +int rs92_process(void *data, int raw, int options) { + rs_data_t *rs_data = data; + int err=0, ret=0; + + if (rs_data->input < 8) { + rs92_framebits2bytes(rs_data); + } + + rs_data->ecc = rs92_ecc(rs_data); + rs_data->crc = 0; + + if ( !raw ) { + + err = 0; + ret = 0; + + ret = rs92_get_FrameConf(rs_data, options & 0x1); + err |= ret<<0; + + ret = rs92_get_PTU(rs_data); + err |= ret<<1; + + ret = rs92_get_GPS(rs_data, options & 0); + err |= ret<<2; + + ret = rs92_get_Aux(rs_data, options & 0x2); + err |= ret<<3; + + } + + return err; +} + + +// manchester1 1->10,0->01: 1.bit +// manchester2 0->10,1->01: 2.bit +// RS92-SGP: 8N1 manchester2 +char manch(char *mbits) { + if (((mbits[0]&1) == 1) && ((mbits[1]&1) == 0)) return 0; + else if (((mbits[0]&1) == 0) && ((mbits[1]&1) == 1)) return 1; + else return -1; +} +int rs92_mbits2byte(void *data, char mbits[]) { +// 0 eq '0'=0x30 +// 1 eq '1'=0x31 + int i, byte=0, d=1; + int bit8[8]; + + if (manch(mbits+0) != 0) return 0x100; + for (i = 0; i < 8; i++) { + bit8[i] = manch(mbits+2*(i+1)); + } + + for (i = 0; i < 8; i++) { // little endian + /* for (i = 7; i >= 0; i--) { // big endian */ + if ((bit8[i]&1) == 1) byte += d; + else if ((bit8[i]&1) == 0) byte += 0; + //else return 0x100; + d <<= 1; + } + + return byte; +} + + +int init_rs92data(rs_data_t *rs_data, int orbdata, char *eph_file) { + + FILE *fp = NULL; + + rs_init_RS255(); + + // int in = rs_data->input; + // memset(rs_data, 0, sizeof(rs_data_t)); + // rs_data->input = in + + rs_data->baud = BAUD; + rs_data->bits = BITS; + + rs_data->header = calloc(sizeof(headerbits_rs92sgp), 1); + if (rs_data->header == NULL) return ERROR_MALLOC; + strcpy(rs_data->header, headerbits_rs92sgp); + rs_data->header_ofs = 40; + rs_data->header_len = 80; + + rs_data->bufpos = -1; + rs_data->buf = calloc((rs_data->header_len)+1, 1); + if (rs_data->buf == NULL) return ERROR_MALLOC; + + if (rs_data->input < 8) { + rs_data->frame_rawbits = calloc(RAWBITFRAME_LEN, 1); + if (rs_data->frame_rawbits == NULL) return ERROR_MALLOC; + strncpy(rs_data->frame_rawbits, headerbits_rs92sgp, strlen(headerbits_rs92sgp)); + + //rs_data->frame_bits = rs_data->frame_rawbits; + } + + rs_data->frame_bytes = calloc(FRAME_LEN, 1); + if (rs_data->frame_bytes == NULL) return ERROR_MALLOC; + memcpy(rs_data->frame_bytes, headerbytes_rs92sgp, sizeof(headerbytes_rs92sgp)); + + rs_data->frame_start = (rs_data->header_ofs + rs_data->header_len) / rs_data->bits; + rs_data->pos_min = pos_PTU; + rs_data->frame_len = FRAME_LEN; + + rs_data->bits2byte = rs92_mbits2byte; + rs_data->rs_process = rs92_process; + + rs_data->addData = &rs92_addData; + + + if (orbdata) { + fp = fopen(eph_file, "r"); // txt-mode + if (fp == NULL) orbdata = 0; + } + + if (orbdata == 1) { + if (read_SEMalmanac(fp, alm) == 0) { + almanac = 1; + } + fclose(fp); + d_err = 4000; + } + if (orbdata == 2) { + ephs = read_RNXpephs(fp); + if (ephs) { + ephem = 1; + almanac = 0; + } + fclose(fp); + d_err = 1000; + } + + + return 0; +} + +int free_rs92data(rs_data_t *rs_data) { + + rs_data->header = NULL; + + free(rs_data->buf); + rs_data->buf = NULL; + + if (rs_data->input < 8) { + // memset(rs_data->frame_rawbits, 0, rs_data->RAWBITFRAME_LEN) ... + free(rs_data->frame_rawbits); + rs_data->frame_rawbits = NULL; + + //rs_data->frame_bits = NULL; + } + + // memset(rs_data->frame_bytes, 0, rs_data->FRAME_LEN) ... + free(rs_data->frame_bytes); + rs_data->frame_bytes = NULL; + + //memset(rs_data, 0, sizeof(rs_data_t)); + + return 0; +} + diff --git a/rs_module/rs_rs92.h b/rs_module/rs_rs92.h new file mode 100644 index 0000000..0619e29 --- /dev/null +++ b/rs_module/rs_rs92.h @@ -0,0 +1,14 @@ + +#ifndef RS_RS92_H +#define RS_RS92_H + + +int rs92_process(void *, int, int); +//int rs92_bits2byte(void *, char *); + +int init_rs92data(rs_data_t *, int, char *); +int free_rs92data(rs_data_t *); + + +#endif /* RS_RS92_H */ +