kopia lustrzana https://github.com/rs1729/RS
rs modules: first commit
Zilog80
rodzic
3af3f55f49
commit
3069379255
|
|
@ -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
|
||||
|
||||
```
|
||||
|
||||
|
||||
|
|
@ -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<<d))) d--; /* d<32, 1L = 1 */
|
||||
}
|
||||
return d;
|
||||
}
|
||||
|
||||
static
|
||||
ui8_t GF2m_mul(ui8_t a, ui8_t b) {
|
||||
ui32_t aa = a;
|
||||
ui8_t ab = 0;
|
||||
int i, m = GF_deg(b);
|
||||
|
||||
if (b & 1) ab = a;
|
||||
|
||||
for (i = 0; i < m; i++) {
|
||||
aa = (aa << 1); // a = a * X
|
||||
if (GF_deg(aa) == GF_deg(GF.f))
|
||||
aa ^= GF.f; // a = a - GF.f
|
||||
b >>= 1;
|
||||
if (b & 1) ab ^= (ui8_t)aa; /* b_{i+1} > 0 ? */
|
||||
}
|
||||
return ab;
|
||||
}
|
||||
|
||||
static
|
||||
int GF_genTab(GF_t gf, 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)
|
||||
for (i = deg_p+1; i <= MAX_DEG; i++) r[i] = 0;
|
||||
}
|
||||
else {
|
||||
for (i = 0; i <= deg_p; i++) r[i] = p[i];
|
||||
while (deg_p >= 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;
|
||||
}
|
||||
|
||||
|
|
@ -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 */
|
||||
|
||||
|
|
@ -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 */
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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 */
|
||||
|
||||
|
|
@ -0,0 +1,173 @@
|
|||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#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#<rev1381?, invers: unten 1, oben -1
|
||||
|
||||
/* Y-offset ? */
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
|
||||
void inc_bufpos(rs_data_t *rs_data) {
|
||||
rs_data->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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* ------------------------------------------------------------------------------------ */
|
||||
|
||||
|
||||
|
||||
|
|
@ -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 */
|
||||
|
||||
Plik diff jest za duży
Load Diff
|
|
@ -0,0 +1,213 @@
|
|||
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,233 @@
|
|||
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,893 @@
|
|||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
|
|
@ -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 */
|
||||
|
||||
|
|
@ -0,0 +1,988 @@
|
|||
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
|
||||
#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<k;i0++) { for (i1=i0+1;i1<k;i1++) { for (i2=i1+1;i2<k;i2++) { for (i3=i2+1;i3<k;i3++) {
|
||||
|
||||
Sat_C[0] = Sat_B[i0];
|
||||
Sat_C[1] = Sat_B[i1];
|
||||
Sat_C[2] = Sat_B[i2];
|
||||
Sat_C[3] = Sat_B[i3];
|
||||
|
||||
for (j = 0; j < 3; j++) pos0_ecef[j] = 0;
|
||||
NAV_bancroft1(4, Sat_C, pos0_ecef, &rx_cl_bias);
|
||||
|
||||
NAV_LinP(4, 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);
|
||||
|
||||
vel_ecef[0] = vel_ecef[1] = vel_ecef[2] = 0;
|
||||
NAV_LinV(4, Sat_C, pos0_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(lat0, lon0, vel_ecef, &vH0, &vD0, &vU0);
|
||||
|
||||
gdop0 = -1;
|
||||
if (calc_DOPn(4, Sat_C, pos0_ecef, DOP) == 0) {
|
||||
gdop0 = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]);
|
||||
}
|
||||
|
||||
|
||||
if ( (diter0 < d_err && gdop0 >= 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;
|
||||
}
|
||||
|
||||
|
|
@ -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 */
|
||||
|
||||
Ładowanie…
Reference in New Issue