RS92: PRN-32 toggle

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;
         }

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) {