From 69b99536202f09d0994d477372a9ae33c8a6e30d Mon Sep 17 00:00:00 2001 From: Zilog80 Date: Wed, 26 Oct 2016 23:50:35 +0200 Subject: [PATCH] RS92: simple RAIM --- rs92/nav_gps_vel.c | 1 + rs92/rs92ecc.c | 66 +++++++++++++++++++++++++++++++++++++++------- rs92/rs92gps.c | 66 +++++++++++++++++++++++++++++++++++++++------- 3 files changed, 115 insertions(+), 18 deletions(-) diff --git a/rs92/nav_gps_vel.c b/rs92/nav_gps_vel.c index 9db8691..e5bcebb 100644 --- a/rs92/nav_gps_vel.c +++ b/rs92/nav_gps_vel.c @@ -117,6 +117,7 @@ typedef struct { int ephhr; double PR; double ephtime; + int prn; } SAT_t; typedef struct {double X; double Y; double Z;} LOC_t; diff --git a/rs92/rs92ecc.c b/rs92/rs92ecc.c index 6ef07f6..944a57a 100644 --- a/rs92/rs92ecc.c +++ b/rs92/rs92ecc.c @@ -986,6 +986,9 @@ int get_pseudorange() { sat1s[prns[j]].pseudorange = -(range[prns[j]].chips - range[prns[j]].deltachips/dl)*df; //+ sat[prns[j]].clock_corr - sat1s[prns[j]].clock_corr sat[prns[j]].pseudorate = - range[prns[j]].deltachips * df / dl; + + sat[prns[j]].prn = prns[j]; + sat1s[prns[j]].prn = prns[j]; } @@ -1026,18 +1029,21 @@ double DOP[4]; int get_GPSkoord(int N) { double lat, lon, alt, rx_cl_bias; double vH, vD, vU; - double lat1s, lon1s, alt1s; + double lat1s, lon1s, alt1s, + lat0 , lon0 , alt0 , pos0_ecef[3]; double pos_ecef[3], pos1s_ecef[3], dpos_ecef[3], vel_ecef[3], dvel_ecef[3]; double gdop, gdop0 = 1000.0; //double hdop, vdop, pdop; - int i0, i1, i2, i3, j; + int i0, i1, i2, i3, j, k, n; int nav_ret = 0; int num = 0; SAT_t Sat_A[4]; SAT_t Sat_B[12]; // N <= 12 SAT_t Sat_B1s[12]; + SAT_t Sat_C[12]; // 11 double diter; + int exN = -1; if (option_vergps == 8) { fprintf(stdout, " sats: "); @@ -1097,7 +1103,7 @@ int get_GPSkoord(int N) { } else gdop = -1; - if (gdop > 0 && gdop < gdop0) { + if (gdop > 0 && gdop < gdop0) { // wenn fehlerhafter Sat, diter wohl besserer Indikator gpx.lat = lat; gpx.lon = lon; gpx.h = alt; @@ -1129,12 +1135,53 @@ int get_GPSkoord(int N) { } NAV_LinP(N, Sat_B, pos_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); - gpx.diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1],dpos_ecef[2]); - if (gpx.diter > 10000) prn32toggle ^= 0x1; if (option_iter) { for (j = 0; j < 3; j++) pos_ecef[j] += dpos_ecef[j]; ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt); } + gpx.diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1],dpos_ecef[2]); + + // Sat mit schlechten Daten suchen + if (gpx.diter > 10000) { + if (N > 5) { // 5; 4 kann auch funktionieren + for (n = 0; n < N; n++) { + k = 0; + for (j = 0; j < N; j++) { + if (j != n) { + Sat_C[k] = Sat_B[j]; + k++; + } + } + for (j = 0; j < 3; j++) pos0_ecef[j] = 0; + NAV_bancroft1(N-1, Sat_C, pos0_ecef, &rx_cl_bias); + NAV_LinP(N-1, Sat_C, pos0_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); + diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1],dpos_ecef[2]); + ecef2elli(pos0_ecef[0], pos0_ecef[1], pos0_ecef[2], &lat0, &lon0, &alt0); + if (diter < gpx.diter) { + gpx.diter = diter; + for (j = 0; j < 3; j++) pos_ecef[j] = pos0_ecef[j]; + lat = lat0; + lon = lon0; + alt = alt0; + exN = n; + } + } + if (exN >= 0) { + for (k = exN; k < N-1; k++) { + Sat_B[k] = Sat_B[k+1]; + prn[k] = prn[k+1]; + if (option_vel == 1) { + Sat_B1s[k] = Sat_B1s[k+1]; + } + } + N = N-1; + if (calc_DOPn(N, Sat_B, pos_ecef, DOP) == 0) { + gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]); + } + } + } + if (exN < 0 || prn[exN-1] == 32) prn32toggle ^= 0x1; + } if (option_vel == 1) { NAV_bancroft1(N, Sat_B1s, pos1s_ecef, &rx_cl_bias); @@ -1238,7 +1285,7 @@ int rs92_ecc(int msglen) { /* ------------------------------------------------------------------------------------ */ int print_position() { // GPS-Hoehe ueber Ellipsoid - int j, k; + int j, k, n; int err1, err2; err1 = 0; @@ -1268,7 +1315,8 @@ int print_position() { // GPS-Hoehe ueber Ellipsoid if (almanac || ephem) { k = get_pseudorange(); if (k >= 4) { - if (get_GPSkoord(k) > 0) { + n = get_GPSkoord(k); + if (n > 0) { fprintf(stdout, " "); if (almanac) fprintf(stdout, " lat: %.4f lon: %.4f alt: %.1f ", gpx.lat, gpx.lon, gpx.h); @@ -1287,9 +1335,9 @@ int print_position() { // GPS-Hoehe ueber Ellipsoid } else { fprintf(stdout, " DOP["); - for (j = 0; j < k; j++) { + for (j = 0; j < n; j++) { fprintf(stdout, "%d", prn[j]); - if (j < k-1) fprintf(stdout, ","); else fprintf(stdout, "] %.1f ", gpx.dop); + if (j < n-1) fprintf(stdout, ","); else fprintf(stdout, "] %.1f ", gpx.dop); } } } diff --git a/rs92/rs92gps.c b/rs92/rs92gps.c index aaafb4f..8f7fa86 100644 --- a/rs92/rs92gps.c +++ b/rs92/rs92gps.c @@ -956,6 +956,9 @@ int get_pseudorange() { sat1s[prns[j]].pseudorange = -(range[prns[j]].chips - range[prns[j]].deltachips/dl)*df; //+ sat[prns[j]].clock_corr - sat1s[prns[j]].clock_corr sat[prns[j]].pseudorate = - range[prns[j]].deltachips * df / dl; + + sat[prns[j]].prn = prns[j]; + sat1s[prns[j]].prn = prns[j]; } @@ -996,18 +999,21 @@ double DOP[4]; int get_GPSkoord(int N) { double lat, lon, alt, rx_cl_bias; double vH, vD, vU; - double lat1s, lon1s, alt1s; + double lat1s, lon1s, alt1s, + lat0 , lon0 , alt0 , pos0_ecef[3]; double pos_ecef[3], pos1s_ecef[3], dpos_ecef[3], vel_ecef[3], dvel_ecef[3]; double gdop, gdop0 = 1000.0; //double hdop, vdop, pdop; - int i0, i1, i2, i3, j; + int i0, i1, i2, i3, j, k, n; int nav_ret = 0; int num = 0; SAT_t Sat_A[4]; SAT_t Sat_B[12]; // N <= 12 SAT_t Sat_B1s[12]; + SAT_t Sat_C[12]; // 11 double diter; + int exN = -1; if (option_vergps == 8) { fprintf(stdout, " sats: "); @@ -1067,7 +1073,7 @@ int get_GPSkoord(int N) { } else gdop = -1; - if (gdop > 0 && gdop < gdop0) { + if (gdop > 0 && gdop < gdop0) { // wenn fehlerhafter Sat, diter wohl besserer Indikator gpx.lat = lat; gpx.lon = lon; gpx.h = alt; @@ -1099,12 +1105,53 @@ int get_GPSkoord(int N) { } NAV_LinP(N, Sat_B, pos_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); - gpx.diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1],dpos_ecef[2]); - if (gpx.diter > 10000) prn32toggle ^= 0x1; if (option_iter) { for (j = 0; j < 3; j++) pos_ecef[j] += dpos_ecef[j]; ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt); } + gpx.diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1],dpos_ecef[2]); + + // Sat mit schlechten Daten suchen + if (gpx.diter > 10000) { + if (N > 5) { // 5; 4 kann auch funktionieren + for (n = 0; n < N; n++) { + k = 0; + for (j = 0; j < N; j++) { + if (j != n) { + Sat_C[k] = Sat_B[j]; + k++; + } + } + for (j = 0; j < 3; j++) pos0_ecef[j] = 0; + NAV_bancroft1(N-1, Sat_C, pos0_ecef, &rx_cl_bias); + NAV_LinP(N-1, Sat_C, pos0_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); + diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[1],dpos_ecef[2]); + ecef2elli(pos0_ecef[0], pos0_ecef[1], pos0_ecef[2], &lat0, &lon0, &alt0); + if (diter < gpx.diter) { + gpx.diter = diter; + for (j = 0; j < 3; j++) pos_ecef[j] = pos0_ecef[j]; + lat = lat0; + lon = lon0; + alt = alt0; + exN = n; + } + } + if (exN >= 0) { + for (k = exN; k < N-1; k++) { + Sat_B[k] = Sat_B[k+1]; + prn[k] = prn[k+1]; + if (option_vel == 1) { + Sat_B1s[k] = Sat_B1s[k+1]; + } + } + N = N-1; + if (calc_DOPn(N, Sat_B, pos_ecef, DOP) == 0) { + gdop = sqrt(DOP[0]+DOP[1]+DOP[2]+DOP[3]); + } + } + } + if (exN < 0 || prn[exN-1] == 32) prn32toggle ^= 0x1; + } if (option_vel == 1) { NAV_bancroft1(N, Sat_B1s, pos1s_ecef, &rx_cl_bias); @@ -1174,7 +1221,7 @@ int get_GPSkoord(int N) { int print_position() { // GPS-Hoehe ueber Ellipsoid - int j, k; + int j, k, n; int err1, err2; err1 = 0; @@ -1204,7 +1251,8 @@ int print_position() { // GPS-Hoehe ueber Ellipsoid if (almanac || ephem) { k = get_pseudorange(); if (k >= 4) { - if (get_GPSkoord(k) > 0) { + n = get_GPSkoord(k); + if (n > 0) { fprintf(stdout, " "); if (almanac) fprintf(stdout, " lat: %.4f lon: %.4f alt: %.1f ", gpx.lat, gpx.lon, gpx.h); @@ -1223,9 +1271,9 @@ int print_position() { // GPS-Hoehe ueber Ellipsoid } else { fprintf(stdout, " DOP["); - for (j = 0; j < k; j++) { + for (j = 0; j < n; j++) { fprintf(stdout, "%d", prn[j]); - if (j < k-1) fprintf(stdout, ","); else fprintf(stdout, "] %.1f ", gpx.dop); + if (j < n-1) fprintf(stdout, ","); else fprintf(stdout, "] %.1f ", gpx.dop); } } }