From c8ffaf7624523c616783caee2b3d5490d6acb69e Mon Sep 17 00:00:00 2001 From: Zilog80 Date: Sat, 15 Oct 2016 21:36:36 +0200 Subject: [PATCH] RS92: PRN-32 toggle --- rs92/rs92ecc.c | 35 ++++++++++++++++++++++++++++++----- rs92/rs92gps.c | 10 +++++++--- 2 files changed, 37 insertions(+), 8 deletions(-) diff --git a/rs92/rs92ecc.c b/rs92/rs92ecc.c index ead36a0..533c203 100644 --- a/rs92/rs92ecc.c +++ b/rs92/rs92ecc.c @@ -95,6 +95,7 @@ typedef struct { int sats[4]; double dop; unsigned short aux[4]; + double diter; } gpx_t; gpx_t gpx; @@ -109,6 +110,7 @@ int option_verbose = 0, // ausfuehrliche Anzeige option_ecc = 0, // Reed-Solomon ECC fileloaded = 0, option_vergps = 0, + option_iter = 0, option_vel = 0, // velocity option_aux = 0, // Aux/Ozon rawin = 0; @@ -665,6 +667,7 @@ int prnbits_le(ui16_t byte16, ui8_t bits[64], int block) { } ui8_t prns[12], // PRNs in data sat_status[12]; +int prn32toggle = 0x1; void prn12(ui8_t *prn_le, ui8_t prns[12]) { int i, j, d; for (i = 0; i < 12; i++) { @@ -683,7 +686,9 @@ void prn12(ui8_t *prn_le, ui8_t prns[12]) { } else if ( (i % 3 != 2) && (prn_le[5*(i+1)] & 1) ) { // Spalte 0,1 prns[i] = 32; // vorausgesetzt im Block folgt auf PRN-32 - if (prns[i+1] > 1) prns[i+1] ^= 0x1; // entweder PRN-1 oder PRN-gerade + if (prns[i+1] > 1) { // entweder PRN-1 oder PRN-gerade + prns[i+1] ^= prn32toggle; + } } } else if ((sat_status[i] & 0x0F) == 0) { // erste beiden bits: 0x03 ? @@ -1015,7 +1020,7 @@ int get_GPSkoord(int N) { double lat, lon, alt, rx_cl_bias; double vH, vD, vU; double lat1s, lon1s, alt1s; - double pos_ecef[3], pos1s_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; @@ -1024,7 +1029,7 @@ int get_GPSkoord(int N) { int num = 0; SAT_t Sat_A[4]; SAT_t Sat_B[12]; // N <= 12 - SAT_t Sat_V[12]; + SAT_t Sat_B1s[12]; if (option_vergps == 8) { fprintf(stdout, " sats: "); @@ -1083,7 +1088,7 @@ int get_GPSkoord(int N) { if (option_vergps == 8 || option_vergps == 2) { for (j = 0; j < N; j++) Sat_B[j] = sat[prn[j]]; - for (j = 0; j < N; j++) Sat_V[j] = sat1s[prn[j]]; + for (j = 0; j < N; j++) Sat_B1s[j] = sat1s[prn[j]]; NAV_bancroft1(N, Sat_B, pos_ecef, &rx_cl_bias); ecef2elli(pos_ecef[0], pos_ecef[1], pos_ecef[2], &lat, &lon, &alt); @@ -1092,8 +1097,20 @@ int get_GPSkoord(int N) { 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); + gpx.diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[0],dpos_ecef[0]); + 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); + } + if (option_vel == 1) { - NAV_bancroft1(N, Sat_V, pos1s_ecef, &rx_cl_bias); + NAV_bancroft1(N, Sat_B1s, pos1s_ecef, &rx_cl_bias); + if (option_iter) { + NAV_LinP(N, Sat_B1s, pos1s_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); + for (j = 0; j < 3; j++) pos1s_ecef[j] += dpos_ecef[j]; + } for (j = 0; j < 3; j++) vel_ecef[j] = pos_ecef[j] - pos1s_ecef[j]; get_GPSvel(lat, lon, vel_ecef, &vH, &vD, &vU); ecef2elli(pos1s_ecef[0], pos1s_ecef[1], pos1s_ecef[2], &lat1s, &lon1s, &alt1s); @@ -1218,8 +1235,13 @@ int print_position() { // GPS-Hoehe ueber Ellipsoid if (k >= 4) { if (get_GPSkoord(k) > 0) { fprintf(stdout, " "); + if (almanac) fprintf(stdout, " lat: %.4f lon: %.4f alt: %.1f ", gpx.lat, gpx.lon, gpx.h); else fprintf(stdout, " lat: %.5f lon: %.5f alt: %.1f ", gpx.lat, gpx.lon, gpx.h); + + if (option_iter && (option_vergps == 8 || option_vergps == 2)) { + fprintf(stdout, " (d:%.1f) ", gpx.diter); + } if (option_vel && option_vergps >= 2) { fprintf(stdout," vH: %4.1f D: %5.1f° vV: %3.1f ", gpx.vH, gpx.vD, gpx.vU); } @@ -1348,6 +1370,9 @@ int main(int argc, char *argv[]) { option_vel = 2; if (option_vergps < 1) option_vergps = 2; } + else if ( (strcmp(*argv, "--iter") == 0) ) { + option_iter = 1; + } else if ( (strcmp(*argv, "-v") == 0) ) { option_verbose = 1; } diff --git a/rs92/rs92gps.c b/rs92/rs92gps.c index 54b04f8..d9dca74 100644 --- a/rs92/rs92gps.c +++ b/rs92/rs92gps.c @@ -637,6 +637,7 @@ int prnbits_le(ui16_t byte16, ui8_t bits[64], int block) { } ui8_t prns[12], // PRNs in data sat_status[12]; +int prn32toggle = 0x1; void prn12(ui8_t *prn_le, ui8_t prns[12]) { int i, j, d; for (i = 0; i < 12; i++) { @@ -655,7 +656,9 @@ void prn12(ui8_t *prn_le, ui8_t prns[12]) { } else if ( (i % 3 != 2) && (prn_le[5*(i+1)] & 1) ) { // Spalte 0,1 prns[i] = 32; // vorausgesetzt im Block folgt auf PRN-32 - if (prns[i+1] > 1) prns[i+1] ^= 0x1; // entweder PRN-1 oder PRN-gerade + if (prns[i+1] > 1) { // entweder PRN-1 oder PRN-gerade + prns[i+1] ^= prn32toggle; + } } } else if ((sat_status[i] & 0x0F) == 0) { // erste beiden bits: 0x03 ? @@ -1064,11 +1067,12 @@ int get_GPSkoord(int N) { 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); + gpx.diter = dist(0, 0, 0, dpos_ecef[0], dpos_ecef[0],dpos_ecef[0]); + if (gpx.diter > 10000) prn32toggle ^= 0x1; if (option_iter) { - NAV_LinP(N, Sat_B, pos_ecef, rx_cl_bias, dpos_ecef, &rx_cl_bias); 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[0],dpos_ecef[0]); } if (option_vel == 1) {